From 79e9bccbd816b7fdaf29e8fcc06e287f06bc6bbe Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 21 Jun 2017 12:32:00 +0200
Subject: [PATCH 001/269] fixed indent

---
 srsue/src/main.cc | 544 +++++++++++++++++++++++-----------------------
 1 file changed, 274 insertions(+), 270 deletions(-)

diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 8065a3571..f114c0606 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -49,284 +49,289 @@ namespace bpo = boost::program_options;
  ***********************************************************************/
 string config_file;
 
-void parse_args(all_args_t *args, int argc, char* argv[]) {
-
-    // Command line only options
-    bpo::options_description general("General options");
-    general.add_options()
-        ("help,h", "Produce help message")
-        ("version,v", "Print version information and exit")
-        ;
-
-    // Command line or config file options
-    bpo::options_description common("Configuration options");
-    common.add_options()
-        ("rf.dl_freq",        bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000),  "Downlink centre frequency")
-        ("rf.ul_freq",        bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000),  "Uplink centre frequency")
-        ("rf.rx_gain",        bpo::value<float>(&args->rf.rx_gain)->default_value(-1),          "Front-end receiver gain")
-        ("rf.tx_gain",        bpo::value<float>(&args->rf.tx_gain)->default_value(-1),          "Front-end transmitter gain")
-        ("rf.nof_rx_ant",     bpo::value<uint32_t>(&args->rf.nof_rx_ant)->default_value(1),         "Number of RX antennas")
-
-        ("rf.device_name",       bpo::value<string>(&args->rf.device_name)->default_value("auto"),    "Front-end device name")
-        ("rf.device_args",       bpo::value<string>(&args->rf.device_args)->default_value("auto"),    "Front-end device arguments")
-        ("rf.time_adv_nsamples", bpo::value<string>(&args->rf.time_adv_nsamples)->default_value("auto"),    "Transmission time advance")
-        ("rf.burst_preamble_us", bpo::value<string>(&args->rf.burst_preamble)->default_value("auto"), "Transmission time advance")
-
-        ("pcap.enable",       bpo::value<bool>(&args->pcap.enable)->default_value(false),           "Enable MAC packet captures for wireshark")
-        ("pcap.filename",     bpo::value<string>(&args->pcap.filename)->default_value("ue.pcap"),   "MAC layer capture filename")
-
-        ("trace.enable",      bpo::value<bool>(&args->trace.enable)->default_value(false),                  "Enable PHY and radio timing traces")
-        ("trace.phy_filename",bpo::value<string>(&args->trace.phy_filename)->default_value("ue.phy_trace"), "PHY timing traces filename")
-        ("trace.radio_filename",bpo::value<string>(&args->trace.radio_filename)->default_value("ue.radio_trace"), "Radio timing traces filename")
-
-        ("gui.enable",        bpo::value<bool>(&args->gui.enable)->default_value(false),                  "Enable GUI plots")
-        
-        ("log.phy_level",     bpo::value<string>(&args->log.phy_level),   "PHY log level")
-        ("log.phy_hex_limit", bpo::value<int>(&args->log.phy_hex_limit),  "PHY log hex dump limit")
-        ("log.mac_level",     bpo::value<string>(&args->log.mac_level),   "MAC log level")
-        ("log.mac_hex_limit", bpo::value<int>(&args->log.mac_hex_limit),  "MAC log hex dump limit")
-        ("log.rlc_level",     bpo::value<string>(&args->log.rlc_level),   "RLC log level")
-        ("log.rlc_hex_limit", bpo::value<int>(&args->log.rlc_hex_limit),  "RLC log hex dump limit")
-        ("log.pdcp_level",    bpo::value<string>(&args->log.pdcp_level),  "PDCP log level")
-        ("log.pdcp_hex_limit",bpo::value<int>(&args->log.pdcp_hex_limit), "PDCP log hex dump limit")
-        ("log.rrc_level",     bpo::value<string>(&args->log.rrc_level),   "RRC log level")
-        ("log.rrc_hex_limit", bpo::value<int>(&args->log.rrc_hex_limit),  "RRC log hex dump limit")
-        ("log.gw_level",      bpo::value<string>(&args->log.gw_level),    "GW log level")
-        ("log.gw_hex_limit",  bpo::value<int>(&args->log.gw_hex_limit),   "GW log hex dump limit")
-        ("log.nas_level",     bpo::value<string>(&args->log.nas_level),   "NAS log level")
-        ("log.nas_hex_limit", bpo::value<int>(&args->log.nas_hex_limit),  "NAS log hex dump limit")
-        ("log.usim_level",    bpo::value<string>(&args->log.usim_level),  "USIM log level")
-        ("log.usim_hex_limit",bpo::value<int>(&args->log.usim_hex_limit), "USIM log hex dump limit")
-
-        ("log.all_level",     bpo::value<string>(&args->log.all_level)->default_value("info"),   "ALL log level")
-        ("log.all_hex_limit", bpo::value<int>(&args->log.all_hex_limit)->default_value(32),  "ALL log hex dump limit")
-
-        ("log.filename",      bpo::value<string>(&args->log.filename)->default_value("/tmp/ue.log"),"Log filename")
-
-        ("usim.algo",         bpo::value<string>(&args->usim.algo),        "USIM authentication algorithm")
-        ("usim.op",           bpo::value<string>(&args->usim.op),          "USIM operator variant")
-        ("usim.amf",          bpo::value<string>(&args->usim.amf),         "USIM authentication management field")
-        ("usim.imsi",         bpo::value<string>(&args->usim.imsi),        "USIM IMSI")
-        ("usim.imei",         bpo::value<string>(&args->usim.imei),        "USIM IMEI")
-        ("usim.k",            bpo::value<string>(&args->usim.k),           "USIM K")
-        
-        
-        /* Expert section */
-        ("expert.phy.worker_cpu_mask",
-            bpo::value<int>(&args->expert.phy.worker_cpu_mask)->default_value(-1),
-            "cpu bit mask (eg 255 = 1111 1111)")        
-        
-        ("expert.phy.sync_cpu_affinity",
-            bpo::value<int>(&args->expert.phy.sync_cpu_affinity)->default_value(-1),
-            "index of the core used by the sync thread")
-    
-        ("expert.ue_category",
-            bpo::value<int>(&args->expert.ue_cateogry)->default_value(4), 
-            "UE Category (1 to 5)")
-
-        ("expert.metrics_period_secs",
-            bpo::value<float>(&args->expert.metrics_period_secs)->default_value(1.0), 
-            "Periodicity for metrics in seconds")
-
-        ("expert.pregenerate_signals",
-            bpo::value<bool>(&args->expert.pregenerate_signals)->default_value(false), 
-            "Pregenerate uplink signals after attach. Improves CPU performance.")
-        
-        ("expert.rssi_sensor_enabled", 
-            bpo::value<bool>(&args->expert.phy.rssi_sensor_enabled)->default_value(true),  
-            "Enable or disable RF frontend RSSI sensor. In some USRP devices can cause segmentation fault")
-        
-        ("expert.prach_gain", 
-            bpo::value<float>(&args->expert.phy.prach_gain)->default_value(-1.0),  
-            "Disable PRACH power control")
-        
-        ("expert.cqi_max",         
-            bpo::value<int>(&args->expert.phy.cqi_max)->default_value(15), 
-            "Upper bound on the maximum CQI to be reported. Default 15.")
-        
-        ("expert.cqi_fixed",         
-            bpo::value<int>(&args->expert.phy.cqi_fixed)->default_value(-1), 
-            "Fixes the reported CQI to a constant value. Default disabled.")
-        
-        ("expert.snr_ema_coeff",         
-            bpo::value<float>(&args->expert.phy.snr_ema_coeff)->default_value(0.1), 
-            "Sets the SNR exponential moving average coefficient (Default 0.1)")
-        
-        ("expert.snr_estim_alg",         
-            bpo::value<string>(&args->expert.phy.snr_estim_alg)->default_value("refs"), 
-            "Sets the noise estimation algorithm. (Default refs)")
-        
-        ("expert.pdsch_max_its",         
-            bpo::value<int>(&args->expert.phy.pdsch_max_its)->default_value(4), 
-            "Maximum number of turbo decoder iterations")
-
-        ("expert.attach_enable_64qam",      
-            bpo::value<bool>(&args->expert.phy.attach_enable_64qam)->default_value(false), 
-            "PUSCH 64QAM modulation before attachment")
-        
-        ("expert.nof_phy_threads",    
-            bpo::value<int>(&args->expert.phy.nof_phy_threads)->default_value(2), 
-            "Number of PHY threads")
-        
-        ("expert.equalizer_mode",    
-            bpo::value<string>(&args->expert.phy.equalizer_mode)->default_value("mmse"), 
-            "Equalizer mode")
-     
-        ("expert.cfo_integer_enabled",    
-            bpo::value<bool>(&args->expert.phy.cfo_integer_enabled)->default_value(false), 
-            "Enables integer CFO estimation and correction.")
-        
-        ("expert.cfo_correct_tol_hz",    
-            bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(50.0), 
-            "Tolerance (in Hz) for digial CFO compensation.")
-        
-        ("expert.time_correct_period",    
-            bpo::value<int>(&args->expert.phy.time_correct_period)->default_value(5), 
-            "Period for sampling time offset correction.")
-        
-        ("expert.sfo_correct_disable",    
-            bpo::value<bool>(&args->expert.phy.sfo_correct_disable)->default_value(false), 
-            "Disables phase correction before channel estimation.")
-        
-        ("expert.sss_algorithm",    
-            bpo::value<string>(&args->expert.phy.sss_algorithm)->default_value("full"), 
-            "Selects the SSS estimation algorithm.")
-
-        ("expert.estimator_fil_w",    
-            bpo::value<float>(&args->expert.phy.estimator_fil_w)->default_value(0.1), 
-            "Chooses the coefficients for the 3-tap channel estimator centered filter.")
-        
-        
-        ("rf_calibration.tx_corr_dc_gain",  bpo::value<float>(&args->rf_cal.tx_corr_dc_gain)->default_value(0.0),  "TX DC offset gain correction")
-        ("rf_calibration.tx_corr_dc_phase", bpo::value<float>(&args->rf_cal.tx_corr_dc_phase)->default_value(0.0), "TX DC offset phase correction")
-        ("rf_calibration.tx_corr_iq_i",     bpo::value<float>(&args->rf_cal.tx_corr_iq_i)->default_value(0.0),     "TX IQ imbalance inphase correction")
-        ("rf_calibration.tx_corr_iq_q",     bpo::value<float>(&args->rf_cal.tx_corr_iq_q)->default_value(0.0),     "TX IQ imbalance quadrature correction")
-        
-    ;
-    
-    // Positional options - config file location
-    bpo::options_description position("Positional options");
-    position.add_options()
-    ("config_file", bpo::value< string >(&config_file), "UE configuration file")
-    ;
-    bpo::positional_options_description p;
-    p.add("config_file", -1);
-
-    // these options are allowed on the command line
-    bpo::options_description cmdline_options;
-    cmdline_options.add(common).add(position).add(general);
-
-    // parse the command line and store result in vm
-    bpo::variables_map vm;
-    bpo::store(bpo::command_line_parser(argc, argv).options(cmdline_options).positional(p).run(), vm);
-    bpo::notify(vm);
+void parse_args(all_args_t *args, int argc, char *argv[]) {
+
+  // Command line only options
+  bpo::options_description general("General options");
+
+  general.add_options()
+    ("help,h", "Produce help message")
+    ("version,v", "Print version information and exit");
+
+  // Command line or config file options
+  bpo::options_description common("Configuration options");
+  common.add_options()
+    ("rf.dl_freq", bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000), "Downlink centre frequency")
+    ("rf.ul_freq", bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000), "Uplink centre frequency")
+    ("rf.rx_gain", bpo::value<float>(&args->rf.rx_gain)->default_value(-1), "Front-end receiver gain")
+    ("rf.tx_gain", bpo::value<float>(&args->rf.tx_gain)->default_value(-1), "Front-end transmitter gain")
+    ("rf.nof_rx_ant", bpo::value<uint32_t>(&args->rf.nof_rx_ant)->default_value(1), "Number of RX antennas")
+
+    ("rf.device_name", bpo::value<string>(&args->rf.device_name)->default_value("auto"), "Front-end device name")
+    ("rf.device_args", bpo::value<string>(&args->rf.device_args)->default_value("auto"), "Front-end device arguments")
+    ("rf.time_adv_nsamples", bpo::value<string>(&args->rf.time_adv_nsamples)->default_value("auto"),
+     "Transmission time advance")
+    ("rf.burst_preamble_us", bpo::value<string>(&args->rf.burst_preamble)->default_value("auto"),
+     "Transmission time advance")
+
+    ("pcap.enable", bpo::value<bool>(&args->pcap.enable)->default_value(false),
+     "Enable MAC packet captures for wireshark")
+    ("pcap.filename", bpo::value<string>(&args->pcap.filename)->default_value("ue.pcap"), "MAC layer capture filename")
+
+    ("trace.enable", bpo::value<bool>(&args->trace.enable)->default_value(false), "Enable PHY and radio timing traces")
+    ("trace.phy_filename", bpo::value<string>(&args->trace.phy_filename)->default_value("ue.phy_trace"),
+     "PHY timing traces filename")
+    ("trace.radio_filename", bpo::value<string>(&args->trace.radio_filename)->default_value("ue.radio_trace"),
+     "Radio timing traces filename")
+
+    ("gui.enable", bpo::value<bool>(&args->gui.enable)->default_value(false), "Enable GUI plots")
+
+    ("log.phy_level", bpo::value<string>(&args->log.phy_level), "PHY log level")
+    ("log.phy_hex_limit", bpo::value<int>(&args->log.phy_hex_limit), "PHY log hex dump limit")
+    ("log.mac_level", bpo::value<string>(&args->log.mac_level), "MAC log level")
+    ("log.mac_hex_limit", bpo::value<int>(&args->log.mac_hex_limit), "MAC log hex dump limit")
+    ("log.rlc_level", bpo::value<string>(&args->log.rlc_level), "RLC log level")
+    ("log.rlc_hex_limit", bpo::value<int>(&args->log.rlc_hex_limit), "RLC log hex dump limit")
+    ("log.pdcp_level", bpo::value<string>(&args->log.pdcp_level), "PDCP log level")
+    ("log.pdcp_hex_limit", bpo::value<int>(&args->log.pdcp_hex_limit), "PDCP log hex dump limit")
+    ("log.rrc_level", bpo::value<string>(&args->log.rrc_level), "RRC log level")
+    ("log.rrc_hex_limit", bpo::value<int>(&args->log.rrc_hex_limit), "RRC log hex dump limit")
+    ("log.gw_level", bpo::value<string>(&args->log.gw_level), "GW log level")
+    ("log.gw_hex_limit", bpo::value<int>(&args->log.gw_hex_limit), "GW log hex dump limit")
+    ("log.nas_level", bpo::value<string>(&args->log.nas_level), "NAS log level")
+    ("log.nas_hex_limit", bpo::value<int>(&args->log.nas_hex_limit), "NAS log hex dump limit")
+    ("log.usim_level", bpo::value<string>(&args->log.usim_level), "USIM log level")
+    ("log.usim_hex_limit", bpo::value<int>(&args->log.usim_hex_limit), "USIM log hex dump limit")
+
+    ("log.all_level", bpo::value<string>(&args->log.all_level)->default_value("info"), "ALL log level")
+    ("log.all_hex_limit", bpo::value<int>(&args->log.all_hex_limit)->default_value(32), "ALL log hex dump limit")
+
+    ("log.filename", bpo::value<string>(&args->log.filename)->default_value("/tmp/ue.log"), "Log filename")
+
+    ("usim.algo", bpo::value<string>(&args->usim.algo), "USIM authentication algorithm")
+    ("usim.op", bpo::value<string>(&args->usim.op), "USIM operator variant")
+    ("usim.amf", bpo::value<string>(&args->usim.amf), "USIM authentication management field")
+    ("usim.imsi", bpo::value<string>(&args->usim.imsi), "USIM IMSI")
+    ("usim.imei", bpo::value<string>(&args->usim.imei), "USIM IMEI")
+    ("usim.k", bpo::value<string>(&args->usim.k), "USIM K")
+
+
+    /* Expert section */
+    ("expert.phy.worker_cpu_mask",
+     bpo::value<int>(&args->expert.phy.worker_cpu_mask)->default_value(-1),
+     "cpu bit mask (eg 255 = 1111 1111)")
+
+    ("expert.phy.sync_cpu_affinity",
+     bpo::value<int>(&args->expert.phy.sync_cpu_affinity)->default_value(-1),
+     "index of the core used by the sync thread")
+
+    ("expert.ue_category",
+     bpo::value<int>(&args->expert.ue_cateogry)->default_value(4),
+     "UE Category (1 to 5)")
+
+    ("expert.metrics_period_secs",
+     bpo::value<float>(&args->expert.metrics_period_secs)->default_value(1.0),
+     "Periodicity for metrics in seconds")
+
+    ("expert.pregenerate_signals",
+     bpo::value<bool>(&args->expert.pregenerate_signals)->default_value(false),
+     "Pregenerate uplink signals after attach. Improves CPU performance.")
+
+    ("expert.rssi_sensor_enabled",
+     bpo::value<bool>(&args->expert.phy.rssi_sensor_enabled)->default_value(true),
+     "Enable or disable RF frontend RSSI sensor. In some USRP devices can cause segmentation fault")
+
+    ("expert.prach_gain",
+     bpo::value<float>(&args->expert.phy.prach_gain)->default_value(-1.0),
+     "Disable PRACH power control")
+
+    ("expert.cqi_max",
+     bpo::value<int>(&args->expert.phy.cqi_max)->default_value(15),
+     "Upper bound on the maximum CQI to be reported. Default 15.")
+
+    ("expert.cqi_fixed",
+     bpo::value<int>(&args->expert.phy.cqi_fixed)->default_value(-1),
+     "Fixes the reported CQI to a constant value. Default disabled.")
+
+    ("expert.snr_ema_coeff",
+     bpo::value<float>(&args->expert.phy.snr_ema_coeff)->default_value(0.1),
+     "Sets the SNR exponential moving average coefficient (Default 0.1)")
+
+    ("expert.snr_estim_alg",
+     bpo::value<string>(&args->expert.phy.snr_estim_alg)->default_value("refs"),
+     "Sets the noise estimation algorithm. (Default refs)")
+
+    ("expert.pdsch_max_its",
+     bpo::value<int>(&args->expert.phy.pdsch_max_its)->default_value(4),
+     "Maximum number of turbo decoder iterations")
+
+    ("expert.attach_enable_64qam",
+     bpo::value<bool>(&args->expert.phy.attach_enable_64qam)->default_value(false),
+     "PUSCH 64QAM modulation before attachment")
+
+    ("expert.nof_phy_threads",
+     bpo::value<int>(&args->expert.phy.nof_phy_threads)->default_value(2),
+     "Number of PHY threads")
+
+    ("expert.equalizer_mode",
+     bpo::value<string>(&args->expert.phy.equalizer_mode)->default_value("mmse"),
+     "Equalizer mode")
+
+    ("expert.cfo_integer_enabled",
+     bpo::value<bool>(&args->expert.phy.cfo_integer_enabled)->default_value(false),
+     "Enables integer CFO estimation and correction.")
+
+    ("expert.cfo_correct_tol_hz",
+     bpo::value<float>(&args->expert.phy.cfo_correct_tol_hz)->default_value(50.0),
+     "Tolerance (in Hz) for digial CFO compensation.")
+
+    ("expert.time_correct_period",
+     bpo::value<int>(&args->expert.phy.time_correct_period)->default_value(5),
+     "Period for sampling time offset correction.")
+
+    ("expert.sfo_correct_disable",
+     bpo::value<bool>(&args->expert.phy.sfo_correct_disable)->default_value(false),
+     "Disables phase correction before channel estimation.")
+
+    ("expert.sss_algorithm",
+     bpo::value<string>(&args->expert.phy.sss_algorithm)->default_value("full"),
+     "Selects the SSS estimation algorithm.")
+
+    ("expert.estimator_fil_w",
+     bpo::value<float>(&args->expert.phy.estimator_fil_w)->default_value(0.1),
+     "Chooses the coefficients for the 3-tap channel estimator centered filter.")
+
+
+    ("rf_calibration.tx_corr_dc_gain", bpo::value<float>(&args->rf_cal.tx_corr_dc_gain)->default_value(0.0),
+     "TX DC offset gain correction")
+    ("rf_calibration.tx_corr_dc_phase", bpo::value<float>(&args->rf_cal.tx_corr_dc_phase)->default_value(0.0),
+     "TX DC offset phase correction")
+    ("rf_calibration.tx_corr_iq_i", bpo::value<float>(&args->rf_cal.tx_corr_iq_i)->default_value(0.0),
+     "TX IQ imbalance inphase correction")
+    ("rf_calibration.tx_corr_iq_q", bpo::value<float>(&args->rf_cal.tx_corr_iq_q)->default_value(0.0),
+     "TX IQ imbalance quadrature correction");
+
+  // Positional options - config file location
+  bpo::options_description position("Positional options");
+  position.add_options()
+    ("config_file", bpo::value<string>(&config_file), "UE configuration file");
+  bpo::positional_options_description p;
+  p.add("config_file", -1);
+
+  // these options are allowed on the command line
+  bpo::options_description cmdline_options;
+  cmdline_options.add(common).add(position).add(general);
+
+  // parse the command line and store result in vm
+  bpo::variables_map vm;
+  bpo::store(bpo::command_line_parser(argc, argv).options(cmdline_options).positional(p).run(), vm);
+  bpo::notify(vm);
+
+  // help option was given - print usage and exit
+  if (vm.count("help")) {
+    cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
+    cout << common << endl << general << endl;
+    exit(0);
+  }
 
-    // help option was given - print usage and exit
-    if (vm.count("help")) {
-        cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
-        cout << common << endl << general << endl;
-        exit(0);
-    }
+  // print version number and exit
+  if (vm.count("version")) {
+    cout << "Version " <<
+         srslte_get_version_major() << "." <<
+         srslte_get_version_minor() << "." <<
+         srslte_get_version_patch() << endl;
+    exit(0);
+  }
 
-    // print version number and exit
-    if (vm.count("version")) {
-        cout << "Version " <<
-                srslte_get_version_major() << "." <<
-                srslte_get_version_minor() << "." <<
-                srslte_get_version_patch() << endl;
-        exit(0);
+  // no config file given - print usage and exit
+  if (!vm.count("config_file")) {
+    cout << "Error: Configuration file not provided" << endl;
+    cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
+    exit(0);
+  } else {
+    cout << "Reading configuration file " << config_file << "..." << endl;
+    ifstream conf(config_file.c_str(), ios::in);
+    if (conf.fail()) {
+      cout << "Failed to read configuration file " << config_file << " - exiting" << endl;
+      exit(1);
     }
+    bpo::store(bpo::parse_config_file(conf, common), vm);
+    bpo::notify(vm);
+  }
 
-    // no config file given - print usage and exit
-    if (!vm.count("config_file")) {
-        cout << "Error: Configuration file not provided" << endl;
-        cout << "Usage: " << argv[0] << " [OPTIONS] config_file" << endl << endl;
-        exit(0);
-    } else {
-        cout << "Reading configuration file " << config_file << "..." << endl;
-        ifstream conf(config_file.c_str(), ios::in);
-        if(conf.fail()) {
-          cout << "Failed to read configuration file " << config_file << " - exiting" << endl;
-          exit(1);
-        }
-        bpo::store(bpo::parse_config_file(conf, common), vm);
-        bpo::notify(vm);
+  // Apply all_level to any unset layers
+  if (vm.count("log.all_level")) {
+    if (!vm.count("log.phy_level")) {
+      args->log.phy_level = args->log.all_level;
     }
-
-    // Apply all_level to any unset layers
-    if (vm.count("log.all_level")) {
-      if(!vm.count("log.phy_level")) {
-        args->log.phy_level = args->log.all_level;
-      }
-      if(!vm.count("log.mac_level")) {
-        args->log.mac_level = args->log.all_level;
-      }
-      if(!vm.count("log.rlc_level")) {
-        args->log.rlc_level = args->log.all_level;
-      }
-      if(!vm.count("log.pdcp_level")) {
-        args->log.pdcp_level = args->log.all_level;
-      }
-      if(!vm.count("log.rrc_level")) {
-        args->log.rrc_level = args->log.all_level;
-      }
-      if(!vm.count("log.nas_level")) {
-        args->log.nas_level = args->log.all_level;
-      }
-      if(!vm.count("log.gw_level")) {
-        args->log.gw_level = args->log.all_level;
-      }
-      if(!vm.count("log.usim_level")) {
-        args->log.usim_level = args->log.all_level;
-      }
+    if (!vm.count("log.mac_level")) {
+      args->log.mac_level = args->log.all_level;
+    }
+    if (!vm.count("log.rlc_level")) {
+      args->log.rlc_level = args->log.all_level;
+    }
+    if (!vm.count("log.pdcp_level")) {
+      args->log.pdcp_level = args->log.all_level;
+    }
+    if (!vm.count("log.rrc_level")) {
+      args->log.rrc_level = args->log.all_level;
+    }
+    if (!vm.count("log.nas_level")) {
+      args->log.nas_level = args->log.all_level;
+    }
+    if (!vm.count("log.gw_level")) {
+      args->log.gw_level = args->log.all_level;
     }
+    if (!vm.count("log.usim_level")) {
+      args->log.usim_level = args->log.all_level;
+    }
+  }
 
-    // Apply all_hex_limit to any unset layers
-    if (vm.count("log.all_hex_limit")) {
-      if(!vm.count("log.phy_hex_limit")) {
-        args->log.phy_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.mac_hex_limit")) {
-        args->log.mac_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.rlc_hex_limit")) {
-        args->log.rlc_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.pdcp_hex_limit")) {
-        args->log.pdcp_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.rrc_hex_limit")) {
-        args->log.rrc_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.nas_hex_limit")) {
-        args->log.nas_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.gw_hex_limit")) {
-        args->log.gw_hex_limit = args->log.all_hex_limit;
-      }
-      if(!vm.count("log.usim_hex_limit")) {
-        args->log.usim_hex_limit = args->log.all_hex_limit;
-      }
+
+  // Apply all_hex_limit to any unset layers
+  if (vm.count("log.all_hex_limit")) {
+    if (!vm.count("log.phy_hex_limit")) {
+      args->log.phy_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.mac_hex_limit")) {
+      args->log.mac_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.rlc_hex_limit")) {
+      args->log.rlc_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.pdcp_hex_limit")) {
+      args->log.pdcp_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.rrc_hex_limit")) {
+      args->log.rrc_hex_limit = args->log.all_hex_limit;
     }
+    if (!vm.count("log.nas_hex_limit")) {
+      args->log.nas_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.gw_hex_limit")) {
+      args->log.gw_hex_limit = args->log.all_hex_limit;
+    }
+    if (!vm.count("log.usim_hex_limit")) {
+      args->log.usim_hex_limit = args->log.all_hex_limit;
+    }
+  }
 }
 
-static bool running    = true;
+static bool running = true;
 static bool do_metrics = false;
 
-void sig_int_handler(int signo)
-{
+void sig_int_handler(int signo) {
   running = false;
 }
 
-void *input_loop(void *m)
-{
-  metrics_stdout *metrics = (metrics_stdout*)m;
+void *input_loop(void *m) {
+  metrics_stdout *metrics = (metrics_stdout *) m;
   char key;
-  while(running) {
+  while (running) {
     cin >> key;
-    if('t' == key) {
+    if ('t' == key) {
       do_metrics = !do_metrics;
-      if(do_metrics) {
+      if (do_metrics) {
         cout << "Enter t to stop trace." << endl;
       } else {
         cout << "Enter t to restart trace." << endl;
@@ -337,17 +342,16 @@ void *input_loop(void *m)
   return NULL;
 }
 
-int main(int argc, char *argv[])
-{
+int main(int argc, char *argv[]) {
   signal(SIGINT, sig_int_handler);
-  all_args_t     args;
+  all_args_t args;
   metrics_stdout metrics;
-  ue            *ue = ue::get_instance();
+  ue *ue = ue::get_instance();
 
   cout << "---  Software Radio Systems LTE UE  ---" << endl << endl;
 
   parse_args(&args, argc, argv);
-  if(!ue->init(&args)) {
+  if (!ue->init(&args)) {
     exit(1);
   }
   metrics.init(ue, args.expert.metrics_period_secs);
@@ -355,17 +359,17 @@ int main(int argc, char *argv[])
   pthread_t input;
   pthread_create(&input, NULL, &input_loop, &metrics);
 
-  bool plot_started         = false; 
-  bool signals_pregenerated = false; 
-  while(running) {
+  bool plot_started = false;
+  bool signals_pregenerated = false;
+  while (running) {
     if (ue->is_attached()) {
       if (!signals_pregenerated && args.expert.pregenerate_signals) {
         ue->pregenerate_signals(true);
-        signals_pregenerated = true; 
+        signals_pregenerated = true;
       }
       if (!plot_started && args.gui.enable) {
         ue->start_plot();
-        plot_started = true; 
+        plot_started = true;
       }
     }
     sleep(1);

From c1ef9da32a17ee5e4a45fd79c84170fefe676950 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Sun, 11 Jun 2017 16:59:31 -0500
Subject: [PATCH 002/269] avx turbo decoder working in tests

---
 .../srslte/phy/fec/turbodecoder_simd.h        | 135 +++++
 lib/src/phy/fec/test/turbodecoder_test.c      |  55 +-
 lib/src/phy/fec/turbodecoder.c                |  67 ++-
 lib/src/phy/fec/turbodecoder_avx.c            | 401 +++++++++++++++
 lib/src/phy/fec/turbodecoder_simd.c           | 486 ++++++++++++++++++
 lib/src/phy/fec/turbodecoder_sse.c            |  15 +-
 6 files changed, 1121 insertions(+), 38 deletions(-)
 create mode 100644 lib/include/srslte/phy/fec/turbodecoder_simd.h
 create mode 100644 lib/src/phy/fec/turbodecoder_avx.c
 create mode 100644 lib/src/phy/fec/turbodecoder_simd.c

diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd.h b/lib/include/srslte/phy/fec/turbodecoder_simd.h
new file mode 100644
index 000000000..7ca3914ca
--- /dev/null
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd.h
@@ -0,0 +1,135 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/**********************************************************************************************
+ *  File:         turbodecoder.h
+ *
+ *  Description:  Turbo Decoder.
+ *                Parallel Concatenated Convolutional Code (PCCC) with two 8-state constituent
+ *                encoders and one turbo code internal interleaver. The coding rate of turbo
+ *                encoder is 1/3.
+ *                MAP_GEN is the MAX-LOG-MAP generic implementation of the decoder.
+ *
+ *  Reference:    3GPP TS 36.212 version 10.0.0 Release 10 Sec. 5.1.3.2
+ *********************************************************************************************/
+
+#ifndef TURBODECODER_SSE_
+#define TURBODECODER_SSE_
+
+#include "srslte/config.h"
+#include "srslte/phy/fec/tc_interl.h"
+#include "srslte/phy/fec/cbsegm.h"
+
+//#define ENABLE_SIMD_INTER
+
+// The constant SRSLTE_TDEC_NPAR defines the maximum number of parallel CB supported by all SIMD decoders 
+#ifdef ENABLE_SIMD_INTER
+  #include "srslte/phy/fec/turbodecoder_simd_inter.h"
+  #if LV_HAVE_AVX2
+    #define SRSLTE_TDEC_NPAR_INTRA 2
+  #else
+    #define SRSLTE_TDEC_NPAR_INTRA 1
+  #endif
+#else
+  #if LV_HAVE_AVX2
+    #define SRSLTE_TDEC_NPAR 2
+  #else
+    #define SRSLTE_TDEC_NPAR 1
+  #endif
+#endif
+
+#define SRSLTE_TCOD_RATE 3
+#define SRSLTE_TCOD_TOTALTAIL 12
+
+#define SRSLTE_TCOD_MAX_LEN_CB     6144
+#define SRSLTE_TCOD_MAX_LEN_CODED  (SRSLTE_TCOD_RATE*SRSLTE_TCOD_MAX_LEN_CB+SRSLTE_TCOD_TOTALTAIL)
+
+typedef struct SRSLTE_API {
+  uint32_t max_long_cb;
+  uint32_t max_par_cb; 
+  int16_t *alpha;
+  int16_t *branch;
+} map_gen_t;
+
+typedef struct SRSLTE_API {
+  uint32_t max_long_cb;
+  uint32_t max_par_cb; 
+  
+  map_gen_t dec;
+
+  int16_t *app1[SRSLTE_TDEC_NPAR];
+  int16_t *app2[SRSLTE_TDEC_NPAR];
+  int16_t *ext1[SRSLTE_TDEC_NPAR];
+  int16_t *ext2[SRSLTE_TDEC_NPAR];
+  int16_t *syst[SRSLTE_TDEC_NPAR];
+  int16_t *parity0[SRSLTE_TDEC_NPAR];
+  int16_t *parity1[SRSLTE_TDEC_NPAR];
+  
+  int cb_mask; 
+  int current_cbidx; 
+  srslte_tc_interl_t interleaver[SRSLTE_NOF_TC_CB_SIZES];
+  int n_iter[SRSLTE_TDEC_NPAR];
+} srslte_tdec_simd_t;
+
+SRSLTE_API int srslte_tdec_simd_init(srslte_tdec_simd_t * h, 
+                                     uint32_t max_par_cb, 
+                                     uint32_t max_long_cb);
+
+SRSLTE_API void srslte_tdec_simd_free(srslte_tdec_simd_t * h);
+
+SRSLTE_API int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, 
+                                      uint32_t long_cb);
+
+SRSLTE_API int srslte_tdec_simd_get_nof_iterations_cb(srslte_tdec_simd_t * h, 
+                                                      uint32_t cb_idx);
+
+SRSLTE_API int srslte_tdec_simd_reset_cb(srslte_tdec_simd_t * h, 
+                                         uint32_t cb_idx);
+
+SRSLTE_API void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, 
+                                           int16_t * input[SRSLTE_TDEC_NPAR], 
+                                           uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, 
+                                          uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                          uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, 
+                                               uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                               uint32_t long_cb); 
+
+SRSLTE_API void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h, 
+                                                  uint8_t *output, 
+                                                  uint32_t cbidx, 
+                                                  uint32_t long_cb); 
+
+SRSLTE_API int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, 
+                                        int16_t * input[SRSLTE_TDEC_NPAR], 
+                                        uint8_t *output[SRSLTE_TDEC_NPAR],
+                                        uint32_t nof_iterations, 
+                                        uint32_t long_cb);
+
+#endif
diff --git a/lib/src/phy/fec/test/turbodecoder_test.c b/lib/src/phy/fec/test/turbodecoder_test.c
index 1d4fd7024..dad9a07c8 100644
--- a/lib/src/phy/fec/test/turbodecoder_test.c
+++ b/lib/src/phy/fec/test/turbodecoder_test.c
@@ -46,6 +46,7 @@ uint32_t seed = 0;
 int K = -1;
 
 #define MAX_ITERATIONS  10
+int nof_cb = 1; 
 int nof_iterations = MAX_ITERATIONS;
 int test_known_data = 0;
 int test_errors = 0;
@@ -59,6 +60,7 @@ void usage(char *prog) {
   printf("Usage: %s [nlesv]\n", prog);
   printf(
       "\t-k Test with known data (ignores frame_length) [Default disabled]\n");
+  printf("\t-c nof_cb in parallel [Default %d]\n", nof_cb);
   printf("\t-i nof_iterations [Default %d]\n", nof_iterations);
   printf("\t-n nof_frames [Default %d]\n", nof_frames);
   printf("\t-N nof_repetitions [Default %d]\n", nof_repetitions);
@@ -70,8 +72,11 @@ void usage(char *prog) {
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "inNlstvekt")) != -1) {
+  while ((opt = getopt(argc, argv, "cinNlstvekt")) != -1) {
     switch (opt) {
+    case 'c':
+      nof_cb = atoi(argv[optind]);
+      break;
     case 'n':
       nof_frames = atoi(argv[optind]);
       break;
@@ -112,7 +117,7 @@ int main(int argc, char **argv) {
   float *llr;
   short *llr_s;
   uint8_t *llr_c;
-  uint8_t *data_tx, *data_rx, *data_rx_bytes, *symbols;
+  uint8_t *data_tx, *data_rx, *data_rx_bytes[SRSLTE_TDEC_NPAR], *symbols;
   uint32_t i, j;
   float var[SNR_POINTS];
   uint32_t snr_points;
@@ -154,10 +159,12 @@ int main(int argc, char **argv) {
     perror("malloc");
     exit(-1);
   }
-  data_rx_bytes = srslte_vec_malloc(frame_length * sizeof(uint8_t));
-  if (!data_rx_bytes) {
-    perror("malloc");
-    exit(-1);
+  for (int cb=0;cb<SRSLTE_TDEC_NPAR;cb++) {
+    data_rx_bytes[cb] = srslte_vec_malloc(frame_length * sizeof(uint8_t));
+    if (!data_rx_bytes[cb]) {
+      perror("malloc");
+      exit(-1);
+    }    
   }
 
   symbols = srslte_vec_malloc(coded_length * sizeof(uint8_t));
@@ -232,7 +239,6 @@ int main(int argc, char **argv) {
       for (j = 0; j < coded_length; j++) {
         llr[j] = symbols[j] ? 1 : -1;
       }
-
       srslte_ch_awgn_f(llr, llr, var[i], coded_length);
 
       for (j=0;j<coded_length;j++) {
@@ -248,23 +254,38 @@ int main(int argc, char **argv) {
         t = nof_iterations;
       }
 
+      int16_t *input[SRSLTE_TDEC_NPAR];
+      uint8_t *output[SRSLTE_TDEC_NPAR];        
+      
+      input[0] = llr_s; 
+      if (SRSLTE_TDEC_NPAR == 2)
+        input[1] = llr_s; 
+      
+      output[0] = data_rx_bytes[0]; 
+      if (SRSLTE_TDEC_NPAR == 2)
+        output[1] = data_rx_bytes[1]; 
+
       gettimeofday(&tdata[1], NULL); 
-      for (int k=0;k<nof_repetitions;k++) {     
-        srslte_tdec_run_all(&tdec, llr_s, data_rx_bytes, t, frame_length);        
+      for (int k=0;k<nof_repetitions;k++) { 
+        srslte_tdec_run_all_par(&tdec, input, output, t, nof_cb, frame_length);        
       }
       gettimeofday(&tdata[2], NULL);
       get_time_interval(tdata);
       mean_usec = (float) mean_usec * 0.9 + (float) (tdata[0].tv_usec/nof_repetitions) * 0.1;
       
-      srslte_bit_unpack_vector(data_rx_bytes, data_rx, frame_length);
-
-      errors += srslte_bit_diff(data_tx, data_rx, frame_length);
-      
       frame_cnt++;
+      uint32_t errors_this = 0; 
+      for (int cb=0;cb<nof_cb;cb++) {
+        srslte_bit_unpack_vector(data_rx_bytes[cb], data_rx, frame_length);
+        
+        errors_this=srslte_bit_diff(data_tx, data_rx, frame_length);
+        //printf("error[%d]=%d\n", cb, errors_this);
+        errors += errors_this;
+      }
       printf("Eb/No: %2.2f %10d/%d   ", SNR_MIN + i * ebno_inc, frame_cnt, nof_frames);
-      printf("BER: %.2e  ", (float) errors / (frame_cnt * frame_length));
-      printf("%3.1f Mbps (%6.2f usec)", (float) frame_length / mean_usec, mean_usec);
-      printf("\r");
+      printf("BER: %.2e  ", (float) errors / (nof_cb*frame_cnt * frame_length));
+      printf("%3.1f Mbps (%6.2f usec)", (float) SRSLTE_TDEC_NPAR*frame_length / mean_usec, mean_usec);
+      printf("\r");        
 
     }    
     printf("\n");
@@ -273,7 +294,7 @@ int main(int argc, char **argv) {
   printf("\n");
   if (snr_points == 1) {
     if (errors) {
-      printf("%d Errors\n", errors);
+      printf("%d Errors\n", errors/nof_cb);
     }
   }    
 
diff --git a/lib/src/phy/fec/turbodecoder.c b/lib/src/phy/fec/turbodecoder.c
index 145c88d6d..17cb52957 100644
--- a/lib/src/phy/fec/turbodecoder.c
+++ b/lib/src/phy/fec/turbodecoder.c
@@ -35,7 +35,7 @@
 
 
 #ifdef LV_HAVE_SSE
-#include "srslte/phy/fec/turbodecoder_sse.h"
+#include "srslte/phy/fec/turbodecoder_simd.h"
 #endif
 
 #include "srslte/phy/utils/vector.h"
@@ -43,7 +43,7 @@
 
 int srslte_tdec_init(srslte_tdec_t * h, uint32_t max_long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_sse_init(&h->tdec_sse, max_long_cb);
+  return srslte_tdec_simd_init(&h->tdec_simd, max_long_cb);
 #else
   h->input_conv = srslte_vec_malloc(sizeof(float) * (3*max_long_cb+12));
   if (!h->input_conv) {
@@ -56,7 +56,7 @@ int srslte_tdec_init(srslte_tdec_t * h, uint32_t max_long_cb) {
 
 void srslte_tdec_free(srslte_tdec_t * h) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_sse_free(&h->tdec_sse);
+  srslte_tdec_simd_free(&h->tdec_simd);
 #else
   if (h->input_conv) {
     free(h->input_conv);
@@ -68,45 +68,74 @@ void srslte_tdec_free(srslte_tdec_t * h) {
 
 int srslte_tdec_reset(srslte_tdec_t * h, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_sse_reset(&h->tdec_sse, long_cb);
+  return srslte_tdec_simd_reset(&h->tdec_simd, long_cb);
 #else
   return srslte_tdec_gen_reset(&h->tdec_gen, long_cb);
 #endif
 }
 
-void srslte_tdec_iteration(srslte_tdec_t * h, int16_t* input, uint32_t long_cb) {
+void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_sse_iteration(&h->tdec_sse, input, long_cb);
+  srslte_tdec_simd_iteration(&h->tdec_simd, input, nof_cb, long_cb);
 #else
-  srslte_vec_convert_if(input, h->input_conv, 0.01, 3*long_cb+12);
+  srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
   srslte_tdec_gen_iteration(&h->tdec_gen, h->input_conv, long_cb);
 #endif
 }
 
-void srslte_tdec_decision(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
+void srslte_tdec_iteration(srslte_tdec_t * h, int16_t* input, uint32_t long_cb) {
+  int16_t *input_par[SRSLTE_TDEC_NPAR]; 
+  input_par[0] = input; 
+  return srslte_tdec_iteration_par(h, input_par, 1, long_cb);
+}
+
+void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_sse_decision(&h->tdec_sse, output, long_cb);
+  return srslte_tdec_simd_decision(&h->tdec_simd, output, nof_cb, long_cb);
 #else
-  return srslte_tdec_gen_decision(&h->tdec_gen, output, long_cb);
+  return srslte_tdec_gen_decision(&h->tdec_gen, output[0], long_cb);
 #endif
+}
+
+void srslte_tdec_decision(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
+  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  output_par[0] = output; 
+  srslte_tdec_decision_par(h, output_par, 1, long_cb);
+}
 
+void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
+#ifdef LV_HAVE_SSE
+  srslte_tdec_simd_decision_byte(&h->tdec_simd, output, nof_cb, long_cb);
+#else
+  srslte_tdec_gen_decision_byte(&h->tdec_gen, output[0], long_cb);
+#endif  
 }
 
 void srslte_tdec_decision_byte(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
+  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  output_par[0] = output; 
+  srslte_tdec_decision_byte_par(h, output_par, 1, long_cb);
+}
+
+int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_NPAR], 
+                            uint8_t *output[SRSLTE_TDEC_NPAR], 
+                            uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_sse_decision_byte(&h->tdec_sse, output, long_cb);
+  return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, nof_cb, long_cb);
 #else
-  return srslte_tdec_gen_decision_byte(&h->tdec_gen, output, long_cb);
+  srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
+  return srslte_tdec_gen_run_all(&h->tdec_gen, h->input_conv, output[0], nof_iterations, long_cb);
 #endif
-  
 }
 
 int srslte_tdec_run_all(srslte_tdec_t * h, int16_t * input, uint8_t *output, uint32_t nof_iterations, uint32_t long_cb)
 {
-#ifdef LV_HAVE_SSE
-  return srslte_tdec_sse_run_all(&h->tdec_sse, input, output, nof_iterations, long_cb);
-#else
-  srslte_vec_convert_if(input, h->input_conv, 0.01, 3*long_cb+12);
-  return srslte_tdec_gen_run_all(&h->tdec_gen, h->input_conv, output, nof_iterations, long_cb);
-#endif
+  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  output_par[0] = output;   
+  int16_t *input_par[SRSLTE_TDEC_NPAR]; 
+  input_par[0] = input; 
+ 
+  return srslte_tdec_run_all_par(h, input_par, output_par, nof_iterations, 1, long_cb);
 }
+
+
diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
new file mode 100644
index 000000000..82cceb5d2
--- /dev/null
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -0,0 +1,401 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <strings.h>
+#include <math.h>
+
+#include "srslte/phy/fec/turbodecoder_simd.h"
+#include "srslte/phy/utils/vector.h"
+
+#include <inttypes.h>
+
+#define NUMSTATES       8
+#define NINPUTS         2
+#define TAIL            3
+#define TOTALTAIL       12
+
+#define INF 10000
+#define ZERO 0
+
+
+#ifdef LV_HAVE_AVX2
+
+#include <smmintrin.h>
+#include <immintrin.h>
+
+
+// Number of CB processed in parllel in AVX
+#define NCB 2 
+
+/*
+static void print_256i(__m256i x) {
+  int16_t *s = (int16_t*) &x; 
+  printf("[%d", s[0]);
+  for (int i=1;i<16;i++) {
+    printf(",%d", s[i]);
+  }
+  printf("]\n");
+}
+*/
+
+/* Computes the horizontal MAX from 8 16-bit integers using the minpos_epu16 SSE4.1 instruction */
+static inline int16_t hMax0(__m256i masked_value)
+{
+  __m128i tmp1 = _mm256_extractf128_si256(masked_value, 0); 
+  __m128i tmp3 = _mm_minpos_epu16(tmp1);
+  return (int16_t)(_mm_cvtsi128_si32(tmp3));
+}
+
+static inline int16_t hMax1(__m256i masked_value)
+{
+  __m128i tmp1 = _mm256_extractf128_si256(masked_value, 1); 
+  __m128i tmp3 = _mm_minpos_epu16(tmp1);
+  return (int16_t)(_mm_cvtsi128_si32(tmp3));
+}
+
+/* Computes beta values */
+void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
+{
+  int k;
+  uint32_t end = long_cb + 3;
+  const __m256i *alphaPtr = (const __m256i*) s->alpha;
+ 
+  __m256i beta_k = _mm256_set_epi16(-INF, -INF, -INF, -INF, -INF, -INF, -INF, 0, -INF, -INF, -INF, -INF, -INF, -INF, -INF, 0);
+  __m256i g, bp, bn, alpha_k; 
+  
+  /* Define the shuffle constant for the positive beta */
+  __m256i shuf_bp = _mm256_set_epi8(
+    // 1st CB
+    15+16, 14+16, // 7
+    7+16,  6+16,  // 3
+    5+16,  4+16,  // 2
+    13+16, 12+16, // 6
+    11+16, 10+16, // 5
+    3+16,  2+16,  // 1
+    1+16,  0+16,  // 0
+    9+16,  8+16,  // 4
+
+    // 2nd CB
+    15, 14, // 7
+    7,  6,  // 3
+    5,  4,  // 2
+    13, 12, // 6
+    11, 10, // 5
+    3,  2,  // 1
+    1,  0,  // 0
+    9,  8   // 4
+  );
+
+  /* Define the shuffle constant for the negative beta */
+  __m256i shuf_bn = _mm256_set_epi8(
+    7+16, 6+16,   // 3
+    15+16, 14+16, // 7
+    13+16, 12+16, // 6
+    5+16,  4+16,  // 2
+    3+16,  2+16,  // 1
+    11+16, 10+16, // 5
+    9+16,  8+16,  // 4
+    1+16,  0+16,  // 0
+    
+    7,   6, // 3
+    15, 14, // 7
+    13, 12, // 6
+    5,  4,  // 2
+    3,  2,  // 1
+    11, 10, // 5
+    9,  8,  // 4
+    1,  0   // 0
+  );
+ 
+  alphaPtr += long_cb-1;
+
+  /* Define shuffle for branch costs */
+  __m256i shuf_g[4];
+  shuf_g[3] = _mm256_set_epi8(3+16,2+16,1+16,0+16,1+16,0+16,3+16,2+16,3+16,2+16,1+16,0+16,1+16,0+16,3+16,2+16, 
+                              3,2,1,0,1,0,3,2,3,2,1,0,1,0,3,2);
+  shuf_g[2] = _mm256_set_epi8(7+16,6+16,5+16,4+16,5+16,4+16,7+16,6+16,7+16,6+16,5+16,4+16,5+16,4+16,7+16,6+16,
+                              7,6,5,4,5,4,7,6,7,6,5,4,5,4,7,6);
+  shuf_g[1] = _mm256_set_epi8(11+16,10+16,9+16,8+16,9+16,8+16,11+16,10+16,11+16,10+16,9+16,8+16,9+16,8+16,11+16,10+16,
+                              11,10,9,8,9,8,11,10,11,10,9,8,9,8,11,10);
+  shuf_g[0] = _mm256_set_epi8(15+16,14+16,13+16,12+16,13+16,12+16,15+16,14+16,15+16,14+16,13+16,12+16,13+16,12+16,15+16,14+16,
+                              15,14,13,12,13,12,15,14,15,14,13,12,13,12,15,14);
+
+  /* Define shuffle for beta normalization */
+  __m256i shuf_norm = _mm256_set_epi8(17,16,17,16,17,16,17,16,17,16,17,16,17,16,17,16,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0);
+
+  __m256i gv;
+  int16_t *b = &s->branch[2*NCB*long_cb-16];
+  __m256i *gPtr = (__m256i*) b;
+
+  __m256i bn2, bp2; 
+  
+  /* This defines a beta computation step: 
+   * Adds and substracts the branch metrics to the previous beta step, 
+   * shuffles the states according to the trellis path and selects maximum state 
+   */
+#define BETA_STEP(g)     bp = _mm256_add_epi16(beta_k, g);\
+    bn = _mm256_sub_epi16(beta_k, g);\
+    bp = _mm256_shuffle_epi8(bp, shuf_bp);\
+    bn = _mm256_shuffle_epi8(bn, shuf_bn);\
+    beta_k = _mm256_max_epi16(bp, bn);    
+
+    /* Loads the alpha metrics from memory and adds them to the temporal bn and bp 
+     * metrics. Then computes horizontal maximum of both metrics and computes difference
+     */
+#define BETA_STEP_CNT(c,d) g = _mm256_shuffle_epi8(gv, shuf_g[c]);\
+    BETA_STEP(g)\
+    alpha_k = _mm256_load_si256(alphaPtr);\
+    alphaPtr--;\
+    bp = _mm256_add_epi16(bp, alpha_k);\
+    bn = _mm256_add_epi16(bn, alpha_k);\
+    bn2 = _mm256_sub_epi8(_mm256_set1_epi16(0x7FFF), bn);\
+    bp2 = _mm256_sub_epi8(_mm256_set1_epi16(0x7FFF), bp);\
+    output[0][k-d] = hMax0(bn2) - hMax0(bp2);\
+    output[1][k-d] = hMax1(bn2) - hMax1(bp2);
+
+  /* The tail does not require to load alpha or produce outputs. Only update 
+   * beta metrics accordingly */
+  for (k=end-1; k>=long_cb; k--) {
+    int16_t g0_1 = s->branch[2*NCB*k];
+    int16_t g1_1 = s->branch[2*NCB*k+1];
+    int16_t g0_2 = s->branch[2*NCB*k+6];
+    int16_t g1_2 = s->branch[2*NCB*k+6+1];
+    g = _mm256_set_epi16(g1_2, g0_2, g0_2, g1_2, g1_2, g0_2, g0_2, g1_2, g1_1, g0_1, g0_1, g1_1, g1_1, g0_1, g0_1, g1_1);
+    BETA_STEP(g);
+  }  
+  
+  /* We inline 2 trelis steps for each normalization */
+  __m256i norm;
+  for (; k >= 0; k-=8) {    
+    gv = _mm256_load_si256(gPtr);
+    gPtr--;
+    BETA_STEP_CNT(0,0);
+    BETA_STEP_CNT(1,1);
+    BETA_STEP_CNT(2,2);
+    BETA_STEP_CNT(3,3);
+    norm = _mm256_shuffle_epi8(beta_k, shuf_norm); 
+    beta_k = _mm256_sub_epi16(beta_k, norm);
+    gv = _mm256_load_si256(gPtr);
+    gPtr--;
+    BETA_STEP_CNT(0,4);
+    BETA_STEP_CNT(1,5);
+    BETA_STEP_CNT(2,6);
+    BETA_STEP_CNT(3,7);
+    norm = _mm256_shuffle_epi8(beta_k, shuf_norm); 
+    beta_k = _mm256_sub_epi16(beta_k, norm);
+  }  
+}
+
+/* Computes alpha metrics */
+void map_avx_alpha(map_gen_t * s, uint32_t long_cb)
+{
+  uint32_t k;
+  int16_t *alpha1 = s->alpha;
+  int16_t *alpha2 = &s->alpha[8];
+  uint32_t i;
+
+  alpha1[0] = 0; 
+  alpha2[0] = 0; 
+  for (i = 1; i < 8; i++) {
+    alpha1[i] = -INF;
+    alpha2[i] = -INF;
+  }
+    
+  /* Define the shuffle constant for the positive alpha */
+  __m256i shuf_ap = _mm256_set_epi8(
+
+    // 1st CB 
+    31, 30, // 7
+    25, 24, // 4
+    23, 22, // 3
+    17, 16, // 0
+    29, 28, // 6
+    27, 26, // 5
+    21, 20, // 2
+    19, 18, // 1
+
+    // 2nd CB
+    15, 14, // 7
+    9,  8,  // 4
+    7,  6,  // 3
+    1,  0,  // 0
+    13, 12, // 6
+    11, 10, // 5
+    5,  4,  // 2
+    3,  2   // 1
+  );
+
+  /* Define the shuffle constant for the negative alpha */
+  __m256i shuf_an = _mm256_set_epi8(
+
+    // 1nd CB 
+    29, 28, // 6
+    27, 26, // 5
+    21, 20, // 2
+    19, 18, // 1
+    31, 30, // 7
+    25, 24, // 4
+    23, 22, // 3
+    17, 16, // 0
+
+    // 2nd CB 
+    13, 12, // 6
+    11, 10, // 5
+    5,  4,  // 2
+    3,  2,  // 1
+    15, 14, // 7
+    9,  8,  // 4
+    7,  6,  // 3
+    1,  0   // 0
+  );
+  
+  /* Define shuffle for branch costs */
+  __m256i shuf_g[4];
+  shuf_g[0] = _mm256_set_epi8(3+16,2+16,3+16,2+16,1+16,0+16,1+16,0+16,1+16,0+16,1+16,0+16,3+16,2+16,3+16,2+16,       
+                              3,2,3,2,1,0,1,0,1,0,1,0,3,2,3,2);
+  shuf_g[1] = _mm256_set_epi8(7+16,6+16,7+16,6+16,5+16,4+16,5+16,4+16,5+16,4+16,5+16,4+16,7+16,6+16,7+16,6+16,    
+                              7,6,7,6,5,4,5,4,5,4,5,4,7,6,7,6);
+  shuf_g[2] = _mm256_set_epi8(11+16,10+16,11+16,10+16,9+16,8+16,9+16,8+16,9+16,8+16,9+16,8+16,11+16,10+16,11+16,10+16,       
+                              11,10,11,10,9,8,9,8,9,8,9,8,11,10,11,10);
+  shuf_g[3] = _mm256_set_epi8(15+16,14+16,15+16,14+16,13+16,12+16,13+16,12+16,13+16,12+16,13+16,12+16,15+16,14+16,15+16,14+16,
+                              15,14,15,14,13,12,13,12,13,12,13,12,15,14,15,14);
+
+  __m256i shuf_norm = _mm256_set_epi8(17,16,17,16,17,16,17,16,17,16,17,16,17,16,17,16,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0);
+  
+  __m256i* alphaPtr = (__m256i*) s->alpha;
+  alphaPtr++;
+
+  __m256i gv; 
+  __m256i *gPtr = (__m256i*) s->branch;
+  __m256i g, ap, an; 
+    
+  __m256i alpha_k = _mm256_set_epi16(-INF, -INF, -INF, -INF, -INF, -INF, -INF, 0, -INF, -INF, -INF, -INF, -INF, -INF, -INF, 0);
+  
+  /* This defines a alpha computation step: 
+   * Adds and substracts the branch metrics to the previous alpha step, 
+   * shuffles the states according to the trellis path and selects maximum state 
+   */
+#define ALPHA_STEP(c)  g = _mm256_shuffle_epi8(gv, shuf_g[c]); \
+  ap = _mm256_add_epi16(alpha_k, g);\
+  an = _mm256_sub_epi16(alpha_k, g);\
+  ap = _mm256_shuffle_epi8(ap, shuf_ap);\
+  an = _mm256_shuffle_epi8(an, shuf_an);\
+  alpha_k = _mm256_max_epi16(ap, an);\
+  _mm256_store_si256(alphaPtr, alpha_k);\
+  alphaPtr++;    \
+
+
+  /* In this loop, we compute 8 steps and normalize twice for each branch metrics memory load */
+  __m256i norm;
+  for (k = 0; k < long_cb/8; k++) {
+    gv = _mm256_load_si256(gPtr);
+    
+    gPtr++;
+    ALPHA_STEP(0);
+    ALPHA_STEP(1);
+    ALPHA_STEP(2);
+    ALPHA_STEP(3);
+    norm = _mm256_shuffle_epi8(alpha_k, shuf_norm); 
+    alpha_k = _mm256_sub_epi16(alpha_k, norm);
+    gv = _mm256_load_si256(gPtr);
+    gPtr++;
+    ALPHA_STEP(0);
+    ALPHA_STEP(1);
+    ALPHA_STEP(2);
+    ALPHA_STEP(3);
+    norm = _mm256_shuffle_epi8(alpha_k, shuf_norm); 
+    alpha_k = _mm256_sub_epi16(alpha_k, norm);
+  }  
+}
+
+/* Compute branch metrics (gamma) */
+void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t long_cb) 
+{
+  __m128i res10, res20, res11, res21, res1, res2; 
+  __m128i in, ap, pa, g1, g0;
+
+  __m128i *inPtr  = (__m128i*) input;
+  __m128i *appPtr = (__m128i*) app;
+  __m128i *paPtr  = (__m128i*) parity;
+  __m128i *resPtr = (__m128i*) h->branch;
+  
+  if (cbidx) {
+    resPtr++;
+  }
+  
+  __m128i res10_mask = _mm_set_epi8(0xff,0xff,7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0);
+  __m128i res20_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
+  __m128i res11_mask = _mm_set_epi8(7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0,0xff,0xff);
+  __m128i res21_mask = _mm_set_epi8(15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8,0xff,0xff);
+
+  for (int i=0;i<long_cb/8;i++) {
+    in = _mm_load_si128(inPtr);
+    inPtr++;
+    pa = _mm_load_si128(paPtr);
+    paPtr++;
+    
+    if (appPtr) {
+      ap = _mm_load_si128(appPtr);
+      appPtr++;
+      in = _mm_add_epi16(ap, in);
+    }
+    
+    g1 = _mm_add_epi16(in, pa);
+    g0 = _mm_sub_epi16(in, pa);
+
+    g1 = _mm_srai_epi16(g1, 1);
+    g0 = _mm_srai_epi16(g0, 1);
+    
+    res10 = _mm_shuffle_epi8(g0, res10_mask);
+    res20 = _mm_shuffle_epi8(g0, res20_mask);
+    res11 = _mm_shuffle_epi8(g1, res11_mask);
+    res21 = _mm_shuffle_epi8(g1, res21_mask);
+
+    res1  = _mm_or_si128(res10, res11);
+    res2  = _mm_or_si128(res20, res21);
+
+    _mm_store_si128(resPtr, res1);
+    resPtr++;
+    resPtr++;
+    _mm_store_si128(resPtr, res2);    
+    resPtr++;
+    resPtr++;    
+  }
+    
+  for (int i=long_cb;i<long_cb+3;i++) {
+    h->branch[2*i*NCB+cbidx*6]   = (input[i] - parity[i])/2;
+    h->branch[2*i*NCB+cbidx*6+1] = (input[i] + parity[i])/2;
+  }
+}
+
+
+#endif
+
+
diff --git a/lib/src/phy/fec/turbodecoder_simd.c b/lib/src/phy/fec/turbodecoder_simd.c
new file mode 100644
index 000000000..a2cddd582
--- /dev/null
+++ b/lib/src/phy/fec/turbodecoder_simd.c
@@ -0,0 +1,486 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <strings.h>
+#include <math.h>
+
+#include "srslte/phy/fec/turbodecoder_simd.h"
+#include "srslte/phy/utils/vector.h"
+
+#include <inttypes.h>
+
+#define NUMSTATES       8
+#define NINPUTS         2
+#define TAIL            3
+#define TOTALTAIL       12
+
+#define INF 10000
+#define ZERO 0
+
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+
+// Define SSE/AVX implementations 
+void map_sse_beta(map_gen_t * s, int16_t * output, uint32_t long_cb);
+void map_sse_alpha(map_gen_t * s, uint32_t long_cb);
+void map_sse_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t long_cb);
+
+#ifdef LV_HAVE_AVX2
+void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t long_cb);
+void map_avx_alpha(map_gen_t * s, uint32_t long_cb);
+void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t long_cb);
+#endif
+
+
+void map_simd_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+  if (nof_cb == 1) {
+    map_sse_beta(s, output[0], long_cb);
+  } 
+#ifdef LV_HAVE_AVX2
+  else if (nof_cb == 2) {
+    map_avx_beta(s, output, long_cb);
+  }
+#endif
+}
+
+void map_simd_alpha(map_gen_t * s, uint32_t nof_cb, uint32_t long_cb)
+{
+  if (nof_cb == 1) {
+    map_sse_alpha(s, long_cb);
+  }
+#ifdef LV_HAVE_AVX2
+  else if (nof_cb == 2) {
+    map_avx_alpha(s, long_cb);
+  }
+#endif
+}
+void map_simd_gamma(map_gen_t * s, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t nof_cb, uint32_t long_cb) 
+{
+  if (nof_cb == 1) {
+    map_sse_gamma(s, input, app, parity, long_cb);
+  } 
+#ifdef LV_HAVE_AVX2
+  else if (nof_cb == 2) {
+    map_avx_gamma(s, input, app, parity, cbidx, long_cb);
+  }    
+#endif
+}
+
+/* Inititalizes constituent decoder object */
+int map_simd_init(map_gen_t * h, int max_long_cb)
+{
+  bzero(h, sizeof(map_gen_t));
+  h->alpha = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * SRSLTE_TDEC_NPAR);
+  if (!h->alpha) {
+    perror("srslte_vec_malloc");
+    return -1;
+  }
+  h->branch = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * SRSLTE_TDEC_NPAR);
+  if (!h->branch) {
+    perror("srslte_vec_malloc");
+    return -1;
+  }
+  h->max_long_cb = max_long_cb;
+  return 0;
+}
+
+void map_simd_free(map_gen_t * h)
+{
+  if (h->alpha) {
+    free(h->alpha);
+  }
+  if (h->branch) {
+    free(h->branch);
+  }
+  bzero(h, sizeof(map_gen_t));
+}
+
+/* Runs one instance of a decoder */
+void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_NPAR], int16_t *app[SRSLTE_TDEC_NPAR], int16_t * parity[SRSLTE_TDEC_NPAR], 
+                  int16_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+ 
+  // Compute branch metrics
+  for (int i=0;i<nof_cb;i++) {
+    map_simd_gamma(h, input[i], app?app[i]:NULL, parity[i], i, nof_cb, long_cb);    
+  }
+
+  // Forward recursion
+  map_simd_alpha(h, nof_cb, long_cb);
+
+  // Backwards recursion + LLR computation
+  map_simd_beta(h, output, nof_cb, long_cb);
+}
+
+/* Initializes the turbo decoder object */
+int srslte_tdec_simd_init(srslte_tdec_simd_t * h, uint32_t max_long_cb)
+{
+  int ret = -1;
+  bzero(h, sizeof(srslte_tdec_simd_t));
+  uint32_t len = max_long_cb + SRSLTE_TCOD_TOTALTAIL;
+
+  h->max_long_cb = max_long_cb;
+
+  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+    h->app1[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->app1[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->app2[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->app2[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->ext1[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->ext1[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->ext2[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->ext2[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->syst[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->syst[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->parity0[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->parity0[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    h->parity1[i] = srslte_vec_malloc(sizeof(int16_t) * len);
+    if (!h->parity1[i]) {
+      perror("srslte_vec_malloc");
+      goto clean_and_exit;
+    }
+    
+  }
+
+  if (map_simd_init(&h->dec, h->max_long_cb)) {
+    goto clean_and_exit;
+  }
+
+  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
+    if (srslte_tc_interl_init(&h->interleaver[i], srslte_cbsegm_cbsize(i)) < 0) {
+      goto clean_and_exit;
+    }
+    srslte_tc_interl_LTE_gen(&h->interleaver[i], srslte_cbsegm_cbsize(i));
+  }
+  h->current_cbidx = -1; 
+  ret = 0;
+clean_and_exit:if (ret == -1) {
+    srslte_tdec_simd_free(h);
+  }
+  return ret;
+}
+
+void srslte_tdec_simd_free(srslte_tdec_simd_t * h)
+{
+  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+    if (h->app1[i]) {
+      free(h->app1[i]);
+    }
+    if (h->app2[i]) {
+      free(h->app2[i]);
+    }
+    if (h->ext1[i]) {
+      free(h->ext1[i]);
+    }
+    if (h->ext2[i]) {
+      free(h->ext2[i]);
+    }
+    if (h->syst[i]) {
+      free(h->syst[i]);
+    }
+    if (h->parity0[i]) {
+      free(h->parity0[i]);
+    }
+    if (h->parity1[i]) {
+      free(h->parity1[i]);
+    }    
+  }
+
+  map_simd_free(&h->dec);
+
+  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
+    srslte_tc_interl_free(&h->interleaver[i]);    
+  }
+
+  bzero(h, sizeof(srslte_tdec_simd_t));
+}
+
+/* Deinterleaves the 3 streams from the input (systematic and 2 parity bits) into 
+ * 3 buffers ready to be used by compute_gamma() 
+ */
+void deinterleave_input_simd(srslte_tdec_simd_t *h, int16_t *input, uint32_t cbidx, uint32_t long_cb) {
+  uint32_t i;
+ 
+  __m128i *inputPtr = (__m128i*) input; 
+  __m128i in0, in1, in2;
+  __m128i s0, s1, s2, s;
+  __m128i p00, p01, p02, p0;
+  __m128i p10, p11, p12, p1;
+  
+  __m128i *sysPtr = (__m128i*) h->syst[cbidx]; 
+  __m128i *pa0Ptr = (__m128i*) h->parity0[cbidx]; 
+  __m128i *pa1Ptr = (__m128i*) h->parity1[cbidx]; 
+  
+  // pick bits 0, 3, 6 from 1st word
+  __m128i s0_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,13,12,7,6,1,0);
+  // pick bits 1, 4, 7 from 2st word
+  __m128i s1_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,15,14,9,8,3,2,0xff,0xff,0xff,0xff,0xff,0xff);
+  // pick bits 2, 5 from 3rd word
+  __m128i s2_mask = _mm_set_epi8(11,10,5,4,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
+
+  // pick bits 1, 4, 7 from 1st word
+  __m128i p00_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,15,14,9,8,3,2);
+  // pick bits 2, 5, from 2st word
+  __m128i p01_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,11,10,5,4,0xff,0xff,0xff,0xff,0xff,0xff);
+  // pick bits 0, 3, 6 from 3rd word
+  __m128i p02_mask = _mm_set_epi8(13,12,7,6,1,0,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
+  
+  // pick bits 2, 5 from 1st word
+  __m128i p10_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,11,10,5,4);
+  // pick bits 0, 3, 6, from 2st word
+  __m128i p11_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,13,12,7,6,1,0,0xff,0xff,0xff,0xff);
+  // pick bits 1, 4, 7 from 3rd word
+  __m128i p12_mask = _mm_set_epi8(15,14,9,8,3,2,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
+      
+  // Split systematic and parity bits
+  for (i = 0; i < long_cb/8; i++) {
+        
+    in0 = _mm_load_si128(inputPtr); inputPtr++; 
+    in1 = _mm_load_si128(inputPtr); inputPtr++;   
+    in2 = _mm_load_si128(inputPtr); inputPtr++;
+    
+    /* Deinterleave Systematic bits */
+    s0 = _mm_shuffle_epi8(in0, s0_mask);
+    s1 = _mm_shuffle_epi8(in1, s1_mask);
+    s2 = _mm_shuffle_epi8(in2, s2_mask);    
+    s = _mm_or_si128(s0, s1);
+    s = _mm_or_si128(s, s2);
+
+    _mm_store_si128(sysPtr, s);
+    sysPtr++;
+
+    /* Deinterleave parity 0 bits */
+    p00 = _mm_shuffle_epi8(in0, p00_mask);
+    p01 = _mm_shuffle_epi8(in1, p01_mask);
+    p02 = _mm_shuffle_epi8(in2, p02_mask);    
+    p0 = _mm_or_si128(p00, p01);
+    p0 = _mm_or_si128(p0, p02);
+    
+    _mm_store_si128(pa0Ptr, p0);
+    pa0Ptr++;
+
+    /* Deinterleave parity 1 bits */
+    p10 = _mm_shuffle_epi8(in0, p10_mask);
+    p11 = _mm_shuffle_epi8(in1, p11_mask);
+    p12 = _mm_shuffle_epi8(in2, p12_mask);    
+    p1 = _mm_or_si128(p10, p11);
+    p1 = _mm_or_si128(p1, p12);
+
+    _mm_store_si128(pa1Ptr, p1);
+    pa1Ptr++;    
+
+  }
+  
+  for (i = 0; i < 3; i++) {
+    h->syst[cbidx][i+long_cb]    = input[3*long_cb + 2*i];
+    h->parity0[cbidx][i+long_cb] = input[3*long_cb + 2*i + 1];
+  }
+  for (i = 0; i < 3; i++) {
+    h->app2[cbidx][i+long_cb]    = input[3*long_cb + 6 + 2*i];
+    h->parity1[cbidx][i+long_cb] = input[3*long_cb + 6 + 2*i + 1];
+  }
+
+}
+
+/* Runs 1 turbo decoder iteration */
+void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+
+  if (h->current_cbidx >= 0) {
+    uint16_t *inter   = h->interleaver[h->current_cbidx].forward;
+    uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
+    
+    if (h->n_iter == 0) {
+      for (int i=0;i<nof_cb;i++) {
+        deinterleave_input_simd(h, input[i], i, long_cb);        
+      }        
+    }
+    
+    // Add apriori information to decoder 1 
+    if (h->n_iter > 0) {
+      for (int i=0;i<nof_cb;i++) {
+        srslte_vec_sub_sss(h->app1[i], h->ext1[i], h->app1[i], long_cb);
+      }
+    }
+
+    // Run MAP DEC #1
+    if (h->n_iter == 0) {
+      map_simd_dec(&h->dec, h->syst, NULL, h->parity0, h->ext1, nof_cb, long_cb);            
+    } else {
+      map_simd_dec(&h->dec, h->syst, h->app1, h->parity0, h->ext1, nof_cb, long_cb);      
+    }
+    
+    // Convert aposteriori information into extrinsic information    
+    if (h->n_iter > 0) {
+      for (int i=0;i<nof_cb;i++) {
+        srslte_vec_sub_sss(h->ext1[i], h->app1[i], h->ext1[i], long_cb);
+      }
+    }
+    
+    // Interleave extrinsic output of DEC1 to form apriori info for decoder 2
+    for (int i=0;i<nof_cb;i++) {
+      srslte_vec_lut_sss(h->ext1[i], deinter, h->app2[i], long_cb);
+    }
+
+    // Run MAP DEC #2. 2nd decoder uses apriori information as systematic bits
+    map_simd_dec(&h->dec, h->app2, NULL, h->parity1, h->ext2, nof_cb, long_cb);
+
+    // Deinterleaved extrinsic bits become apriori info for decoder 1 
+    for (int i=0;i<nof_cb;i++) {
+      srslte_vec_lut_sss(h->ext2[i], inter, h->app1[i], long_cb);
+    }
+
+    h->n_iter++;
+  } else {
+    fprintf(stderr, "Error CB index not set (call srslte_tdec_simd_reset() first\n");    
+  }
+}
+
+/* Resets the decoder and sets the codeblock length */
+int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, uint32_t long_cb)
+{
+  if (long_cb > h->max_long_cb) {
+    fprintf(stderr, "TDEC was initialized for max_long_cb=%d\n",
+            h->max_long_cb);
+    return -1;
+  }
+  h->n_iter = 0; 
+  h->current_cbidx = srslte_cbsegm_cbindex(long_cb);
+  if (h->current_cbidx < 0) {
+    fprintf(stderr, "Invalid CB length %d\n", long_cb);
+    return -1; 
+  }
+  return 0;
+}
+
+void tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx, uint32_t long_cb)
+{
+  __m128i zero     = _mm_set1_epi16(0);
+  __m128i lsb_mask = _mm_set1_epi16(1);
+  
+  __m128i *appPtr = (__m128i*) h->app1[cbidx];
+  __m128i *outPtr = (__m128i*) output;
+  __m128i ap, out, out0, out1; 
+    
+  for (uint32_t i = 0; i < long_cb/16; i++) {
+    ap   = _mm_load_si128(appPtr); appPtr++;    
+    out0 = _mm_and_si128(_mm_cmpgt_epi16(ap, zero), lsb_mask);
+    ap   = _mm_load_si128(appPtr); appPtr++;
+    out1 = _mm_and_si128(_mm_cmpgt_epi16(ap, zero), lsb_mask);
+    
+    out  = _mm_packs_epi16(out0, out1);
+    _mm_store_si128(outPtr, out);
+    outPtr++;
+  }
+  if (long_cb%16) {
+    for (int i=0;i<8;i++) {
+      output[long_cb-8+i] = h->app1[cbidx][long_cb-8+i]>0?1:0;
+    }
+  }
+}
+
+void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+  for (int i=0;i<nof_cb;i++) {
+    tdec_simd_decision(h, output[i], i, long_cb);
+  }
+}
+
+void tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx, uint32_t long_cb)
+{
+  uint8_t mask[8] = {0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1};
+  
+  // long_cb is always byte aligned
+  for (uint32_t i = 0; i < long_cb/8; i++) {
+    uint8_t out0 = h->app1[cbidx][8*i+0]>0?mask[0]:0;
+    uint8_t out1 = h->app1[cbidx][8*i+1]>0?mask[1]:0;
+    uint8_t out2 = h->app1[cbidx][8*i+2]>0?mask[2]:0;
+    uint8_t out3 = h->app1[cbidx][8*i+3]>0?mask[3]:0;
+    uint8_t out4 = h->app1[cbidx][8*i+4]>0?mask[4]:0;
+    uint8_t out5 = h->app1[cbidx][8*i+5]>0?mask[5]:0;
+    uint8_t out6 = h->app1[cbidx][8*i+6]>0?mask[6]:0;
+    uint8_t out7 = h->app1[cbidx][8*i+7]>0?mask[7]:0;
+    
+    output[i] = out0 | out1 | out2 | out3 | out4 | out5 | out6 | out7;
+    
+    //if (i<10) {
+    //  printf("output[%d]=%d\n",i,output[i]);
+    //}
+  }
+}
+
+void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+  for (int i=0;i<nof_cb;i++) {
+    tdec_simd_decision_byte(h, output[i], i, long_cb);
+  }
+}
+
+
+/* Runs nof_iterations iterations and decides the output bits */
+int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint8_t *output[SRSLTE_TDEC_NPAR],
+                            uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb)
+{
+  if (srslte_tdec_simd_reset(h, long_cb)) {
+    return SRSLTE_ERROR; 
+  }
+
+  do {
+    srslte_tdec_simd_iteration(h, input, nof_cb, long_cb);
+  } while (h->n_iter < nof_iterations);
+
+  srslte_tdec_simd_decision_byte(h, output, nof_cb, long_cb);
+  
+  return SRSLTE_SUCCESS;
+}
+
+#endif
+
+
diff --git a/lib/src/phy/fec/turbodecoder_sse.c b/lib/src/phy/fec/turbodecoder_sse.c
index 91d96c287..4e92efcc0 100644
--- a/lib/src/phy/fec/turbodecoder_sse.c
+++ b/lib/src/phy/fec/turbodecoder_sse.c
@@ -52,6 +52,17 @@
 
 #ifdef LV_HAVE_SSE
 
+/*
+static void print_128i(__m128i x) {
+  int16_t *s = (int16_t*) &x; 
+  printf("[%d", s[0]);
+  for (int i=1;i<8;i++) {
+    printf(",%d", s[i]);
+  }
+  printf("]\n");
+}
+*/
+
 /* Computes the horizontal MAX from 8 16-bit integers using the minpos_epu16 SSE4.1 instruction */
 static inline int16_t hMax(__m128i buffer)
 {
@@ -126,7 +137,8 @@ void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
     alpha_k = _mm_load_si128(alphaPtr);\
     alphaPtr--;\
     bp = _mm_add_epi16(bp, alpha_k);\
-    bn = _mm_add_epi16(bn, alpha_k); output[k-d] = hMax(bn) - hMax(bp);
+    bn = _mm_add_epi16(bn, alpha_k);\
+    output[k-d] = hMax(bn) - hMax(bp);
 
   /* The tail does not require to load alpha or produce outputs. Only update 
    * beta metrics accordingly */
@@ -134,7 +146,6 @@ void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
     int16_t g0 = s->branch[2*k];
     int16_t g1 = s->branch[2*k+1];
     g = _mm_set_epi16(g1, g0, g0, g1, g1, g0, g0, g1);
-  
     BETA_STEP(g);
   }  
   

From f00ea8c8ed89f9fee067406fca3f8711145ac7df Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 19 Jun 2017 17:21:46 +0200
Subject: [PATCH 003/269] Integrated AVX2 decoder in PDSCH object. Added
 inter-frame SSE decoder (not working and not integrated)

---
 lib/include/srslte/phy/common/phy_common.h    |   2 +
 lib/include/srslte/phy/fec/turbodecoder.h     |  44 +-
 lib/include/srslte/phy/fec/turbodecoder_gen.h |   2 +
 .../srslte/phy/fec/turbodecoder_simd_inter.h  | 119 ++++
 lib/src/phy/fec/test/turbodecoder_test.c      |  13 +-
 lib/src/phy/fec/turbodecoder.c                |  35 +-
 lib/src/phy/fec/turbodecoder_avx.c            | 124 ++++-
 lib/src/phy/fec/turbodecoder_simd.c           |  72 ++-
 lib/src/phy/fec/turbodecoder_simd_inter.c     | 299 ++++++++++
 lib/src/phy/fec/turbodecoder_sse.c            | 514 ++++++------------
 lib/src/phy/fec/turbodecoder_sse_inter.c      | 198 +++++++
 lib/src/phy/phch/sch.c                        | 247 +++++----
 12 files changed, 1143 insertions(+), 526 deletions(-)
 create mode 100644 lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
 create mode 100644 lib/src/phy/fec/turbodecoder_simd_inter.c
 create mode 100644 lib/src/phy/fec/turbodecoder_sse_inter.c

diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index bfb2a993a..efd498c86 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -53,6 +53,8 @@
 #define SRSLTE_MAX_LAYERS    4
 #define SRSLTE_MAX_CODEWORDS 2
 
+#define SRSLTE_MAX_CODEBLOCKS 32
+
 #define SRSLTE_LTE_CRC24A  0x1864CFB
 #define SRSLTE_LTE_CRC24B  0X1800063
 #define SRSLTE_LTE_CRC16   0x11021
diff --git a/lib/include/srslte/phy/fec/turbodecoder.h b/lib/include/srslte/phy/fec/turbodecoder.h
index 149841fad..24e38d09e 100644
--- a/lib/include/srslte/phy/fec/turbodecoder.h
+++ b/lib/include/srslte/phy/fec/turbodecoder.h
@@ -52,12 +52,14 @@
 #include "srslte/phy/fec/turbodecoder_gen.h"
 
 #ifdef LV_HAVE_SSE
-#include "srslte/phy/fec/turbodecoder_sse.h"
+#include "srslte/phy/fec/turbodecoder_simd.h"
+#else
+#define SRSLTE_TDEC_NPAR 1
 #endif
 
 typedef struct SRSLTE_API {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_sse_t tdec_sse;
+  srslte_tdec_simd_t tdec_simd;
 #else
   float *input_conv; 
   srslte_tdec_gen_t tdec_gen;
@@ -69,7 +71,16 @@ SRSLTE_API int srslte_tdec_init(srslte_tdec_t * h,
 
 SRSLTE_API void srslte_tdec_free(srslte_tdec_t * h);
 
-SRSLTE_API int srslte_tdec_reset(srslte_tdec_t * h, uint32_t long_cb);
+SRSLTE_API int srslte_tdec_reset(srslte_tdec_t * h, 
+                                 uint32_t long_cb);
+
+SRSLTE_API int srslte_tdec_reset_cb(srslte_tdec_t * h, 
+                                    uint32_t cb_idx);
+
+SRSLTE_API int srslte_tdec_get_nof_iterations_cb(srslte_tdec_t * h, 
+                                                 uint32_t cb_idx);
+
+SRSLTE_API int srslte_tdec_get_nof_parallel(srslte_tdec_t * h); 
 
 SRSLTE_API void srslte_tdec_iteration(srslte_tdec_t * h, 
                                       int16_t* input, 
@@ -89,4 +100,31 @@ SRSLTE_API int srslte_tdec_run_all(srslte_tdec_t * h,
                                    uint32_t nof_iterations, 
                                    uint32_t long_cb);
 
+SRSLTE_API void srslte_tdec_iteration_par(srslte_tdec_t * h, 
+                                          int16_t* input[SRSLTE_TDEC_NPAR], 
+                                          uint32_t nof_cb,
+                                          uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_decision_par(srslte_tdec_t * h, 
+                                         uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                         uint32_t nof_cb,
+                                         uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_decision_byte_par(srslte_tdec_t * h, 
+                                              uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                              uint32_t nof_cb,
+                                              uint32_t long_cb); 
+
+SRSLTE_API void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, 
+                                                 uint8_t *output, 
+                                                 uint32_t cb_idx, 
+                                                 uint32_t long_cb);
+
+SRSLTE_API int srslte_tdec_run_all_par(srslte_tdec_t * h, 
+                                       int16_t * input[SRSLTE_TDEC_NPAR], 
+                                       uint8_t *output[SRSLTE_TDEC_NPAR],
+                                       uint32_t nof_iterations, 
+                                       uint32_t nof_cb,
+                                       uint32_t long_cb);
+
 #endif
diff --git a/lib/include/srslte/phy/fec/turbodecoder_gen.h b/lib/include/srslte/phy/fec/turbodecoder_gen.h
index 7f219201a..4a023396c 100644
--- a/lib/include/srslte/phy/fec/turbodecoder_gen.h
+++ b/lib/include/srslte/phy/fec/turbodecoder_gen.h
@@ -66,6 +66,8 @@ typedef struct SRSLTE_API {
   float *parity;
 
   int current_cbidx; 
+  uint32_t current_cb_len;
+  uint32_t n_iter; 
   srslte_tc_interl_t interleaver[SRSLTE_NOF_TC_CB_SIZES];
 } srslte_tdec_gen_t;
 
diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h b/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
new file mode 100644
index 000000000..402faa314
--- /dev/null
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
@@ -0,0 +1,119 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/**********************************************************************************************
+ *  File:         turbodecoder.h
+ *
+ *  Description:  Turbo Decoder.
+ *                Parallel Concatenated Convolutional Code (PCCC) with two 8-state constituent
+ *                encoders and one turbo code internal interleaver. The coding rate of turbo
+ *                encoder is 1/3.
+ *                MAP_GEN is the MAX-LOG-MAP generic implementation of the decoder.
+ *
+ *  Reference:    3GPP TS 36.212 version 10.0.0 Release 10 Sec. 5.1.3.2
+ *********************************************************************************************/
+
+#ifndef TURBODECODER_SSE_INTER_
+#define TURBODECODER_SSE_INTER_
+
+
+/** This is an simd inter-frame parallel turbo decoder. Parallizes 8 code-blocks using SSE
+ *  This implementation is currently not functional and not used by the rest of the code 
+ */
+
+#include "srslte/config.h"
+#include "srslte/phy/fec/tc_interl.h"
+#include "srslte/phy/fec/cbsegm.h"
+
+#if LV_HAVE_AVX2
+  #define SRSLTE_TDEC_NPAR 16
+#else
+  #define SRSLTE_TDEC_NPAR 8
+#endif
+
+
+typedef struct SRSLTE_API {
+  int max_long_cb;
+
+  int16_t *syst0;
+  int16_t *parity0;
+  int16_t *syst1;
+  int16_t *parity1;
+  int16_t *llr1;
+  int16_t *llr2;
+  int16_t *w;
+  int16_t *alpha;
+
+  uint32_t max_par_cb;   
+  int current_cbidx; 
+  uint32_t current_long_cb;
+  srslte_tc_interl_t interleaver[SRSLTE_NOF_TC_CB_SIZES];
+  int n_iter[SRSLTE_TDEC_NPAR];
+} srslte_tdec_simd_inter_t;
+
+SRSLTE_API int srslte_tdec_simd_inter_init(srslte_tdec_simd_inter_t * h, 
+                                           uint32_t max_par_cb, 
+                                           uint32_t max_long_cb);
+
+SRSLTE_API void srslte_tdec_simd_inter_free(srslte_tdec_simd_inter_t * h);
+
+SRSLTE_API int srslte_tdec_simd_inter_reset(srslte_tdec_simd_inter_t * h, 
+                                      uint32_t long_cb);
+
+SRSLTE_API int srslte_tdec_simd_inter_get_nof_iterations_cb(srslte_tdec_simd_inter_t * h, 
+                                                      uint32_t cb_idx);
+
+SRSLTE_API int srslte_tdec_simd_inter_reset_cb(srslte_tdec_simd_inter_t * h, 
+                                         uint32_t cb_idx);
+
+SRSLTE_API void srslte_tdec_simd_inter_iteration(srslte_tdec_simd_inter_t * h, 
+                                           int16_t * input[SRSLTE_TDEC_NPAR], 
+                                           uint32_t nof_cb,
+                                           uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_simd_inter_decision(srslte_tdec_simd_inter_t * h, 
+                                          uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                          uint32_t nof_cb,
+                                          uint32_t long_cb);
+
+SRSLTE_API void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, 
+                                               uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                               uint32_t nof_cb,
+                                               uint32_t long_cb); 
+
+SRSLTE_API void srslte_tdec_simd_inter_decision_byte_cb(srslte_tdec_simd_inter_t * h, 
+                                                  uint8_t *output, 
+                                                  uint32_t cbidx, 
+                                                  uint32_t long_cb); 
+
+SRSLTE_API int srslte_tdec_simd_inter_run_all(srslte_tdec_simd_inter_t * h, 
+                                        int16_t *input[SRSLTE_TDEC_NPAR], 
+                                        uint8_t *output[SRSLTE_TDEC_NPAR],
+                                        uint32_t nof_iterations, 
+                                        uint32_t nof_cb,
+                                        uint32_t long_cb);
+
+#endif
diff --git a/lib/src/phy/fec/test/turbodecoder_test.c b/lib/src/phy/fec/test/turbodecoder_test.c
index dad9a07c8..839bc4202 100644
--- a/lib/src/phy/fec/test/turbodecoder_test.c
+++ b/lib/src/phy/fec/test/turbodecoder_test.c
@@ -257,13 +257,10 @@ int main(int argc, char **argv) {
       int16_t *input[SRSLTE_TDEC_NPAR];
       uint8_t *output[SRSLTE_TDEC_NPAR];        
       
-      input[0] = llr_s; 
-      if (SRSLTE_TDEC_NPAR == 2)
-        input[1] = llr_s; 
-      
-      output[0] = data_rx_bytes[0]; 
-      if (SRSLTE_TDEC_NPAR == 2)
-        output[1] = data_rx_bytes[1]; 
+      for (int n=0;n<SRSLTE_TDEC_NPAR;n++) {
+        input[n] = llr_s;         
+        output[n] = data_rx_bytes[n]; 
+      }
 
       gettimeofday(&tdata[1], NULL); 
       for (int k=0;k<nof_repetitions;k++) { 
@@ -284,7 +281,7 @@ int main(int argc, char **argv) {
       }
       printf("Eb/No: %2.2f %10d/%d   ", SNR_MIN + i * ebno_inc, frame_cnt, nof_frames);
       printf("BER: %.2e  ", (float) errors / (nof_cb*frame_cnt * frame_length));
-      printf("%3.1f Mbps (%6.2f usec)", (float) SRSLTE_TDEC_NPAR*frame_length / mean_usec, mean_usec);
+      printf("%3.1f Mbps (%6.2f usec)", (float) (nof_cb*frame_length) / mean_usec, mean_usec);
       printf("\r");        
 
     }    
diff --git a/lib/src/phy/fec/turbodecoder.c b/lib/src/phy/fec/turbodecoder.c
index 17cb52957..3eb71bb29 100644
--- a/lib/src/phy/fec/turbodecoder.c
+++ b/lib/src/phy/fec/turbodecoder.c
@@ -43,7 +43,7 @@
 
 int srslte_tdec_init(srslte_tdec_t * h, uint32_t max_long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_simd_init(&h->tdec_simd, max_long_cb);
+  return srslte_tdec_simd_init(&h->tdec_simd, SRSLTE_TDEC_NPAR, max_long_cb);
 #else
   h->input_conv = srslte_vec_malloc(sizeof(float) * (3*max_long_cb+12));
   if (!h->input_conv) {
@@ -56,7 +56,7 @@ int srslte_tdec_init(srslte_tdec_t * h, uint32_t max_long_cb) {
 
 void srslte_tdec_free(srslte_tdec_t * h) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_simd_free(&h->tdec_simd);
+  srslte_tdec_simd_free(&h->tdec_simd);  
 #else
   if (h->input_conv) {
     free(h->input_conv);
@@ -74,9 +74,26 @@ int srslte_tdec_reset(srslte_tdec_t * h, uint32_t long_cb) {
 #endif
 }
 
+int srslte_tdec_reset_cb(srslte_tdec_t * h, uint32_t cb_idx) {
+#ifdef LV_HAVE_SSE
+  return srslte_tdec_simd_reset_cb(&h->tdec_simd, cb_idx);      
+#else
+  return srslte_tdec_gen_reset(&h->tdec_gen, h->tdec_gen.current_cb_len);
+#endif
+}
+
+int srslte_tdec_get_nof_iterations_cb(srslte_tdec_t * h, uint32_t cb_idx)
+{
+#ifdef LV_HAVE_SSE
+  return srslte_tdec_simd_get_nof_iterations_cb(&h->tdec_simd, cb_idx);
+#else
+  return h->tdec_gen.n_iter;
+#endif  
+}
+
 void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_simd_iteration(&h->tdec_simd, input, nof_cb, long_cb);
+  srslte_tdec_simd_iteration(&h->tdec_simd, input, nof_cb, long_cb);      
 #else
   srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
   srslte_tdec_gen_iteration(&h->tdec_gen, h->input_conv, long_cb);
@@ -105,12 +122,20 @@ void srslte_tdec_decision(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb)
 
 void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_simd_decision_byte(&h->tdec_simd, output, nof_cb, long_cb);
+  srslte_tdec_simd_decision_byte(&h->tdec_simd, output, nof_cb, long_cb);  
 #else
   srslte_tdec_gen_decision_byte(&h->tdec_gen, output[0], long_cb);
 #endif  
 }
 
+void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, uint8_t *output, uint32_t cb_idx, uint32_t long_cb) {
+#ifdef LV_HAVE_SSE
+  srslte_tdec_simd_decision_byte_cb(&h->tdec_simd, output, cb_idx, long_cb);
+#else
+  srslte_tdec_gen_decision_byte(&h->tdec_gen, output, long_cb);
+#endif  
+}
+
 void srslte_tdec_decision_byte(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
   uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
   output_par[0] = output; 
@@ -121,7 +146,7 @@ int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_NPAR]
                             uint8_t *output[SRSLTE_TDEC_NPAR], 
                             uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, nof_cb, long_cb);
+  return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, nof_cb, long_cb);  
 #else
   srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
   return srslte_tdec_gen_run_all(&h->tdec_gen, h->input_conv, output[0], nof_iterations, long_cb);
diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
index 82cceb5d2..a98279d25 100644
--- a/lib/src/phy/fec/turbodecoder_avx.c
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -153,8 +153,6 @@ void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t lo
   __m256i gv;
   int16_t *b = &s->branch[2*NCB*long_cb-16];
   __m256i *gPtr = (__m256i*) b;
-
-  __m256i bn2, bp2; 
   
   /* This defines a beta computation step: 
    * Adds and substracts the branch metrics to the previous beta step, 
@@ -175,10 +173,10 @@ void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t lo
     alphaPtr--;\
     bp = _mm256_add_epi16(bp, alpha_k);\
     bn = _mm256_add_epi16(bn, alpha_k);\
-    bn2 = _mm256_sub_epi8(_mm256_set1_epi16(0x7FFF), bn);\
-    bp2 = _mm256_sub_epi8(_mm256_set1_epi16(0x7FFF), bp);\
-    output[0][k-d] = hMax0(bn2) - hMax0(bp2);\
-    output[1][k-d] = hMax1(bn2) - hMax1(bp2);
+    bn = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bn);\
+    bp = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bp);\
+    output[0][k-d] = hMax0(bn) - hMax0(bp);\
+    output[1][k-d] = hMax1(bn) - hMax1(bp);
 
   /* The tail does not require to load alpha or produce outputs. Only update 
    * beta metrics accordingly */
@@ -309,7 +307,7 @@ void map_avx_alpha(map_gen_t * s, uint32_t long_cb)
   an = _mm256_shuffle_epi8(an, shuf_an);\
   alpha_k = _mm256_max_epi16(ap, an);\
   _mm256_store_si256(alphaPtr, alpha_k);\
-  alphaPtr++;    \
+  alphaPtr++;\
 
 
   /* In this loop, we compute 8 steps and normalize twice for each branch metrics memory load */
@@ -335,15 +333,62 @@ void map_avx_alpha(map_gen_t * s, uint32_t long_cb)
   }  
 }
 
-/* Compute branch metrics (gamma) */
-void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t long_cb) 
+void map_sse_gamma_single(int16_t *output, int16_t *input, int16_t *app, int16_t *parity) 
 {
-  __m128i res10, res20, res11, res21, res1, res2; 
+  __m128i res00, res10, res01, res11, res0, res1; 
   __m128i in, ap, pa, g1, g0;
 
   __m128i *inPtr  = (__m128i*) input;
   __m128i *appPtr = (__m128i*) app;
   __m128i *paPtr  = (__m128i*) parity;
+  __m128i *resPtr = (__m128i*) output;
+  
+  __m128i res00_mask = _mm_set_epi8(0xff,0xff,7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0);
+  __m128i res10_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
+  __m128i res01_mask = _mm_set_epi8(7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0,0xff,0xff);
+  __m128i res11_mask = _mm_set_epi8(15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8,0xff,0xff);
+  
+  in = _mm_load_si128(inPtr);
+  inPtr++;
+  pa = _mm_load_si128(paPtr);
+  paPtr++;
+  
+  if (appPtr) {
+    ap = _mm_load_si128(appPtr);
+    appPtr++;
+    in = _mm_add_epi16(ap, in);
+  }
+  
+  g1 = _mm_add_epi16(in, pa);
+  g0 = _mm_sub_epi16(in, pa);
+
+  g1 = _mm_srai_epi16(g1, 1);
+  g0 = _mm_srai_epi16(g0, 1);
+  
+  res00 = _mm_shuffle_epi8(g0, res00_mask);
+  res10 = _mm_shuffle_epi8(g0, res10_mask);
+  res01 = _mm_shuffle_epi8(g1, res01_mask);
+  res11 = _mm_shuffle_epi8(g1, res11_mask);
+
+  res0  = _mm_or_si128(res00, res01);
+  res1  = _mm_or_si128(res10, res11);
+
+  _mm_store_si128(resPtr, res0);
+  resPtr++;
+  _mm_store_si128(resPtr, res1);    
+  resPtr++;
+}
+
+
+/* Compute branch metrics (gamma) */
+void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t long_cb) 
+{
+  __m128i res10, res20, res11, res21, res1, res2; 
+  __m256i in, ap, pa, g1, g0;
+
+  __m256i *inPtr  = (__m256i*) input;
+  __m256i *appPtr = (__m256i*) app;
+  __m256i *paPtr  = (__m256i*) parity;
   __m128i *resPtr = (__m128i*) h->branch;
   
   if (cbidx) {
@@ -351,32 +396,56 @@ void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
   }
   
   __m128i res10_mask = _mm_set_epi8(0xff,0xff,7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0);
-  __m128i res20_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
   __m128i res11_mask = _mm_set_epi8(7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0,0xff,0xff);
+
+  __m128i res20_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
   __m128i res21_mask = _mm_set_epi8(15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8,0xff,0xff);
 
-  for (int i=0;i<long_cb/8;i++) {
-    in = _mm_load_si128(inPtr);
+  for (int i=0;i<long_cb/16;i++) {
+    in = _mm256_load_si256(inPtr);
     inPtr++;
-    pa = _mm_load_si128(paPtr);
+    pa = _mm256_load_si256(paPtr);
     paPtr++;
     
     if (appPtr) {
-      ap = _mm_load_si128(appPtr);
+      ap = _mm256_load_si256(appPtr);
       appPtr++;
-      in = _mm_add_epi16(ap, in);
+      in = _mm256_add_epi16(ap, in);
     }
     
-    g1 = _mm_add_epi16(in, pa);
-    g0 = _mm_sub_epi16(in, pa);
+    g0 = _mm256_sub_epi16(in, pa);
+    g1 = _mm256_add_epi16(in, pa);
+ 
+    g0 = _mm256_srai_epi16(g0, 1);
+    g1 = _mm256_srai_epi16(g1, 1);
+    
+    __m128i g0_t = _mm256_extractf128_si256(g0, 0);
+    __m128i g1_t = _mm256_extractf128_si256(g1, 0);
+    
+    res10 = _mm_shuffle_epi8(g0_t, res10_mask);
+    res11 = _mm_shuffle_epi8(g1_t, res11_mask);
 
-    g1 = _mm_srai_epi16(g1, 1);
-    g0 = _mm_srai_epi16(g0, 1);
+    res20 = _mm_shuffle_epi8(g0_t, res20_mask);
+    res21 = _mm_shuffle_epi8(g1_t, res21_mask);
+
+    res1  = _mm_or_si128(res10, res11);
+    res2  = _mm_or_si128(res20, res21);
+
+    _mm_store_si128(resPtr, res1);
+    resPtr++;
+    resPtr++;
+    _mm_store_si128(resPtr, res2);    
+    resPtr++;
+    resPtr++;          
+    
+    g0_t = _mm256_extractf128_si256(g0, 1);
+    g1_t = _mm256_extractf128_si256(g1, 1);
     
-    res10 = _mm_shuffle_epi8(g0, res10_mask);
-    res20 = _mm_shuffle_epi8(g0, res20_mask);
-    res11 = _mm_shuffle_epi8(g1, res11_mask);
-    res21 = _mm_shuffle_epi8(g1, res21_mask);
+    res10 = _mm_shuffle_epi8(g0_t, res10_mask);
+    res11 = _mm_shuffle_epi8(g1_t, res11_mask);
+
+    res20 = _mm_shuffle_epi8(g0_t, res20_mask);
+    res21 = _mm_shuffle_epi8(g1_t, res21_mask);
 
     res1  = _mm_or_si128(res10, res11);
     res2  = _mm_or_si128(res20, res21);
@@ -386,7 +455,12 @@ void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
     resPtr++;
     _mm_store_si128(resPtr, res2);    
     resPtr++;
-    resPtr++;    
+    resPtr++;          
+
+  }
+  
+  if (long_cb%16) {
+    map_sse_gamma_single((int16_t*) resPtr, (int16_t*) appPtr, (int16_t*) inPtr, (int16_t*) paPtr);
   }
     
   for (int i=long_cb;i<long_cb+3;i++) {
diff --git a/lib/src/phy/fec/turbodecoder_simd.c b/lib/src/phy/fec/turbodecoder_simd.c
index a2cddd582..7a36c79fa 100644
--- a/lib/src/phy/fec/turbodecoder_simd.c
+++ b/lib/src/phy/fec/turbodecoder_simd.c
@@ -96,20 +96,23 @@ void map_simd_gamma(map_gen_t * s, int16_t *input, int16_t *app, int16_t *parity
 }
 
 /* Inititalizes constituent decoder object */
-int map_simd_init(map_gen_t * h, int max_long_cb)
+int map_simd_init(map_gen_t * h, uint32_t max_par_cb, uint32_t max_long_cb)
 {
   bzero(h, sizeof(map_gen_t));
-  h->alpha = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * SRSLTE_TDEC_NPAR);
+
+  h->max_par_cb  = max_par_cb;
+  h->max_long_cb = max_long_cb;
+
+  h->alpha = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * h->max_par_cb);
   if (!h->alpha) {
     perror("srslte_vec_malloc");
     return -1;
   }
-  h->branch = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * SRSLTE_TDEC_NPAR);
+  h->branch = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES * h->max_par_cb);
   if (!h->branch) {
     perror("srslte_vec_malloc");
     return -1;
   }
-  h->max_long_cb = max_long_cb;
   return 0;
 }
 
@@ -142,15 +145,16 @@ void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_NPAR], int16_t *app
 }
 
 /* Initializes the turbo decoder object */
-int srslte_tdec_simd_init(srslte_tdec_simd_t * h, uint32_t max_long_cb)
+int srslte_tdec_simd_init(srslte_tdec_simd_t * h, uint32_t max_par_cb, uint32_t max_long_cb)
 {
   int ret = -1;
   bzero(h, sizeof(srslte_tdec_simd_t));
   uint32_t len = max_long_cb + SRSLTE_TCOD_TOTALTAIL;
 
   h->max_long_cb = max_long_cb;
-
-  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+  h->max_par_cb  = max_par_cb; 
+  
+  for (int i=0;i<h->max_par_cb;i++) {
     h->app1[i] = srslte_vec_malloc(sizeof(int16_t) * len);
     if (!h->app1[i]) {
       perror("srslte_vec_malloc");
@@ -189,7 +193,7 @@ int srslte_tdec_simd_init(srslte_tdec_simd_t * h, uint32_t max_long_cb)
     
   }
 
-  if (map_simd_init(&h->dec, h->max_long_cb)) {
+  if (map_simd_init(&h->dec, h->max_par_cb, h->max_long_cb)) {
     goto clean_and_exit;
   }
 
@@ -209,7 +213,7 @@ clean_and_exit:if (ret == -1) {
 
 void srslte_tdec_simd_free(srslte_tdec_simd_t * h)
 {
-  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+  for (int i=0;i<h->max_par_cb;i++) {
     if (h->app1[i]) {
       free(h->app1[i]);
     }
@@ -333,33 +337,34 @@ void deinterleave_input_simd(srslte_tdec_simd_t *h, int16_t *input, uint32_t cbi
 void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
 
+  int16_t *tmp_app[SRSLTE_TDEC_NPAR];
+
   if (h->current_cbidx >= 0) {
     uint16_t *inter   = h->interleaver[h->current_cbidx].forward;
     uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
     
-    if (h->n_iter == 0) {
-      for (int i=0;i<nof_cb;i++) {
+    for (int i=0;i<nof_cb;i++) {
+      if (h->n_iter[i] == 0) {
         deinterleave_input_simd(h, input[i], i, long_cb);        
       }        
     }
     
     // Add apriori information to decoder 1 
-    if (h->n_iter > 0) {
-      for (int i=0;i<nof_cb;i++) {
+    for (int i=0;i<nof_cb;i++) {
+      if (h->n_iter[i] > 0) {
         srslte_vec_sub_sss(h->app1[i], h->ext1[i], h->app1[i], long_cb);
       }
     }
 
     // Run MAP DEC #1
-    if (h->n_iter == 0) {
-      map_simd_dec(&h->dec, h->syst, NULL, h->parity0, h->ext1, nof_cb, long_cb);            
-    } else {
-      map_simd_dec(&h->dec, h->syst, h->app1, h->parity0, h->ext1, nof_cb, long_cb);      
+    for (int i=0;i<h->max_par_cb;i++) {
+      tmp_app[i] = h->n_iter[i]?h->app1[i]:NULL;
     }
+    map_simd_dec(&h->dec, h->syst, tmp_app, h->parity0, h->ext1, nof_cb, long_cb);            
     
     // Convert aposteriori information into extrinsic information    
-    if (h->n_iter > 0) {
-      for (int i=0;i<nof_cb;i++) {
+    for (int i=0;i<nof_cb;i++) {
+      if (h->n_iter[i] > 0) {
         srslte_vec_sub_sss(h->ext1[i], h->app1[i], h->ext1[i], long_cb);
       }
     }
@@ -377,7 +382,9 @@ void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_T
       srslte_vec_lut_sss(h->ext2[i], inter, h->app1[i], long_cb);
     }
 
-    h->n_iter++;
+    for (int i=0;i<h->max_par_cb;i++) {
+      h->n_iter[i]++;     
+    }
   } else {
     fprintf(stderr, "Error CB index not set (call srslte_tdec_simd_reset() first\n");    
   }
@@ -391,7 +398,9 @@ int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, uint32_t long_cb)
             h->max_long_cb);
     return -1;
   }
-  h->n_iter = 0; 
+  for (int i=0;i<h->max_par_cb;i++) {
+    h->n_iter[i] = 0;     
+  }
   h->current_cbidx = srslte_cbsegm_cbindex(long_cb);
   if (h->current_cbidx < 0) {
     fprintf(stderr, "Invalid CB length %d\n", long_cb);
@@ -400,6 +409,17 @@ int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, uint32_t long_cb)
   return 0;
 }
 
+int srslte_tdec_simd_reset_cb(srslte_tdec_simd_t * h, uint32_t cb_idx)
+{
+  h->n_iter[cb_idx] = 0; 
+  return 0;
+}
+
+int srslte_tdec_simd_get_nof_iterations_cb(srslte_tdec_simd_t * h, uint32_t cb_idx)
+{
+  return h->n_iter[cb_idx];
+}
+
 void tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx, uint32_t long_cb)
 {
   __m128i zero     = _mm_set1_epi16(0);
@@ -433,7 +453,7 @@ void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TD
   }
 }
 
-void tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx, uint32_t long_cb)
+void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx, uint32_t long_cb)
 {
   uint8_t mask[8] = {0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1};
   
@@ -449,17 +469,13 @@ void tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output, uint32_t c
     uint8_t out7 = h->app1[cbidx][8*i+7]>0?mask[7]:0;
     
     output[i] = out0 | out1 | out2 | out3 | out4 | out5 | out6 | out7;
-    
-    //if (i<10) {
-    //  printf("output[%d]=%d\n",i,output[i]);
-    //}
   }
 }
 
 void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
   for (int i=0;i<nof_cb;i++) {
-    tdec_simd_decision_byte(h, output[i], i, long_cb);
+    srslte_tdec_simd_decision_byte_cb(h, output[i], i, long_cb);
   }
 }
 
@@ -474,7 +490,7 @@ int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC
 
   do {
     srslte_tdec_simd_iteration(h, input, nof_cb, long_cb);
-  } while (h->n_iter < nof_iterations);
+  } while (h->n_iter[0] < nof_iterations);
 
   srslte_tdec_simd_decision_byte(h, output, nof_cb, long_cb);
   
diff --git a/lib/src/phy/fec/turbodecoder_simd_inter.c b/lib/src/phy/fec/turbodecoder_simd_inter.c
new file mode 100644
index 000000000..05d8b2cf5
--- /dev/null
+++ b/lib/src/phy/fec/turbodecoder_simd_inter.c
@@ -0,0 +1,299 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <strings.h>
+#include <math.h>
+
+#include "srslte/phy/fec/turbodecoder_simd_inter.h"
+#include "srslte/phy/utils/vector.h"
+
+#define TOTALTAIL       12
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+
+void map_see_inter_alpha(srslte_tdec_simd_inter_t * s, int16_t *input, int16_t *parity, uint32_t long_cb);
+void map_sse_inter_beta(srslte_tdec_simd_inter_t * s, int16_t *input, int16_t *parity, int16_t * output, uint32_t long_cb);
+void sse_inter_update_w(srslte_tdec_simd_inter_t *h, uint16_t *deinter, uint32_t long_cb); 
+void sse_inter_extract_syst1(srslte_tdec_simd_inter_t *h, uint16_t *inter, uint32_t long_cb);
+
+
+static void map_sse_inter_dec(srslte_tdec_simd_inter_t * h, int16_t * input, int16_t * parity, int16_t * output,
+                 uint32_t long_cb)
+{
+  map_see_inter_alpha(h, input, parity, long_cb);
+  map_sse_inter_beta(h, input, parity, output, long_cb);
+}
+
+/************************************************
+ *
+ *  TURBO DECODER INTERFACE
+ *
+ ************************************************/
+int srslte_tdec_simd_inter_init(srslte_tdec_simd_inter_t * h, uint32_t max_par_cb, uint32_t max_long_cb)
+{
+  int ret = -1;
+  bzero(h, sizeof(srslte_tdec_simd_inter_t));
+  uint32_t len = max_long_cb + 12;
+
+  h->max_long_cb = max_long_cb;
+  h->max_par_cb  = max_par_cb;
+
+  h->llr1 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->llr1) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->llr2 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->llr2) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->w = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->w) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->syst0 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->syst0) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->syst1 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->syst1) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->parity0 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->parity0) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->parity1 = srslte_vec_malloc(sizeof(int16_t) * len * h->max_par_cb);
+  if (!h->parity1) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+  h->alpha = srslte_vec_malloc(sizeof(int16_t) * 8*(len+12) * h->max_par_cb);
+  if (!h->alpha) {
+    perror("srslte_vec_malloc");
+    goto clean_and_exit;
+  }
+
+  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
+    if (srslte_tc_interl_init(&h->interleaver[i], srslte_cbsegm_cbsize(i)) < 0) {
+      goto clean_and_exit;
+    }
+    srslte_tc_interl_LTE_gen(&h->interleaver[i], srslte_cbsegm_cbsize(i));
+  }
+  h->current_cbidx = -1; 
+  ret = 0;
+clean_and_exit:if (ret == -1) {
+    srslte_tdec_simd_inter_free(h);
+  }
+  return ret;
+}
+
+void srslte_tdec_simd_inter_free(srslte_tdec_simd_inter_t * h)
+{
+  if (h->llr1) {
+    free(h->llr1);
+  }
+  if (h->llr2) {
+    free(h->llr2);
+  }
+  if (h->w) {
+    free(h->w);
+  }
+  if (h->syst0) {
+    free(h->syst0);
+  }
+  if (h->syst1) {
+    free(h->syst1);
+  }
+  if (h->parity0) {
+    free(h->parity0);
+  }
+  if (h->parity1) {
+    free(h->parity1);
+  }
+  if (h->alpha) {
+    free(h->alpha);
+  }
+
+  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
+    srslte_tc_interl_free(&h->interleaver[i]);    
+  }
+
+  bzero(h, sizeof(srslte_tdec_simd_inter_t));
+}
+
+
+/* Deinterleave for inter-frame parallelization */
+void extract_input(srslte_tdec_simd_inter_t *h, int16_t *input, uint32_t cbidx, uint32_t long_cb) 
+{  
+  for (int i=0;i<long_cb;i++) {
+    h->syst0[h->max_par_cb*i+cbidx]    = input[3*i+0];
+    h->parity0[h->max_par_cb*i+cbidx] = input[3*i+1];
+    h->parity1[h->max_par_cb*i+cbidx] = input[3*i+2];
+  }
+  for (int i = long_cb; i < long_cb + 3; i++) {
+    h->syst0[h->max_par_cb*i+cbidx] = input[3*long_cb + 2*(i - long_cb)];
+    h->syst1[h->max_par_cb*i+cbidx] = input[3*long_cb + 2*(i - long_cb)];
+    h->parity0[h->max_par_cb*i+cbidx] = input[3*long_cb + 2*(i - long_cb) + 1];
+    h->parity0[h->max_par_cb*i+cbidx] = input[3*long_cb + 2*(i - long_cb) + 2];
+  }
+}
+
+void srslte_tdec_simd_inter_iteration(srslte_tdec_simd_inter_t * h, int16_t *input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+
+  if (h->current_cbidx >= 0) {
+
+    uint16_t *inter = h->interleaver[h->current_cbidx].forward;
+    uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
+    
+    // Prepare systematic and parity bits for MAP DEC #1
+    for (int i=0;i<nof_cb;i++) {
+      if (h->n_iter[i] == 0) {
+        extract_input(h, input[i], i, long_cb);
+      }
+      srslte_vec_sum_sss(h->syst0, h->w, h->syst0, long_cb*h->max_par_cb);      
+    }
+    
+    // Run MAP DEC #1
+    map_sse_inter_dec(h, h->syst0, h->parity0, h->llr1, long_cb);
+
+    // Prepare systematic and parity bits for MAP DEC #1
+    sse_inter_extract_syst1(h, inter, long_cb);
+
+    // Run MAP DEC #2
+    map_sse_inter_dec(h, h->syst1, h->parity1, h->llr2, long_cb);
+    
+    // Update a-priori LLR from the last iteration
+    sse_inter_update_w(h, deinter, long_cb);
+    
+  } else {
+    fprintf(stderr, "Error CB index not set (call srslte_tdec_simd_inter_reset() first\n");    
+  }
+}
+
+int srslte_tdec_simd_inter_reset_cb(srslte_tdec_simd_inter_t * h, uint32_t cb_idx)
+{
+  for (int i=0;i<h->current_long_cb;i++) {
+    h->w[h->max_par_cb*i+cb_idx] = 0; 
+  }
+  return 0;
+}
+
+int srslte_tdec_simd_inter_reset(srslte_tdec_simd_inter_t * h, uint32_t long_cb)
+{
+  if (long_cb > h->max_long_cb) {
+    fprintf(stderr, "TDEC was initialized for max_long_cb=%d\n",
+            h->max_long_cb);
+    return -1;
+  }
+  h->current_long_cb = long_cb; 
+  h->current_cbidx   = srslte_cbsegm_cbindex(long_cb);
+  if (h->current_cbidx < 0) {
+    fprintf(stderr, "Invalid CB length %d\n", long_cb);
+    return -1; 
+  }
+  memset(h->w, 0, sizeof(int16_t) * long_cb * h->max_par_cb);
+  return 0; 
+}
+
+void srslte_tdec_simd_inter_decision_cb(srslte_tdec_simd_inter_t * h, uint8_t *output, uint32_t cb_idx, uint32_t long_cb)
+{
+  uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
+  uint32_t i;
+  for (i = 0; i < long_cb; i++) {
+    output[i] = (h->llr2[h->max_par_cb*deinter[i]+cb_idx] > 0) ? 1 : 0;    
+  }
+}
+
+void srslte_tdec_simd_inter_decision(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+  for (int i=0;i<nof_cb;i++) {
+    srslte_tdec_simd_inter_decision_cb(h, output[i], i, long_cb);
+  }
+}
+
+void srslte_tdec_simd_inter_decision_byte_cb(srslte_tdec_simd_inter_t * h, uint8_t *output, uint32_t cb_idx, uint32_t long_cb)
+{
+  uint32_t i;
+  uint8_t mask[8] = {0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1};
+  uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
+  
+#define indexOf_cb(idx, cb) (h->max_par_cb*(deinter[8*i+idx])+cb)
+  
+  // long_cb is always byte aligned
+  for (i = 0; i < long_cb/8; i++) {
+    uint8_t out0 = h->llr2[indexOf_cb(0, cb_idx)]>0?mask[0]:0;
+    uint8_t out1 = h->llr2[indexOf_cb(1, cb_idx)]>0?mask[1]:0;
+    uint8_t out2 = h->llr2[indexOf_cb(2, cb_idx)]>0?mask[2]:0;
+    uint8_t out3 = h->llr2[indexOf_cb(3, cb_idx)]>0?mask[3]:0;
+    uint8_t out4 = h->llr2[indexOf_cb(4, cb_idx)]>0?mask[4]:0;
+    uint8_t out5 = h->llr2[indexOf_cb(5, cb_idx)]>0?mask[5]:0;
+    uint8_t out6 = h->llr2[indexOf_cb(6, cb_idx)]>0?mask[6]:0;
+    uint8_t out7 = h->llr2[indexOf_cb(7, cb_idx)]>0?mask[7]:0;
+    
+    output[i] = out0 | out1 | out2 | out3 | out4 | out5 | out6 | out7; 
+  }
+}
+
+void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+{
+  for (int i=0;i<nof_cb;i++) {
+    srslte_tdec_simd_inter_decision_byte_cb(h, output[i], i, long_cb);
+  }
+}
+
+int srslte_tdec_simd_inter_run_all(srslte_tdec_simd_inter_t * h, 
+                                  int16_t *input[SRSLTE_TDEC_NPAR], uint8_t *output[SRSLTE_TDEC_NPAR],
+                                  uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb)
+{
+  uint32_t iter = 0;
+
+  if (srslte_tdec_simd_inter_reset(h, long_cb)) {
+    return SRSLTE_ERROR; 
+  }
+
+  do {
+    srslte_tdec_simd_inter_iteration(h, input, nof_cb, long_cb);
+    iter++;
+  } while (iter < nof_iterations);
+
+  srslte_tdec_simd_inter_decision_byte(h, output, nof_cb, long_cb);
+  
+  return SRSLTE_SUCCESS;
+}
+
+#endif
diff --git a/lib/src/phy/fec/turbodecoder_sse.c b/lib/src/phy/fec/turbodecoder_sse.c
index 4e92efcc0..ab4fbd72b 100644
--- a/lib/src/phy/fec/turbodecoder_sse.c
+++ b/lib/src/phy/fec/turbodecoder_sse.c
@@ -31,7 +31,7 @@
 #include <strings.h>
 #include <math.h>
 
-#include "srslte/phy/fec/turbodecoder_sse.h"
+#include "srslte/phy/fec/turbodecoder_simd.h"
 #include "srslte/phy/utils/vector.h"
 
 #include <inttypes.h>
@@ -62,17 +62,20 @@ static void print_128i(__m128i x) {
   printf("]\n");
 }
 */
+//#define use_beta_transposed_max
+
+#ifndef use_beta_transposed_max
 
 /* Computes the horizontal MAX from 8 16-bit integers using the minpos_epu16 SSE4.1 instruction */
 static inline int16_t hMax(__m128i buffer)
 {
-  __m128i tmp1 = _mm_sub_epi8(_mm_set1_epi16(0x7FFF), buffer);
+  __m128i tmp1 = _mm_sub_epi16(_mm_set1_epi16(0x7FFF), buffer);
   __m128i tmp3 = _mm_minpos_epu16(tmp1);
   return (int16_t)(_mm_cvtsi128_si32(tmp3));
 }
 
 /* Computes beta values */
-void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
+void map_sse_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
 {
   int k;
   uint32_t end = long_cb + 3;
@@ -138,8 +141,8 @@ void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
     alphaPtr--;\
     bp = _mm_add_epi16(bp, alpha_k);\
     bn = _mm_add_epi16(bn, alpha_k);\
-    output[k-d] = hMax(bn) - hMax(bp);
-
+    output[k-d] = hMax(bn)-hMax(bp);
+    
   /* The tail does not require to load alpha or produce outputs. Only update 
    * beta metrics accordingly */
   for (k=end-1; k>=long_cb; k--) {
@@ -154,6 +157,7 @@ void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
   for (; k >= 0; k-=8) {    
     gv = _mm_load_si128(gPtr);
     gPtr--;
+
     BETA_STEP_CNT(0,0);
     BETA_STEP_CNT(1,1);
     BETA_STEP_CNT(2,2);
@@ -165,14 +169,17 @@ void map_gen_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
     BETA_STEP_CNT(0,4);
     BETA_STEP_CNT(1,5);
     BETA_STEP_CNT(2,6);
-    BETA_STEP_CNT(3,7);
+    BETA_STEP_CNT(3,7);    
+        
     norm = _mm_shuffle_epi8(beta_k, shuf_norm); 
     beta_k = _mm_sub_epi16(beta_k, norm);
   }  
 }
 
+#endif
+
 /* Computes alpha metrics */
-void map_gen_alpha(map_gen_t * s, uint32_t long_cb)
+void map_sse_alpha(map_gen_t * s, uint32_t long_cb)
 {
   uint32_t k;
   int16_t *alpha = s->alpha;
@@ -261,9 +268,9 @@ void map_gen_alpha(map_gen_t * s, uint32_t long_cb)
 }
 
 /* Compute branch metrics (gamma) */
-void map_gen_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t long_cb) 
+void map_sse_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t long_cb) 
 {
-  __m128i res10, res20, res11, res21, res1, res2; 
+  __m128i res00, res10, res01, res11, res0, res1; 
   __m128i in, ap, pa, g1, g0;
 
   __m128i *inPtr  = (__m128i*) input;
@@ -271,10 +278,10 @@ void map_gen_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
   __m128i *paPtr  = (__m128i*) parity;
   __m128i *resPtr = (__m128i*) h->branch;
   
-  __m128i res10_mask = _mm_set_epi8(0xff,0xff,7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0);
-  __m128i res20_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
-  __m128i res11_mask = _mm_set_epi8(7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0,0xff,0xff);
-  __m128i res21_mask = _mm_set_epi8(15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8,0xff,0xff);
+  __m128i res00_mask = _mm_set_epi8(0xff,0xff,7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0);
+  __m128i res10_mask = _mm_set_epi8(0xff,0xff,15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8);
+  __m128i res01_mask = _mm_set_epi8(7,6,0xff,0xff,5,4,0xff,0xff,3,2,0xff,0xff,1,0,0xff,0xff);
+  __m128i res11_mask = _mm_set_epi8(15,14,0xff,0xff,13,12,0xff,0xff,11,10,0xff,0xff,9,8,0xff,0xff);
   
   for (int i=0;i<long_cb/8;i++) {
     in = _mm_load_si128(inPtr);
@@ -294,17 +301,17 @@ void map_gen_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
     g1 = _mm_srai_epi16(g1, 1);
     g0 = _mm_srai_epi16(g0, 1);
     
+    res00 = _mm_shuffle_epi8(g0, res00_mask);
     res10 = _mm_shuffle_epi8(g0, res10_mask);
-    res20 = _mm_shuffle_epi8(g0, res20_mask);
+    res01 = _mm_shuffle_epi8(g1, res01_mask);
     res11 = _mm_shuffle_epi8(g1, res11_mask);
-    res21 = _mm_shuffle_epi8(g1, res21_mask);
 
+    res0  = _mm_or_si128(res00, res01);
     res1  = _mm_or_si128(res10, res11);
-    res2  = _mm_or_si128(res20, res21);
 
-    _mm_store_si128(resPtr, res1);
+    _mm_store_si128(resPtr, res0);
     resPtr++;
-    _mm_store_si128(resPtr, res2);    
+    _mm_store_si128(resPtr, res1);    
     resPtr++;
   }
 
@@ -314,356 +321,177 @@ void map_gen_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
   }
 }
 
-/* Inititalizes constituent decoder object */
-int map_gen_init(map_gen_t * h, int max_long_cb)
-{
-  bzero(h, sizeof(map_gen_t));
-  h->alpha = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES);
-  if (!h->alpha) {
-    perror("srslte_vec_malloc");
-    return -1;
-  }
-  h->branch = srslte_vec_malloc(sizeof(int16_t) * (max_long_cb + SRSLTE_TCOD_TOTALTAIL + 1) * NUMSTATES);
-  if (!h->branch) {
-    perror("srslte_vec_malloc");
-    return -1;
-  }
-  h->max_long_cb = max_long_cb;
-  return 0;
-}
 
-void map_gen_free(map_gen_t * h)
-{
-  if (h->alpha) {
-    free(h->alpha);
-  }
-  if (h->branch) {
-    free(h->branch);
-  }
-  bzero(h, sizeof(map_gen_t));
-}
 
-/* Runs one instance of a decoder */
-void map_gen_dec(map_gen_t * h, int16_t * input, int16_t *app, int16_t * parity, int16_t * output,
-                 uint32_t long_cb)
-{
- 
-  // Compute branch metrics
-  map_gen_gamma(h, input, app, parity, long_cb);
 
-  // Forward recursion
-  map_gen_alpha(h, long_cb);
 
-  // Backwards recursion + LLR computation
-  map_gen_beta(h, output, long_cb);
-  
-}
-
-/* Initializes the turbo decoder object */
-int srslte_tdec_sse_init(srslte_tdec_sse_t * h, uint32_t max_long_cb)
-{
-  int ret = -1;
-  bzero(h, sizeof(srslte_tdec_sse_t));
-  uint32_t len = max_long_cb + SRSLTE_TCOD_TOTALTAIL;
 
-  h->max_long_cb = max_long_cb;
-
-  h->app1 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->app1) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->app2 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->app2) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->ext1 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->ext1) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->ext2 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->ext2) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->syst = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->syst) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->parity0 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->parity0) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
-  h->parity1 = srslte_vec_malloc(sizeof(int16_t) * len);
-  if (!h->parity1) {
-    perror("srslte_vec_malloc");
-    goto clean_and_exit;
-  }
+/***********************
+ * 
+ * This is an attempt to parallelize the horizontal max 
+ * by doing a 8x8 tranpose of the vectors and computing max 
+ * in cascade. However since we need to store 16 registers 
+ * for the positive and negative values the performance is not very good
+ */
 
-  if (map_gen_init(&h->dec, h->max_long_cb)) {
-    goto clean_and_exit;
-  }
 
-  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
-    if (srslte_tc_interl_init(&h->interleaver[i], srslte_cbsegm_cbsize(i)) < 0) {
-      goto clean_and_exit;
-    }
-    srslte_tc_interl_LTE_gen(&h->interleaver[i], srslte_cbsegm_cbsize(i));
-  }
-  h->current_cbidx = -1; 
-  ret = 0;
-clean_and_exit:if (ret == -1) {
-    srslte_tdec_sse_free(h);
-  }
-  return ret;
-}
+#ifdef use_beta_transposed_max
 
-void srslte_tdec_sse_free(srslte_tdec_sse_t * h)
+static inline __m128i transposed_max(__m128i a, __m128i b, __m128i c, __m128i d, 
+                                     __m128i e, __m128i f, __m128i g, __m128i h)
 {
-  if (h->app1) {
-    free(h->app1);
-  }
-  if (h->app2) {
-    free(h->app2);
-  }
-  if (h->ext1) {
-    free(h->ext1);
-  }
-  if (h->ext2) {
-    free(h->ext2);
-  }
-  if (h->syst) {
-    free(h->syst);
-  }
-  if (h->parity0) {
-    free(h->parity0);
-  }
-  if (h->parity1) {
-    free(h->parity1);
-  }
-
-  map_gen_free(&h->dec);
-
-  for (int i=0;i<SRSLTE_NOF_TC_CB_SIZES;i++) {
-    srslte_tc_interl_free(&h->interleaver[i]);    
-  }
-
-  bzero(h, sizeof(srslte_tdec_sse_t));
+  // Transpose 8 vectors 
+  __m128i t0 = _mm_unpacklo_epi16(a, b);
+  __m128i t1 = _mm_unpacklo_epi16(c, d);
+  __m128i t2 = _mm_unpacklo_epi16(e, f);
+  __m128i t3 = _mm_unpacklo_epi16(g, h);
+  __m128i t4 = _mm_unpackhi_epi16(a, b);
+  __m128i t5 = _mm_unpackhi_epi16(c, d);
+  __m128i t6 = _mm_unpackhi_epi16(e, f);
+  __m128i t7 = _mm_unpackhi_epi16(g, h);
+
+  __m128i s0 = _mm_unpacklo_epi32(t0, t1);
+  __m128i s1 = _mm_unpackhi_epi32(t0, t1);
+  __m128i s2 = _mm_unpacklo_epi32(t2, t3);
+  __m128i s3 = _mm_unpackhi_epi32(t2, t3);
+  __m128i s4 = _mm_unpacklo_epi32(t4, t5);
+  __m128i s5 = _mm_unpackhi_epi32(t4, t5);
+  __m128i s6 = _mm_unpacklo_epi32(t6, t7);
+  __m128i s7 = _mm_unpackhi_epi32(t6, t7);
+
+  __m128i x0 = _mm_unpacklo_epi64(s0, s2);
+  __m128i x1 = _mm_unpackhi_epi64(s0, s2);
+  __m128i x2 = _mm_unpacklo_epi64(s1, s3);
+  __m128i x3 = _mm_unpackhi_epi64(s1, s3);
+  __m128i x4 = _mm_unpacklo_epi64(s4, s6);
+  __m128i x5 = _mm_unpackhi_epi64(s4, s6);
+  __m128i x6 = _mm_unpacklo_epi64(s5, s7);
+  __m128i x7 = _mm_unpackhi_epi64(s5, s7);
+
+  // Cascade max on the transposed vector 
+  __m128i res = _mm_max_epi16(x0,
+                _mm_max_epi16(x1,
+                _mm_max_epi16(x2,
+                _mm_max_epi16(x3,
+                _mm_max_epi16(x4,
+                _mm_max_epi16(x5,
+                _mm_max_epi16(x6,
+                              x7)))))));
+                              
+  return res;   
 }
 
-/* Deinterleaves the 3 streams from the input (systematic and 2 parity bits) into 
- * 3 buffers ready to be used by compute_gamma() 
- */
-void deinterleave_input(srslte_tdec_sse_t *h, int16_t *input, uint32_t long_cb) {
-  uint32_t i;
+void map_sse_beta(map_gen_t * s, int16_t * output, uint32_t long_cb)
+{
+  int k;
+  uint32_t end = long_cb + 3;
+  const __m128i *alphaPtr = (const __m128i*) s->alpha;
  
-  __m128i *inputPtr = (__m128i*) input; 
-  __m128i in0, in1, in2;
-  __m128i s0, s1, s2, s;
-  __m128i p00, p01, p02, p0;
-  __m128i p10, p11, p12, p1;
-  
-  __m128i *sysPtr = (__m128i*) h->syst; 
-  __m128i *pa0Ptr = (__m128i*) h->parity0; 
-  __m128i *pa1Ptr = (__m128i*) h->parity1; 
-  
-  // pick bits 0, 3, 6 from 1st word
-  __m128i s0_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,13,12,7,6,1,0);
-  // pick bits 1, 4, 7 from 2st word
-  __m128i s1_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,15,14,9,8,3,2,0xff,0xff,0xff,0xff,0xff,0xff);
-  // pick bits 2, 5 from 3rd word
-  __m128i s2_mask = _mm_set_epi8(11,10,5,4,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
-
-  // pick bits 1, 4, 7 from 1st word
-  __m128i p00_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,15,14,9,8,3,2);
-  // pick bits 2, 5, from 2st word
-  __m128i p01_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,11,10,5,4,0xff,0xff,0xff,0xff,0xff,0xff);
-  // pick bits 0, 3, 6 from 3rd word
-  __m128i p02_mask = _mm_set_epi8(13,12,7,6,1,0,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
+  __m128i beta_k = _mm_set_epi16(-INF, -INF, -INF, -INF, -INF, -INF, -INF, 0);
+  __m128i g, alpha_k; 
+  __m128i bn, bn_0, bn_1, bn_2, bn_3, bn_4, bn_5, bn_6, bn_7;
+  __m128i bp, bp_0, bp_1, bp_2, bp_3, bp_4, bp_5, bp_6, bp_7;
   
-  // pick bits 2, 5 from 1st word
-  __m128i p10_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,11,10,5,4);
-  // pick bits 0, 3, 6, from 2st word
-  __m128i p11_mask = _mm_set_epi8(0xff,0xff,0xff,0xff,0xff,0xff,13,12,7,6,1,0,0xff,0xff,0xff,0xff);
-  // pick bits 1, 4, 7 from 3rd word
-  __m128i p12_mask = _mm_set_epi8(15,14,9,8,3,2,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff);
-      
-  // Split systematic and parity bits
-  for (i = 0; i < long_cb/8; i++) {
-        
-    in0 = _mm_load_si128(inputPtr); inputPtr++; 
-    in1 = _mm_load_si128(inputPtr); inputPtr++;   
-    in2 = _mm_load_si128(inputPtr); inputPtr++;
-    
-    /* Deinterleave Systematic bits */
-    s0 = _mm_shuffle_epi8(in0, s0_mask);
-    s1 = _mm_shuffle_epi8(in1, s1_mask);
-    s2 = _mm_shuffle_epi8(in2, s2_mask);    
-    s = _mm_or_si128(s0, s1);
-    s = _mm_or_si128(s, s2);
-
-    _mm_store_si128(sysPtr, s);
-    sysPtr++;
-
-    /* Deinterleave parity 0 bits */
-    p00 = _mm_shuffle_epi8(in0, p00_mask);
-    p01 = _mm_shuffle_epi8(in1, p01_mask);
-    p02 = _mm_shuffle_epi8(in2, p02_mask);    
-    p0 = _mm_or_si128(p00, p01);
-    p0 = _mm_or_si128(p0, p02);
-    
-    _mm_store_si128(pa0Ptr, p0);
-    pa0Ptr++;
-
-    /* Deinterleave parity 1 bits */
-    p10 = _mm_shuffle_epi8(in0, p10_mask);
-    p11 = _mm_shuffle_epi8(in1, p11_mask);
-    p12 = _mm_shuffle_epi8(in2, p12_mask);    
-    p1 = _mm_or_si128(p10, p11);
-    p1 = _mm_or_si128(p1, p12);
+  /* Define the shuffle constant for the positive beta */
+  __m128i shuf_bp = _mm_set_epi8(
+    15, 14, // 7
+    7,  6,  // 3
+    5,  4,  // 2
+    13, 12, // 6
+    11, 10, // 5
+    3,  2,  // 1
+    1,  0,  // 0
+    9,  8   // 4
+  );
 
-    _mm_store_si128(pa1Ptr, p1);
-    pa1Ptr++;    
+  /* Define the shuffle constant for the negative beta */
+  __m128i shuf_bn = _mm_set_epi8(
+    7,   6, // 3
+    15, 14, // 7
+    13, 12, // 6
+    5,  4,  // 2
+    3,  2,  // 1
+    11, 10, // 5
+    9,  8,  // 4
+    1,  0   // 0
+  );
+ 
+  alphaPtr += long_cb-1;
 
-  }
+  /* Define shuffle for branch costs */
+  __m128i shuf_g[4];  
+  shuf_g[3] = _mm_set_epi8(3,2,1,0,1,0,3,2,3,2,1,0,1,0,3,2);
+  shuf_g[2] = _mm_set_epi8(7,6,5,4,5,4,7,6,7,6,5,4,5,4,7,6);
+  shuf_g[1] = _mm_set_epi8(11,10,9,8,9,8,11,10,11,10,9,8,9,8,11,10);
+  shuf_g[0] = _mm_set_epi8(15,14,13,12,13,12,15,14,15,14,13,12,13,12,15,14);
+  __m128i gv;
+  int16_t *b = &s->branch[2*long_cb-8];
+  __m128i *gPtr = (__m128i*) b;
+  /* Define shuffle for beta normalization */
+  __m128i shuf_norm = _mm_set_epi8(1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0);
   
-  for (i = 0; i < 3; i++) {
-    h->syst[i+long_cb]    = input[3*long_cb + 2*i];
-    h->parity0[i+long_cb] = input[3*long_cb + 2*i + 1];
-  }
-  for (i = 0; i < 3; i++) {
-    h->app2[i+long_cb]    = input[3*long_cb + 6 + 2*i];
-    h->parity1[i+long_cb] = input[3*long_cb + 6 + 2*i + 1];
-  }
-
-}
-
-/* Runs 1 turbo decoder iteration */
-void srslte_tdec_sse_iteration(srslte_tdec_sse_t * h, int16_t * input, uint32_t long_cb)
-{
-
-  if (h->current_cbidx >= 0) {
-    uint16_t *inter   = h->interleaver[h->current_cbidx].forward;
-    uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
-    
-    if (h->n_iter == 0) {
-      deinterleave_input(h, input, long_cb);
-    }
-    
-    // Add apriori information to decoder 1 
-    if (h->n_iter > 0) {
-      srslte_vec_sub_sss(h->app1, h->ext1, h->app1, long_cb);
-    }
-        
-    // Run MAP DEC #1
-    if (h->n_iter == 0) {
-      map_gen_dec(&h->dec, h->syst, NULL, h->parity0, h->ext1, long_cb);            
-    } else {
-      map_gen_dec(&h->dec, h->syst, h->app1, h->parity0, h->ext1, long_cb);      
-    }
-
-    // Convert aposteriori information into extrinsic information    
-    if (h->n_iter > 0) {
-      srslte_vec_sub_sss(h->ext1, h->app1, h->ext1, long_cb);
-    }
-    
-    // Interleave extrinsic output of DEC1 to form apriori info for decoder 2
-    srslte_vec_lut_sss(h->ext1, deinter, h->app2, long_cb);
-
-    // Run MAP DEC #2. 2nd decoder uses apriori information as systematic bits
-    map_gen_dec(&h->dec, h->app2, NULL, h->parity1, h->ext2, long_cb);
-
-    // Deinterleaved extrinsic bits become apriori info for decoder 1 
-    srslte_vec_lut_sss(h->ext2, inter, h->app1, long_cb);
-    
-    h->n_iter++;
-  } else {
-    fprintf(stderr, "Error CB index not set (call srslte_tdec_sse_reset() first\n");    
-  }
-}
+  /* This defines a beta computation step: 
+   * Adds and substracts the branch metrics to the previous beta step, 
+   * shuffles the states according to the trellis path and selects maximum state 
+   */
+#define BETA_STEP(g)     bp = _mm_add_epi16(beta_k, g);\
+    bn = _mm_sub_epi16(beta_k, g);\
+    bp = _mm_shuffle_epi8(bp, shuf_bp);\
+    bn = _mm_shuffle_epi8(bn, shuf_bn);\
+    beta_k = _mm_max_epi16(bp, bn);    
 
-/* Resets the decoder and sets the codeblock length */
-int srslte_tdec_sse_reset(srslte_tdec_sse_t * h, uint32_t long_cb)
-{
-  if (long_cb > h->max_long_cb) {
-    fprintf(stderr, "TDEC was initialized for max_long_cb=%d\n",
-            h->max_long_cb);
-    return -1;
-  }
-  h->n_iter = 0; 
-  h->current_cbidx = srslte_cbsegm_cbindex(long_cb);
-  if (h->current_cbidx < 0) {
-    fprintf(stderr, "Invalid CB length %d\n", long_cb);
-    return -1; 
-  }
-  return 0;
-}
+    /* Loads the alpha metrics from memory and adds them to the temporal bn and bp 
+     * metrics. 
+     */
+#define BETA_STEP_CNT(c,d) g = _mm_shuffle_epi8(gv, shuf_g[c]);\
+    BETA_STEP(g)\
+    alpha_k = _mm_load_si128(alphaPtr);\
+    alphaPtr--;\
+    bp_##d = _mm_add_epi16(bp, alpha_k);\
+    bn_##d = _mm_add_epi16(bn, alpha_k);\
 
-void srslte_tdec_sse_decision(srslte_tdec_sse_t * h, uint8_t *output, uint32_t long_cb)
-{
-  __m128i zero     = _mm_set1_epi16(0);
-  __m128i lsb_mask = _mm_set1_epi16(1);
+  /* The tail does not require to load alpha or produce outputs. Only update 
+   * beta metrics accordingly */
+  for (k=end-1; k>=long_cb; k--) {
+    int16_t g0 = s->branch[2*k];
+    int16_t g1 = s->branch[2*k+1];
+    g = _mm_set_epi16(g1, g0, g0, g1, g1, g0, g0, g1);
+    BETA_STEP(g);
+  }  
   
-  __m128i *appPtr = (__m128i*) h->app1;
-  __m128i *outPtr = (__m128i*) output;
-  __m128i ap, out, out0, out1; 
-    
-  for (uint32_t i = 0; i < long_cb/16; i++) {
-    ap   = _mm_load_si128(appPtr); appPtr++;    
-    out0 = _mm_and_si128(_mm_cmpgt_epi16(ap, zero), lsb_mask);
-    ap   = _mm_load_si128(appPtr); appPtr++;
-    out1 = _mm_and_si128(_mm_cmpgt_epi16(ap, zero), lsb_mask);
+  /* We inline 2 trelis steps for each normalization */
+  __m128i norm;
+  __m128i *outPtr = (__m128i*) &output[long_cb-8];
+  for (; k >= 0; k-=8) {    
+    gv = _mm_load_si128(gPtr);
+    gPtr--;
     
-    out  = _mm_packs_epi16(out0, out1);
-    _mm_store_si128(outPtr, out);
-    outPtr++;
-  }
-  if (long_cb%16) {
-    for (int i=0;i<8;i++) {
-      output[long_cb-8+i] = h->app1[long_cb-8+i]>0?1:0;
-    }
-  }
-}
-
-void srslte_tdec_sse_decision_byte(srslte_tdec_sse_t * h, uint8_t *output, uint32_t long_cb)
-{
-  uint8_t mask[8] = {0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1};
-  
-  // long_cb is always byte aligned
-  for (uint32_t i = 0; i < long_cb/8; i++) {
-    uint8_t out0 = h->app1[8*i+0]>0?mask[0]:0;
-    uint8_t out1 = h->app1[8*i+1]>0?mask[1]:0;
-    uint8_t out2 = h->app1[8*i+2]>0?mask[2]:0;
-    uint8_t out3 = h->app1[8*i+3]>0?mask[3]:0;
-    uint8_t out4 = h->app1[8*i+4]>0?mask[4]:0;
-    uint8_t out5 = h->app1[8*i+5]>0?mask[5]:0;
-    uint8_t out6 = h->app1[8*i+6]>0?mask[6]:0;
-    uint8_t out7 = h->app1[8*i+7]>0?mask[7]:0;
+    BETA_STEP_CNT(0,0);
+    BETA_STEP_CNT(1,1);
+    BETA_STEP_CNT(2,2);
+    BETA_STEP_CNT(3,3);
+    norm = _mm_shuffle_epi8(beta_k, shuf_norm); 
+    beta_k = _mm_sub_epi16(beta_k, norm);
+    gv = _mm_load_si128(gPtr);
+    gPtr--;
+    BETA_STEP_CNT(0,4);
+    BETA_STEP_CNT(1,5);
+    BETA_STEP_CNT(2,6);
+    BETA_STEP_CNT(3,7);    
+    norm = _mm_shuffle_epi8(beta_k, shuf_norm); 
+    beta_k = _mm_sub_epi16(beta_k, norm);
     
-    output[i] = out0 | out1 | out2 | out3 | out4 | out5 | out6 | out7; 
-  }
+    __m128i bn_transp = transposed_max(bn_7, bn_6, bn_5, bn_4, bn_3, bn_2, bn_1, bn_0);
+    __m128i bp_transp = transposed_max(bp_7, bp_6, bp_5, bp_4, bp_3, bp_2, bp_1, bp_0);
+    __m128i outval = _mm_sub_epi16(bp_transp,bn_transp);
+    _mm_store_si128(outPtr, outval);
+    outPtr--;    
+  }  
 }
+#endif
 
-/* Runs nof_iterations iterations and decides the output bits */
-int srslte_tdec_sse_run_all(srslte_tdec_sse_t * h, int16_t * input, uint8_t *output,
-                  uint32_t nof_iterations, uint32_t long_cb)
-{
-  if (srslte_tdec_sse_reset(h, long_cb)) {
-    return SRSLTE_ERROR; 
-  }
 
-  do {
-    srslte_tdec_sse_iteration(h, input, long_cb);
-  } while (h->n_iter < nof_iterations);
 
-  srslte_tdec_sse_decision_byte(h, output, long_cb);
-  
-  return SRSLTE_SUCCESS;
-}
 
 #endif
 
diff --git a/lib/src/phy/fec/turbodecoder_sse_inter.c b/lib/src/phy/fec/turbodecoder_sse_inter.c
new file mode 100644
index 000000000..bb1168368
--- /dev/null
+++ b/lib/src/phy/fec/turbodecoder_sse_inter.c
@@ -0,0 +1,198 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <strings.h>
+#include <math.h>
+
+#include "srslte/phy/fec/turbodecoder_simd_inter.h"
+#include "srslte/phy/utils/vector.h"
+
+
+#define NCB 8
+
+#define INF 10000
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+
+void sse_inter_extract_syst1(srslte_tdec_simd_inter_t *h, uint16_t *inter, uint32_t long_cb) 
+{
+  __m128i *llr1Ptr  = (__m128i*) h->llr1; 
+  __m128i *wPtr     = (__m128i*) h->w; 
+  __m128i *syst1Ptr = (__m128i*) h->syst1; 
+  
+  for (int i = 0; i < long_cb; i++) {    
+    __m128i llr1 = _mm_load_si128(&llr1Ptr[inter[i]]);
+    __m128i w    = _mm_load_si128(&wPtr[inter[i]]);
+    _mm_store_si128(syst1Ptr++, _mm_sub_epi16(llr1, w));
+  }
+}
+
+void sse_inter_update_w(srslte_tdec_simd_inter_t *h, uint16_t *deinter, uint32_t long_cb) 
+{
+  __m128i *llr1Ptr  = (__m128i*) h->llr1; 
+  __m128i *llr2Ptr  = (__m128i*) h->llr2; 
+  __m128i *wPtr     = (__m128i*) h->w; 
+  __m128i *syst1Ptr = (__m128i*) h->syst1; 
+  
+  for (int i = 0; i < long_cb; i++) {    
+    __m128i llr1 = _mm_load_si128(llr1Ptr++);
+    __m128i w    = _mm_load_si128(wPtr++);
+    __m128i llr2 = _mm_load_si128(&llr2Ptr[deinter[i]]);
+    
+    _mm_store_si128(syst1Ptr++, _mm_add_epi16(w, _mm_sub_epi16(llr2, llr1)));
+  }
+}
+
+/* Computes beta values */
+void map_sse_inter_beta(srslte_tdec_simd_inter_t * s, int16_t *input, int16_t *parity, int16_t * output, uint32_t long_cb)
+{
+  __m128i m_b[8], new[8], old[8], max1[8], max0[8];
+  __m128i x, y, xy;
+  __m128i m1, m0;
+  uint32_t end = long_cb + 3;
+  uint32_t i;
+
+  __m128i *inputPtr  = (__m128i*) input;
+  __m128i *parityPtr = (__m128i*) parity;
+  __m128i *outputPtr = (__m128i*) output;
+  __m128i *alphaPtr  = (__m128i*) s->alpha;
+
+  for (int k = end - 1; k >= 0; k--) {
+    x = _mm_load_si128(inputPtr++);
+    y = _mm_load_si128(parityPtr++);
+
+    xy = _mm_add_epi16(x,y);
+
+    m_b[0] = _mm_add_epi16(old[4], xy);
+    m_b[1] = old[4];
+    m_b[2] = _mm_add_epi16(old[5], y);
+    m_b[3] = _mm_add_epi16(old[5], x);
+    m_b[4] = _mm_add_epi16(old[6], x);
+    m_b[5] = _mm_add_epi16(old[6], y);
+    m_b[6] = old[7];
+    m_b[7] = _mm_add_epi16(old[7], xy);
+
+    new[0] = old[0];
+    new[1] = _mm_add_epi16(old[0], xy);
+    new[2] = _mm_add_epi16(old[1], x);
+    new[3] = _mm_add_epi16(old[1], y);
+    new[4] = _mm_add_epi16(old[2], y);
+    new[5] = _mm_add_epi16(old[2], x);
+    new[6] = _mm_add_epi16(old[3], xy);
+    new[7] = old[3];
+
+    for (i = 0; i < 8; i++) {
+      __m128i alpha = _mm_load_si128(alphaPtr++);
+      max0[i] = _mm_add_epi16(alpha, m_b[i]);
+      max1[i] = _mm_add_epi16(alpha, new[i]);
+    }
+
+    m1 = _mm_max_epi16(max1[0], max1[1]);
+    m0 = _mm_max_epi16(max0[0], max0[1]);
+
+    for (i = 2; i < 8; i++) {
+      m1 = _mm_max_epi16(m1, max1[i]);
+      m0 = _mm_max_epi16(m0, max0[i]);      
+    }
+
+    for (i = 0; i < 8; i++) {
+      new[i] = _mm_max_epi16(m_b[i], new[i]);
+      old[i] = new[i];
+    }
+
+    __m128i out = _mm_sub_epi16(m1, m0);
+    _mm_store_si128(outputPtr++, out);
+
+    // normalize 
+    if ((k%4)==0) {
+      for (int i=1;i<8;i++) {
+        _mm_sub_epi16(old[i], old[0]);
+      }
+    }
+  }
+}
+
+/* Computes alpha metrics */
+void map_see_inter_alpha(srslte_tdec_simd_inter_t * s, int16_t *input, int16_t *parity, uint32_t long_cb)
+{
+  __m128i m_b[8], new[8], old[8];
+  __m128i x, y, xy;
+  uint32_t k;
+  
+  __m128i *inputPtr  = (__m128i*) input;
+  __m128i *parityPtr = (__m128i*) parity;
+  __m128i *alphaPtr  = (__m128i*) s->alpha;
+  
+  old[0] = _mm_set1_epi16(0);
+  for (int i = 1; i < 8; i++) {
+    old[i] = _mm_set1_epi16(-INF);
+  }
+
+  for (k = 0; k < long_cb; k++) {
+    x = _mm_load_si128(inputPtr++);
+    y = _mm_load_si128(parityPtr++);
+
+    xy = _mm_add_epi16(x,y);
+
+    m_b[0] = old[0];
+    m_b[1] = _mm_add_epi16(old[3], y);
+    m_b[2] = _mm_add_epi16(old[4], y);
+    m_b[3] = old[7];
+    m_b[4] = old[1];
+    m_b[5] = _mm_add_epi16(old[2], y);
+    m_b[6] = _mm_add_epi16(old[5], y);
+    m_b[7] = old[6];
+
+    new[0] = _mm_add_epi16(old[1], xy);
+    new[1] = _mm_add_epi16(old[2], x);
+    new[2] = _mm_add_epi16(old[5], x);
+    new[3] = _mm_add_epi16(old[6], xy);
+    new[4] = _mm_add_epi16(old[0], xy);
+    new[5] = _mm_add_epi16(old[3], x);
+    new[6] = _mm_add_epi16(old[4], x);
+    new[7] = _mm_add_epi16(old[7], xy);    
+    
+    for (int i = 0; i < 8; i++) {
+      new[i] = _mm_max_epi16(m_b[i], new[i]);
+      old[i] = new[i];
+      _mm_store_si128(alphaPtr++, old[i]);
+    }
+
+    // normalize 
+    if ((k%4)==0) {
+      for (int i=1;i<8;i++) {
+        _mm_sub_epi16(old[i], old[0]);
+      }
+    }
+  }
+}
+
+#endif
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index ba4c8951b..5ae539a22 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -311,8 +311,124 @@ static int encode_tb(srslte_sch_t *q,
   return encode_tb_off(q, soft_buffer, cb_segm, Qm, rv, nof_e_bits, data, e_bits, 0);
 }
 
+bool decode_tb_cb(srslte_sch_t *q, 
+                     srslte_softbuffer_rx_t *softbuffer, srslte_cbsegm_t *cb_segm, 
+                     uint32_t Qm, uint32_t rv, uint32_t nof_e_bits, 
+                     int16_t *e_bits, uint8_t *data,
+                     uint32_t cb_size_group, uint8_t parity[3]) 
+{
+
+  bool cb_map[SRSLTE_MAX_CODEBLOCKS];
+  
+  bzero(cb_map, sizeof(bool)*SRSLTE_MAX_CODEBLOCKS);
+  
+  uint32_t cb_idx[SRSLTE_TDEC_NPAR];
+  int16_t *decoder_input[SRSLTE_TDEC_NPAR];
+  
+  uint32_t nof_cb     = cb_size_group?cb_segm->C2:cb_segm->C1;
+  uint32_t first_cb   = cb_size_group?cb_segm->C1:0;
+  uint32_t cb_len     = cb_size_group?cb_segm->K2:cb_segm->K1;
+  uint32_t cb_len_idx = cb_size_group?cb_segm->K2_idx:cb_segm->K1_idx;
+
+  uint32_t rlen       = cb_segm->C==1?cb_len:(cb_len-24);
+  uint32_t Gp         = nof_e_bits / Qm;
+  uint32_t gamma      = cb_segm->C>0?Gp%cb_segm->C:Gp;
+  uint32_t n_e        = Qm * (Gp/cb_segm->C);
   
+  if (nof_cb > SRSLTE_MAX_CODEBLOCKS) {
+    fprintf(stderr, "Error SRSLTE_MAX_CODEBLOCKS=%d\n", SRSLTE_MAX_CODEBLOCKS);
+    return false; 
+  }
+  
+  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+    cb_idx[i] = i+first_cb; 
+  }
+    
+  srslte_tdec_reset(&q->decoder, cb_len);
+  
+  uint32_t remaining_cb = nof_cb; 
+  
+  while(remaining_cb>0) {
+    uint32_t npar = SRSLTE_MIN(remaining_cb, SRSLTE_TDEC_NPAR);
+
+    // Unratematch the codeblocks left to decode 
+    for (int i=0;i<npar;i++) {
+      
+      // Find a not processed CB 
+      cb_idx[i]=first_cb;
+      while(cb_idx[i]<first_cb+nof_cb && cb_map[cb_idx[i]]) {
+        cb_idx[i]++;
+      }
+ 
+      cb_map[cb_idx[i]] = true; 
+      
+      uint32_t rp   = cb_idx[i]*n_e;  
+      uint32_t n_e2 = n_e;
+      
+      if (cb_idx[i] > cb_segm->C - gamma) {
+        n_e2 = n_e+Qm;
+        rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
+      }
+            
+      INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
+      if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
+        fprintf(stderr, "Error in rate matching\n");
+        return SRSLTE_ERROR;
+      }
+
+      decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
+    }
+    
+    // Run 1 iteration for up to TDEC_NPAR codeblocks 
+    if (SRSLTE_TDEC_NPAR > 1) {
+      INFO("Processing %d CBs, index %d,%d\n", npar, cb_idx[0], cb_idx[1]);      
+    }
+    srslte_tdec_iteration_par(&q->decoder, decoder_input, npar, cb_len);
+
+    // Decide output bits and compute CRC 
+    for (int i=0;i<npar;i++) {
+      srslte_tdec_decision_byte_par_cb(&q->decoder, q->cb_in, i, cb_len);
 
+      uint32_t len_crc; 
+      srslte_crc_t *crc_ptr; 
+      
+      if (cb_segm->C > 1) {
+        len_crc = cb_len; 
+        crc_ptr = &q->crc_cb; 
+      } else {
+        len_crc = cb_segm->tbs+24; 
+        crc_ptr = &q->crc_tb; 
+      }
+
+      // CRC is OK
+      if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
+
+        uint32_t wp = cb_idx[i]*rlen;
+        
+        // If it's not the last CB, copy data to another buffer and remove CRC */
+        if (cb_idx[i] < cb_segm->C - 1) {
+          memcpy(&data[wp/8], q->cb_in, rlen/8 * sizeof(uint8_t));
+        // If it's the last CB Append Transport Block parity bits to the last CB
+        } else {        
+          memcpy(&data[wp/8], q->cb_in, (rlen - 24)/8 * sizeof(uint8_t));
+          memcpy(parity, &q->cb_in[(rlen - 24)/8], 3 * sizeof(uint8_t));
+        }
+
+        // Reset number of iterations for that CB in the decoder 
+        srslte_tdec_reset_cb(&q->decoder, i);
+        remaining_cb--;        
+        
+      // CRC is error and exceeded maximum iterations for this CB. 
+      // Early stop the whole transport block.
+      } else if (srslte_tdec_get_nof_iterations_cb(&q->decoder, i) >= q->max_iterations) {
+        INFO("CB %d: Error. TB is erroneous.\n", cb_idx[i]);
+        return false; 
+      }
+    }    
+  }
+  
+  return true;  
+}
 
 /**
  * Decode a transport block according to 36.212 5.3.2
@@ -332,10 +448,6 @@ static int decode_tb(srslte_sch_t *q,
                      uint32_t Qm, uint32_t rv, uint32_t nof_e_bits, 
                      int16_t *e_bits, uint8_t *data) 
 {
-  uint8_t parity[3] = {0, 0, 0};
-  uint32_t par_rx, par_tx;
-  uint32_t i;
-  uint32_t cb_len, rp, wp, rlen, n_e;
   
   if (q            != NULL && 
       data         != NULL &&       
@@ -343,17 +455,11 @@ static int decode_tb(srslte_sch_t *q,
       e_bits       != NULL &&
       cb_segm      != NULL)
   {
-
+    
     if (cb_segm->tbs == 0 || cb_segm->C == 0) {
       return SRSLTE_SUCCESS;
     }
     
-    rp = 0;
-    rp = 0;
-    wp = 0;
-    uint32_t Gp = nof_e_bits / Qm;
-    uint32_t gamma=Gp;
-
     if (cb_segm->F) {
       fprintf(stderr, "Error filler bits are not supported. Use standard TBS\n");
       return SRSLTE_ERROR;       
@@ -363,128 +469,41 @@ static int decode_tb(srslte_sch_t *q,
       fprintf(stderr, "Error number of CB (%d) exceeds soft buffer size (%d CBs)\n", cb_segm->C, softbuffer->max_cb);
       return SRSLTE_ERROR;
     }
+        
+    uint8_t parity[3] = {0, 0, 0};
+    bool crc_ok = true; 
     
-    if (cb_segm->C>0) {
-      gamma = Gp%cb_segm->C;
-    }
+    uint32_t nof_cb_groups = cb_segm->C2>0?2:1; 
     
-    bool early_stop = true;
-    for (i = 0; i < cb_segm->C && early_stop; i++) {
-
-      /* Get read/write lengths */
-      uint32_t cblen_idx; 
-      if (i < cb_segm->C2) {
-        cb_len = cb_segm->K2;
-        cblen_idx = cb_segm->K2_idx;
-      } else {
-        cb_len = cb_segm->K1;
-        cblen_idx = cb_segm->K1_idx;
-      }
-      
-      if (cb_segm->C == 1) {
-        rlen = cb_len;
-      } else {
-        rlen = cb_len - 24;
-      }
-
-      if (i <= cb_segm->C - gamma - 1) {
-        n_e = Qm * (Gp/cb_segm->C);
-      } else {
-        n_e = Qm * ((uint32_t) ceilf((float) Gp/cb_segm->C));
-      }
-
-      /* 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");
-        return SRSLTE_ERROR;
-      }
-
-      if (SRSLTE_VERBOSE_ISDEBUG()) {
-        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 */
-      q->nof_iterations = 0; 
-      uint32_t len_crc; 
-      srslte_crc_t *crc_ptr; 
-      early_stop = false; 
-
-      srslte_tdec_reset(&q->decoder, cb_len);
-            
-      do {
-        srslte_tdec_iteration(&q->decoder, softbuffer->buffer_f[i], cb_len); 
-        q->nof_iterations++;
-        
-        if (cb_segm->C > 1) {
-          len_crc = cb_len; 
-          crc_ptr = &q->crc_cb; 
-        } else {
-          len_crc = cb_segm->tbs+24; 
-          crc_ptr = &q->crc_tb; 
-        }
-
-        srslte_tdec_decision_byte(&q->decoder, q->cb_in, cb_len);
-                 
-        /* Check Codeblock CRC and stop early if correct */
-        if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
-          early_stop = true;           
-        }
-       
-      } while (q->nof_iterations < q->max_iterations && !early_stop);
-      q->average_nof_iterations = SRSLTE_VEC_EMA((float) q->nof_iterations, q->average_nof_iterations, 0.2);
-
-      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 CB CRC is not correct, early_stop will be false and wont continue with rest of CBs
-
-      /* Copy data to another buffer, removing the Codeblock CRC */
-      if (i < cb_segm->C - 1) {
-        memcpy(&data[wp/8], q->cb_in, rlen/8 * sizeof(uint8_t));
-      } else {        
-        /* Append Transport Block parity bits to the last CB */
-        memcpy(&data[wp/8], q->cb_in, (rlen - 24)/8 * sizeof(uint8_t));
-        memcpy(parity, &q->cb_in[(rlen - 24)/8], 3 * sizeof(uint8_t));
-      }
-      
-      if (SRSLTE_VERBOSE_ISDEBUG()) {
-        early_stop = true; 
-      }
-      
-      /* Set read/write pointers */
-      wp += rlen;
-      rp += n_e;
+    // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_NPAR
+    for (uint32_t i=0;i<nof_cb_groups && crc_ok;i++) {
+      crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i, parity);            
     }
     
-    if (!early_stop) {
-      INFO("CB %d failed. TB is erroneous.\n",i-1);
-      return SRSLTE_ERROR; 
-    } else {
-      INFO("END CB#%d: wp: %d, rp: %d\n", i, wp, rp);
+    if (crc_ok) {
 
+      uint32_t par_rx = 0, par_tx = 0;
+  
       // Compute transport block CRC
       par_rx = srslte_crc_checksum_byte(&q->crc_tb, data, cb_segm->tbs);
 
       // check parity bits
       par_tx = ((uint32_t) parity[0])<<16 | ((uint32_t) parity[1])<<8 | ((uint32_t) parity[2]);
-      
+
       if (!par_rx) {
-        INFO("Warning: Received all-zero transport block\n\n", 0);      
+        INFO("Warning: Received all-zero transport block\n\n",0);      
       }
 
       if (par_rx == par_tx) {
-        INFO("TB decoded OK\n",i);
+        INFO("TB decoded OK\n",0);
         return SRSLTE_SUCCESS;
       } else {
         INFO("Error in TB parity: par_tx=0x%x, par_rx=0x%x\n", par_tx, par_rx);
         return SRSLTE_ERROR;
       }
-      
-    }
+    } else {
+      return SRSLTE_ERROR; 
+    }        
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }

From e717ccf73a390df33bb5d7a6ab54fd3d375bee36 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 19 Jun 2017 23:57:15 +0200
Subject: [PATCH 004/269] new AVX2 decoder working over the air

---
 lib/src/phy/fec/turbodecoder_avx.c  |   2 +-
 lib/src/phy/fec/turbodecoder_simd.c |   6 +-
 lib/src/phy/phch/sch.c              | 135 +++++++++++++++-------------
 3 files changed, 76 insertions(+), 67 deletions(-)

diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
index a98279d25..2a2f6f925 100644
--- a/lib/src/phy/fec/turbodecoder_avx.c
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -460,7 +460,7 @@ void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity,
   }
   
   if (long_cb%16) {
-    map_sse_gamma_single((int16_t*) resPtr, (int16_t*) appPtr, (int16_t*) inPtr, (int16_t*) paPtr);
+    map_sse_gamma_single((int16_t*) resPtr, (int16_t*) inPtr, (int16_t*) appPtr, (int16_t*) paPtr);
   }
     
   for (int i=long_cb;i<long_cb+3;i++) {
diff --git a/lib/src/phy/fec/turbodecoder_simd.c b/lib/src/phy/fec/turbodecoder_simd.c
index 7a36c79fa..24f550f08 100644
--- a/lib/src/phy/fec/turbodecoder_simd.c
+++ b/lib/src/phy/fec/turbodecoder_simd.c
@@ -131,9 +131,9 @@ void map_simd_free(map_gen_t * h)
 void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_NPAR], int16_t *app[SRSLTE_TDEC_NPAR], int16_t * parity[SRSLTE_TDEC_NPAR], 
                   int16_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
- 
+  
   // Compute branch metrics
-  for (int i=0;i<nof_cb;i++) {
+  for (int i=0;i<nof_cb;i++) {    
     map_simd_gamma(h, input[i], app?app[i]:NULL, parity[i], i, nof_cb, long_cb);    
   }
 
@@ -344,7 +344,7 @@ void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_T
     uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
     
     for (int i=0;i<nof_cb;i++) {
-      if (h->n_iter[i] == 0) {
+      if (h->n_iter[i] == 0 && input[i]) {
         deinterleave_input_simd(h, input[i], i, long_cb);        
       }        
     }
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 5ae539a22..f08054988 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -315,14 +315,13 @@ bool decode_tb_cb(srslte_sch_t *q,
                      srslte_softbuffer_rx_t *softbuffer, srslte_cbsegm_t *cb_segm, 
                      uint32_t Qm, uint32_t rv, uint32_t nof_e_bits, 
                      int16_t *e_bits, uint8_t *data,
-                     uint32_t cb_size_group, uint8_t parity[3]) 
+                     uint32_t cb_size_group) 
 {
 
   bool cb_map[SRSLTE_MAX_CODEBLOCKS];
-  
-  bzero(cb_map, sizeof(bool)*SRSLTE_MAX_CODEBLOCKS);
-  
+    
   uint32_t cb_idx[SRSLTE_TDEC_NPAR];
+  bool cb_in_use[SRSLTE_TDEC_NPAR];
   int16_t *decoder_input[SRSLTE_TDEC_NPAR];
   
   uint32_t nof_cb     = cb_size_group?cb_segm->C2:cb_segm->C1;
@@ -341,7 +340,13 @@ bool decode_tb_cb(srslte_sch_t *q,
   }
   
   for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
-    cb_idx[i] = i+first_cb; 
+    cb_idx[i]    = i+first_cb; 
+    cb_in_use[i] = false; 
+    decoder_input[i] = NULL; 
+  }
+  
+  for (int i=0;i<nof_cb;i++) {
+    cb_map[i] = false; 
   }
     
   srslte_tdec_reset(&q->decoder, cb_len);
@@ -350,33 +355,35 @@ bool decode_tb_cb(srslte_sch_t *q,
   
   while(remaining_cb>0) {
     uint32_t npar = SRSLTE_MIN(remaining_cb, SRSLTE_TDEC_NPAR);
-
+        
     // Unratematch the codeblocks left to decode 
     for (int i=0;i<npar;i++) {
       
-      // Find a not processed CB 
-      cb_idx[i]=first_cb;
-      while(cb_idx[i]<first_cb+nof_cb && cb_map[cb_idx[i]]) {
-        cb_idx[i]++;
-      }
- 
-      cb_map[cb_idx[i]] = true; 
-      
-      uint32_t rp   = cb_idx[i]*n_e;  
-      uint32_t n_e2 = n_e;
-      
-      if (cb_idx[i] > cb_segm->C - gamma) {
-        n_e2 = n_e+Qm;
-        rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
-      }
-            
-      INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
-      if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
-        fprintf(stderr, "Error in rate matching\n");
-        return SRSLTE_ERROR;
-      }
+      if (!cb_in_use[i] && i < remaining_cb) {        
+        // Find an unprocessed CB 
+        cb_idx[i]=first_cb;
+        while(cb_idx[i]<first_cb+nof_cb-1 && cb_map[cb_idx[i]]) {
+          cb_idx[i]++;
+        }
+        cb_in_use[i]      = true; 
+        cb_map[cb_idx[i]] = true; 
+        
+        uint32_t rp   = cb_idx[i]*n_e;  
+        uint32_t n_e2 = n_e;
+        
+        if (cb_idx[i] > cb_segm->C - gamma) {
+          n_e2 = n_e+Qm;
+          rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
+        }
+              
+        INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
+        if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
+          fprintf(stderr, "Error in rate matching\n");
+          return SRSLTE_ERROR;
+        }
 
-      decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
+        decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
+      }
     }
     
     // Run 1 iteration for up to TDEC_NPAR codeblocks 
@@ -385,44 +392,41 @@ bool decode_tb_cb(srslte_sch_t *q,
     }
     srslte_tdec_iteration_par(&q->decoder, decoder_input, npar, cb_len);
 
+    q->nof_iterations = srslte_tdec_get_nof_iterations_cb(&q->decoder, 0); 
+    
     // Decide output bits and compute CRC 
     for (int i=0;i<npar;i++) {
-      srslte_tdec_decision_byte_par_cb(&q->decoder, q->cb_in, i, cb_len);
-
-      uint32_t len_crc; 
-      srslte_crc_t *crc_ptr; 
-      
-      if (cb_segm->C > 1) {
-        len_crc = cb_len; 
-        crc_ptr = &q->crc_cb; 
-      } else {
-        len_crc = cb_segm->tbs+24; 
-        crc_ptr = &q->crc_tb; 
-      }
-
-      // CRC is OK
-      if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
+      if (cb_in_use[i]) {        
+        srslte_tdec_decision_byte_par_cb(&q->decoder, q->cb_in, i, cb_len);
 
-        uint32_t wp = cb_idx[i]*rlen;
+        uint32_t len_crc; 
+        srslte_crc_t *crc_ptr; 
         
-        // If it's not the last CB, copy data to another buffer and remove CRC */
-        if (cb_idx[i] < cb_segm->C - 1) {
-          memcpy(&data[wp/8], q->cb_in, rlen/8 * sizeof(uint8_t));
-        // If it's the last CB Append Transport Block parity bits to the last CB
-        } else {        
-          memcpy(&data[wp/8], q->cb_in, (rlen - 24)/8 * sizeof(uint8_t));
-          memcpy(parity, &q->cb_in[(rlen - 24)/8], 3 * sizeof(uint8_t));
+        if (cb_segm->C > 1) {
+          len_crc = cb_len; 
+          crc_ptr = &q->crc_cb; 
+        } else {
+          len_crc = cb_segm->tbs+24; 
+          crc_ptr = &q->crc_tb; 
         }
 
-        // Reset number of iterations for that CB in the decoder 
-        srslte_tdec_reset_cb(&q->decoder, i);
-        remaining_cb--;        
-        
-      // CRC is error and exceeded maximum iterations for this CB. 
-      // Early stop the whole transport block.
-      } else if (srslte_tdec_get_nof_iterations_cb(&q->decoder, i) >= q->max_iterations) {
-        INFO("CB %d: Error. TB is erroneous.\n", cb_idx[i]);
-        return false; 
+        // CRC is OK
+        if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
+
+          memcpy(&data[(cb_idx[i]*rlen)/8], q->cb_in, rlen/8 * sizeof(uint8_t));
+          
+          // Reset number of iterations for that CB in the decoder 
+          srslte_tdec_reset_cb(&q->decoder, i);
+          remaining_cb--;        
+          cb_in_use[i] = false; 
+          
+        // CRC is error and exceeded maximum iterations for this CB. 
+        // Early stop the whole transport block.
+        } else if (srslte_tdec_get_nof_iterations_cb(&q->decoder, i) >= q->max_iterations) {
+          INFO("CB %d: Error. CB is erroneous. remaining_cb=%d, i=%d, first_cb=%d, nof_cb=%d, npar=%d\n", 
+                cb_idx[i], remaining_cb, i, first_cb, nof_cb, npar);          
+          return false; 
+        }
       }
     }    
   }
@@ -470,14 +474,17 @@ static int decode_tb(srslte_sch_t *q,
       return SRSLTE_ERROR;
     }
         
-    uint8_t parity[3] = {0, 0, 0};
     bool crc_ok = true; 
     
     uint32_t nof_cb_groups = cb_segm->C2>0?2:1; 
     
+    data[cb_segm->tbs/8+0] = 0; 
+    data[cb_segm->tbs/8+1] = 0; 
+    data[cb_segm->tbs/8+2] = 0; 
+    
     // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_NPAR
     for (uint32_t i=0;i<nof_cb_groups && crc_ok;i++) {
-      crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i, parity);            
+      crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i);            
     }
     
     if (crc_ok) {
@@ -488,7 +495,9 @@ static int decode_tb(srslte_sch_t *q,
       par_rx = srslte_crc_checksum_byte(&q->crc_tb, data, cb_segm->tbs);
 
       // check parity bits
-      par_tx = ((uint32_t) parity[0])<<16 | ((uint32_t) parity[1])<<8 | ((uint32_t) parity[2]);
+      par_tx = ((uint32_t) data[cb_segm->tbs/8+0])<<16  | 
+               ((uint32_t) data[cb_segm->tbs/8+1])<<8   | 
+               ((uint32_t) data[cb_segm->tbs/8+2]);
 
       if (!par_rx) {
         INFO("Warning: Received all-zero transport block\n\n",0);      

From 026eecf58da5093e1dd161f920ba6d221d033419 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 20 Jun 2017 13:01:43 +0200
Subject: [PATCH 005/269] allow null CB

---
 lib/src/phy/fec/turbodecoder_avx.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
index 2a2f6f925..e25f3a1e7 100644
--- a/lib/src/phy/fec/turbodecoder_avx.c
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -175,8 +175,8 @@ void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t lo
     bn = _mm256_add_epi16(bn, alpha_k);\
     bn = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bn);\
     bp = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bp);\
-    output[0][k-d] = hMax0(bn) - hMax0(bp);\
-    output[1][k-d] = hMax1(bn) - hMax1(bp);
+    if (output[0]) output[0][k-d] = hMax0(bn) - hMax0(bp);\
+    if (output[1]) output[1][k-d] = hMax1(bn) - hMax1(bp);
 
   /* The tail does not require to load alpha or produce outputs. Only update 
    * beta metrics accordingly */

From 9623081da181017332c6f7839a57bcecac806234 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 21 Jun 2017 10:16:01 +0200
Subject: [PATCH 006/269] Fixed decoder doing more iterations than necessary

---
 lib/include/srslte/phy/fec/turbodecoder.h |  4 --
 lib/src/phy/fec/test/turbodecoder_test.c  | 10 ++-
 lib/src/phy/fec/turbodecoder.c            | 24 +++----
 lib/src/phy/fec/turbodecoder_avx.c        |  4 +-
 lib/src/phy/fec/turbodecoder_simd.c       | 88 ++++++++++++++++-------
 lib/src/phy/phch/sch.c                    | 64 ++++++++---------
 6 files changed, 114 insertions(+), 80 deletions(-)

diff --git a/lib/include/srslte/phy/fec/turbodecoder.h b/lib/include/srslte/phy/fec/turbodecoder.h
index 24e38d09e..d42a20ae6 100644
--- a/lib/include/srslte/phy/fec/turbodecoder.h
+++ b/lib/include/srslte/phy/fec/turbodecoder.h
@@ -102,17 +102,14 @@ SRSLTE_API int srslte_tdec_run_all(srslte_tdec_t * h,
 
 SRSLTE_API void srslte_tdec_iteration_par(srslte_tdec_t * h, 
                                           int16_t* input[SRSLTE_TDEC_NPAR], 
-                                          uint32_t nof_cb,
                                           uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_decision_par(srslte_tdec_t * h, 
                                          uint8_t *output[SRSLTE_TDEC_NPAR], 
-                                         uint32_t nof_cb,
                                          uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_decision_byte_par(srslte_tdec_t * h, 
                                               uint8_t *output[SRSLTE_TDEC_NPAR], 
-                                              uint32_t nof_cb,
                                               uint32_t long_cb); 
 
 SRSLTE_API void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, 
@@ -124,7 +121,6 @@ SRSLTE_API int srslte_tdec_run_all_par(srslte_tdec_t * h,
                                        int16_t * input[SRSLTE_TDEC_NPAR], 
                                        uint8_t *output[SRSLTE_TDEC_NPAR],
                                        uint32_t nof_iterations, 
-                                       uint32_t nof_cb,
                                        uint32_t long_cb);
 
 #endif
diff --git a/lib/src/phy/fec/test/turbodecoder_test.c b/lib/src/phy/fec/test/turbodecoder_test.c
index 839bc4202..df1d4b884 100644
--- a/lib/src/phy/fec/test/turbodecoder_test.c
+++ b/lib/src/phy/fec/test/turbodecoder_test.c
@@ -258,13 +258,17 @@ int main(int argc, char **argv) {
       uint8_t *output[SRSLTE_TDEC_NPAR];        
       
       for (int n=0;n<SRSLTE_TDEC_NPAR;n++) {
-        input[n] = llr_s;         
-        output[n] = data_rx_bytes[n]; 
+        if (n < nof_cb) {
+          input[n] = llr_s;         
+        } else {
+          input[n] = NULL; 
+        }
+        output[n] = data_rx_bytes[n];           
       }
 
       gettimeofday(&tdata[1], NULL); 
       for (int k=0;k<nof_repetitions;k++) { 
-        srslte_tdec_run_all_par(&tdec, input, output, t, nof_cb, frame_length);        
+        srslte_tdec_run_all_par(&tdec, input, output, t, frame_length);        
       }
       gettimeofday(&tdata[2], NULL);
       get_time_interval(tdata);
diff --git a/lib/src/phy/fec/turbodecoder.c b/lib/src/phy/fec/turbodecoder.c
index 3eb71bb29..b19466a53 100644
--- a/lib/src/phy/fec/turbodecoder.c
+++ b/lib/src/phy/fec/turbodecoder.c
@@ -91,9 +91,9 @@ int srslte_tdec_get_nof_iterations_cb(srslte_tdec_t * h, uint32_t cb_idx)
 #endif  
 }
 
-void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
+void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_simd_iteration(&h->tdec_simd, input, nof_cb, long_cb);      
+  srslte_tdec_simd_iteration(&h->tdec_simd, input, long_cb);      
 #else
   srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
   srslte_tdec_gen_iteration(&h->tdec_gen, h->input_conv, long_cb);
@@ -103,12 +103,12 @@ void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPA
 void srslte_tdec_iteration(srslte_tdec_t * h, int16_t* input, uint32_t long_cb) {
   int16_t *input_par[SRSLTE_TDEC_NPAR]; 
   input_par[0] = input; 
-  return srslte_tdec_iteration_par(h, input_par, 1, long_cb);
+  return srslte_tdec_iteration_par(h, input_par, long_cb);
 }
 
-void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
+void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_simd_decision(&h->tdec_simd, output, nof_cb, long_cb);
+  return srslte_tdec_simd_decision(&h->tdec_simd, output, long_cb);
 #else
   return srslte_tdec_gen_decision(&h->tdec_gen, output[0], long_cb);
 #endif
@@ -117,12 +117,12 @@ void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPA
 void srslte_tdec_decision(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
   uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
   output_par[0] = output; 
-  srslte_tdec_decision_par(h, output_par, 1, long_cb);
+  srslte_tdec_decision_par(h, output_par, long_cb);
 }
 
-void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb) {
+void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  srslte_tdec_simd_decision_byte(&h->tdec_simd, output, nof_cb, long_cb);  
+  srslte_tdec_simd_decision_byte(&h->tdec_simd, output, long_cb);  
 #else
   srslte_tdec_gen_decision_byte(&h->tdec_gen, output[0], long_cb);
 #endif  
@@ -139,14 +139,14 @@ void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, uint8_t *output, uint32
 void srslte_tdec_decision_byte(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
   uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
   output_par[0] = output; 
-  srslte_tdec_decision_byte_par(h, output_par, 1, long_cb);
+  srslte_tdec_decision_byte_par(h, output_par, long_cb);
 }
 
 int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_NPAR], 
                             uint8_t *output[SRSLTE_TDEC_NPAR], 
-                            uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb) {
+                            uint32_t nof_iterations, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, nof_cb, long_cb);  
+  return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, long_cb);  
 #else
   srslte_vec_convert_if(input[0], h->input_conv, 0.01, 3*long_cb+12);
   return srslte_tdec_gen_run_all(&h->tdec_gen, h->input_conv, output[0], nof_iterations, long_cb);
@@ -160,7 +160,7 @@ int srslte_tdec_run_all(srslte_tdec_t * h, int16_t * input, uint8_t *output, uin
   int16_t *input_par[SRSLTE_TDEC_NPAR]; 
   input_par[0] = input; 
  
-  return srslte_tdec_run_all_par(h, input_par, output_par, nof_iterations, 1, long_cb);
+  return srslte_tdec_run_all_par(h, input_par, output_par, nof_iterations, long_cb);
 }
 
 
diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
index e25f3a1e7..2a2f6f925 100644
--- a/lib/src/phy/fec/turbodecoder_avx.c
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -175,8 +175,8 @@ void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t lo
     bn = _mm256_add_epi16(bn, alpha_k);\
     bn = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bn);\
     bp = _mm256_sub_epi16(_mm256_set1_epi16(0x7FFF), bp);\
-    if (output[0]) output[0][k-d] = hMax0(bn) - hMax0(bp);\
-    if (output[1]) output[1][k-d] = hMax1(bn) - hMax1(bp);
+    output[0][k-d] = hMax0(bn) - hMax0(bp);\
+    output[1][k-d] = hMax1(bn) - hMax1(bp);
 
   /* The tail does not require to load alpha or produce outputs. Only update 
    * beta metrics accordingly */
diff --git a/lib/src/phy/fec/turbodecoder_simd.c b/lib/src/phy/fec/turbodecoder_simd.c
index 24f550f08..e245c84a4 100644
--- a/lib/src/phy/fec/turbodecoder_simd.c
+++ b/lib/src/phy/fec/turbodecoder_simd.c
@@ -129,19 +129,38 @@ void map_simd_free(map_gen_t * h)
 
 /* Runs one instance of a decoder */
 void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_NPAR], int16_t *app[SRSLTE_TDEC_NPAR], int16_t * parity[SRSLTE_TDEC_NPAR], 
-                  int16_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+                  int16_t *output[SRSLTE_TDEC_NPAR], uint32_t cb_mask, uint32_t long_cb)
 {
   
+  uint32_t nof_cb = 1;
+  int16_t *outptr[SRSLTE_TDEC_NPAR];
+  
   // Compute branch metrics
-  for (int i=0;i<nof_cb;i++) {    
-    map_simd_gamma(h, input[i], app?app[i]:NULL, parity[i], i, nof_cb, long_cb);    
+  switch(cb_mask) {
+    case 1:
+      nof_cb = 1; 
+      outptr[0] = output[0];
+      map_simd_gamma(h, input[0], app?app[0]:NULL, parity[0], 0, 1, long_cb);    
+      break;
+    case 2:
+      nof_cb = 1; 
+      outptr[0] = output[1];
+      map_simd_gamma(h, input[1], app?app[1]:NULL, parity[1], 0, 1, long_cb);    
+      break;
+    case 3:
+      nof_cb = 2; 
+      for (int i=0;i<2;i++) {    
+        outptr[i] = output[i];
+        map_simd_gamma(h, input[i], app?app[i]:NULL, parity[i], i, 2, long_cb);    
+      }
+      break;
   }
 
   // Forward recursion
   map_simd_alpha(h, nof_cb, long_cb);
 
   // Backwards recursion + LLR computation
-  map_simd_beta(h, output, nof_cb, long_cb);
+  map_simd_beta(h, outptr, nof_cb, long_cb);
 }
 
 /* Initializes the turbo decoder object */
@@ -204,6 +223,7 @@ int srslte_tdec_simd_init(srslte_tdec_simd_t * h, uint32_t max_par_cb, uint32_t
     srslte_tc_interl_LTE_gen(&h->interleaver[i], srslte_cbsegm_cbsize(i));
   }
   h->current_cbidx = -1; 
+  h->cb_mask = 0; 
   ret = 0;
 clean_and_exit:if (ret == -1) {
     srslte_tdec_simd_free(h);
@@ -334,7 +354,7 @@ void deinterleave_input_simd(srslte_tdec_simd_t *h, int16_t *input, uint32_t cbi
 }
 
 /* Runs 1 turbo decoder iteration */
-void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint32_t long_cb)
 {
 
   int16_t *tmp_app[SRSLTE_TDEC_NPAR];
@@ -343,47 +363,64 @@ void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_T
     uint16_t *inter   = h->interleaver[h->current_cbidx].forward;
     uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
     
-    for (int i=0;i<nof_cb;i++) {
+#if SRSLTE_TDEC_NPAR == 2
+    h->cb_mask = (input[0]?1:0) | (input[1]?2:0);
+#else
+    h->cb_mask = input[0]?1:0;
+#endif
+    
+    for (int i=0;i<h->max_par_cb;i++) {
       if (h->n_iter[i] == 0 && input[i]) {
+        //printf("deinterleaveing %d\n",i);
         deinterleave_input_simd(h, input[i], i, long_cb);        
       }        
     }
     
     // Add apriori information to decoder 1 
-    for (int i=0;i<nof_cb;i++) {
-      if (h->n_iter[i] > 0) {
+    for (int i=0;i<h->max_par_cb;i++) {
+      if (h->n_iter[i] > 0 && input[i]) {
         srslte_vec_sub_sss(h->app1[i], h->ext1[i], h->app1[i], long_cb);
       }
     }
 
     // Run MAP DEC #1
     for (int i=0;i<h->max_par_cb;i++) {
-      tmp_app[i] = h->n_iter[i]?h->app1[i]:NULL;
+      if (input[i]) {
+        tmp_app[i] = h->n_iter[i]?h->app1[i]:NULL;
+      } else {
+        tmp_app[i] = NULL; 
+      }
     }
-    map_simd_dec(&h->dec, h->syst, tmp_app, h->parity0, h->ext1, nof_cb, long_cb);            
+    map_simd_dec(&h->dec, h->syst, tmp_app, h->parity0, h->ext1, h->cb_mask, long_cb);            
     
     // Convert aposteriori information into extrinsic information    
-    for (int i=0;i<nof_cb;i++) {
-      if (h->n_iter[i] > 0) {
+    for (int i=0;i<h->max_par_cb;i++) {
+      if (h->n_iter[i] > 0 && input[i]) {
         srslte_vec_sub_sss(h->ext1[i], h->app1[i], h->ext1[i], long_cb);
       }
     }
     
     // Interleave extrinsic output of DEC1 to form apriori info for decoder 2
-    for (int i=0;i<nof_cb;i++) {
-      srslte_vec_lut_sss(h->ext1[i], deinter, h->app2[i], long_cb);
+    for (int i=0;i<h->max_par_cb;i++) {
+      if (input[i]) {
+        srslte_vec_lut_sss(h->ext1[i], deinter, h->app2[i], long_cb);
+      }
     }
 
     // Run MAP DEC #2. 2nd decoder uses apriori information as systematic bits
-    map_simd_dec(&h->dec, h->app2, NULL, h->parity1, h->ext2, nof_cb, long_cb);
+    map_simd_dec(&h->dec, h->app2, NULL, h->parity1, h->ext2, h->cb_mask, long_cb);
 
     // Deinterleaved extrinsic bits become apriori info for decoder 1 
-    for (int i=0;i<nof_cb;i++) {
-      srslte_vec_lut_sss(h->ext2[i], inter, h->app1[i], long_cb);
+    for (int i=0;i<h->max_par_cb;i++) {
+      if (input[i]) {
+        srslte_vec_lut_sss(h->ext2[i], inter, h->app1[i], long_cb);
+      }
     }
 
     for (int i=0;i<h->max_par_cb;i++) {
-      h->n_iter[i]++;     
+      if (input[i]) {
+        h->n_iter[i]++;     
+      }
     }
   } else {
     fprintf(stderr, "Error CB index not set (call srslte_tdec_simd_reset() first\n");    
@@ -401,6 +438,7 @@ int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, uint32_t long_cb)
   for (int i=0;i<h->max_par_cb;i++) {
     h->n_iter[i] = 0;     
   }
+  h->cb_mask = 0; 
   h->current_cbidx = srslte_cbsegm_cbindex(long_cb);
   if (h->current_cbidx < 0) {
     fprintf(stderr, "Invalid CB length %d\n", long_cb);
@@ -446,9 +484,9 @@ void tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx,
   }
 }
 
-void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
 {
-  for (int i=0;i<nof_cb;i++) {
+  for (int i=0;i<h->max_par_cb;i++) {    
     tdec_simd_decision(h, output[i], i, long_cb);
   }
 }
@@ -472,9 +510,9 @@ void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h, uint8_t *output,
   }
 }
 
-void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
 {
-  for (int i=0;i<nof_cb;i++) {
+  for (int i=0;i<h->max_par_cb;i++) {
     srslte_tdec_simd_decision_byte_cb(h, output[i], i, long_cb);
   }
 }
@@ -482,17 +520,17 @@ void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSL
 
 /* Runs nof_iterations iterations and decides the output bits */
 int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint8_t *output[SRSLTE_TDEC_NPAR],
-                            uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb)
+                            uint32_t nof_iterations, uint32_t long_cb)
 {
   if (srslte_tdec_simd_reset(h, long_cb)) {
     return SRSLTE_ERROR; 
   }
 
   do {
-    srslte_tdec_simd_iteration(h, input, nof_cb, long_cb);
+    srslte_tdec_simd_iteration(h, input, long_cb);
   } while (h->n_iter[0] < nof_iterations);
 
-  srslte_tdec_simd_decision_byte(h, output, nof_cb, long_cb);
+  srslte_tdec_simd_decision_byte(h, output, long_cb);
   
   return SRSLTE_SUCCESS;
 }
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index f08054988..dceab2506 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -321,7 +321,6 @@ bool decode_tb_cb(srslte_sch_t *q,
   bool cb_map[SRSLTE_MAX_CODEBLOCKS];
     
   uint32_t cb_idx[SRSLTE_TDEC_NPAR];
-  bool cb_in_use[SRSLTE_TDEC_NPAR];
   int16_t *decoder_input[SRSLTE_TDEC_NPAR];
   
   uint32_t nof_cb     = cb_size_group?cb_segm->C2:cb_segm->C1;
@@ -340,9 +339,8 @@ bool decode_tb_cb(srslte_sch_t *q,
   }
   
   for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
-    cb_idx[i]    = i+first_cb; 
-    cb_in_use[i] = false; 
-    decoder_input[i] = NULL; 
+    cb_idx[i]        = i+first_cb; 
+    decoder_input[i] = false; 
   }
   
   for (int i=0;i<nof_cb;i++) {
@@ -354,49 +352,46 @@ bool decode_tb_cb(srslte_sch_t *q,
   uint32_t remaining_cb = nof_cb; 
   
   while(remaining_cb>0) {
-    uint32_t npar = SRSLTE_MIN(remaining_cb, SRSLTE_TDEC_NPAR);
         
     // Unratematch the codeblocks left to decode 
-    for (int i=0;i<npar;i++) {
+    for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
       
-      if (!cb_in_use[i] && i < remaining_cb) {        
+      if (!decoder_input[i] && remaining_cb > 0) {        
         // Find an unprocessed CB 
         cb_idx[i]=first_cb;
         while(cb_idx[i]<first_cb+nof_cb-1 && cb_map[cb_idx[i]]) {
           cb_idx[i]++;
         }
-        cb_in_use[i]      = true; 
-        cb_map[cb_idx[i]] = true; 
-        
-        uint32_t rp   = cb_idx[i]*n_e;  
-        uint32_t n_e2 = n_e;
-        
-        if (cb_idx[i] > cb_segm->C - gamma) {
-          n_e2 = n_e+Qm;
-          rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
-        }
-              
-        INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
-        if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
-          fprintf(stderr, "Error in rate matching\n");
-          return SRSLTE_ERROR;
+        if (cb_map[cb_idx[i]] == false) {
+          cb_map[cb_idx[i]] = true; 
+          
+          uint32_t rp   = cb_idx[i]*n_e;  
+          uint32_t n_e2 = n_e;
+          
+          if (cb_idx[i] > cb_segm->C - gamma) {
+            n_e2 = n_e+Qm;
+            rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
+          }
+                
+          INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
+          if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
+            fprintf(stderr, "Error in rate matching\n");
+            return SRSLTE_ERROR;
+          }
+
+          decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];          
         }
-
-        decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
       }
     }
-    
+        
     // Run 1 iteration for up to TDEC_NPAR codeblocks 
-    if (SRSLTE_TDEC_NPAR > 1) {
-      INFO("Processing %d CBs, index %d,%d\n", npar, cb_idx[0], cb_idx[1]);      
-    }
-    srslte_tdec_iteration_par(&q->decoder, decoder_input, npar, cb_len);
+    srslte_tdec_iteration_par(&q->decoder, decoder_input, cb_len);
 
     q->nof_iterations = srslte_tdec_get_nof_iterations_cb(&q->decoder, 0); 
     
     // Decide output bits and compute CRC 
-    for (int i=0;i<npar;i++) {
-      if (cb_in_use[i]) {        
+    for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+      if (decoder_input[i]) {        
         srslte_tdec_decision_byte_par_cb(&q->decoder, q->cb_in, i, cb_len);
 
         uint32_t len_crc; 
@@ -418,13 +413,14 @@ bool decode_tb_cb(srslte_sch_t *q,
           // Reset number of iterations for that CB in the decoder 
           srslte_tdec_reset_cb(&q->decoder, i);
           remaining_cb--;        
-          cb_in_use[i] = false; 
+          decoder_input[i] = NULL; 
+          cb_idx[i] = 0; 
           
         // CRC is error and exceeded maximum iterations for this CB. 
         // Early stop the whole transport block.
         } else if (srslte_tdec_get_nof_iterations_cb(&q->decoder, i) >= q->max_iterations) {
-          INFO("CB %d: Error. CB is erroneous. remaining_cb=%d, i=%d, first_cb=%d, nof_cb=%d, npar=%d\n", 
-                cb_idx[i], remaining_cb, i, first_cb, nof_cb, npar);          
+          INFO("CB %d: Error. CB is erroneous. remaining_cb=%d, i=%d, first_cb=%d, nof_cb=%d\n", 
+                cb_idx[i], remaining_cb, i, first_cb, nof_cb);          
           return false; 
         }
       }

From c47c4b8b03fdd974c9eac1e504bbc0fe423d496b Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 21 Jun 2017 11:22:10 +0200
Subject: [PATCH 007/269] fixed overflow in tests

---
 lib/src/phy/phch/test/pdsch_test.c | 2 +-
 lib/src/phy/phch/test/pusch_test.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 8926e3bd3..d4e5163d3 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -171,7 +171,7 @@ int main(int argc, char **argv) {
     }
   }
   
-  data = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs/8);
+  data = srslte_vec_malloc(sizeof(uint8_t) * (grant.mcs.tbs/8)+24);
   if (!data) {
     perror("srslte_vec_malloc");
     goto quit;
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index 7cf80a7d1..0cb2847b7 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -188,7 +188,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }
   
-  data = srslte_vec_malloc(sizeof(uint8_t) * cfg.grant.mcs.tbs);
+  data = srslte_vec_malloc(sizeof(uint8_t) * (cfg.grant.mcs.tbs+24));
   if (!data) {
     perror("malloc");
     exit(-1);

From 4b5cbafdb5e8afdafc7492f9b70d7202cfcfd98e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 21 Jun 2017 18:29:17 +0200
Subject: [PATCH 008/269] upgraded to new code restruct

---
 lib/include/srslte/interfaces/ue_interfaces.h |  31 +-
 lib/src/upper/gw.cc                           |   4 +-
 srsue/hdr/phy/phch_recv.h                     |  19 +-
 srsue/hdr/phy/phy.h                           |  17 +-
 srsue/hdr/upper/nas.h                         | 220 +++--
 srsue/hdr/upper/rrc.h                         | 384 ++++----
 srsue/src/mac/mac.cc                          |  31 +-
 srsue/src/phy/phch_recv.cc                    | 856 +++++++++---------
 srsue/src/phy/phy.cc                          |  28 +-
 srsue/src/ue.cc                               |   7 +-
 srsue/src/upper/nas.cc                        |  88 +-
 srsue/src/upper/rrc.cc                        | 611 ++++++++-----
 srsue/test/phy/ue_itf_test_prach.cc           |   4 +-
 srsue/test/phy/ue_itf_test_sib1.cc            |   8 +-
 srsue/test/upper/CMakeLists.txt               |  11 -
 srsue/test/upper/ip_test.cc                   | 645 -------------
 16 files changed, 1331 insertions(+), 1633 deletions(-)
 delete mode 100644 srsue/test/upper/ip_test.cc

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 62b2a7ef7..cc84c992f 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -101,6 +101,16 @@ public:
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
+  virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
+  virtual void      cell_selected() = 0;
+};
+
+// NAS interface for UE
+class nas_interface_ue
+{
+public:
+  virtual void      attach_request() = 0;
+  virtual void      deattach_request() = 0;
 };
 
 // RRC interface for MAC
@@ -117,6 +127,7 @@ class rrc_interface_phy
 public:
   virtual void in_sync() = 0;
   virtual void out_of_sync() = 0;
+  virtual void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) = 0;
 };
 
 // RRC interface for NAS
@@ -127,14 +138,18 @@ public:
   virtual uint16_t get_mcc() = 0;
   virtual uint16_t get_mnc() = 0;
   virtual void enable_capabilities() = 0;
+  virtual void plmn_search() = 0;
+  virtual void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) = 0;
+  virtual void connect() = 0;
+
 };
 
 // RRC interface for GW
 class rrc_interface_gw
 {
 public:
-  virtual bool rrc_connected() = 0;
-  virtual void rrc_connect() = 0; 
+  virtual bool is_connected() = 0;
+  virtual void connect() = 0;
   virtual bool have_drb() = 0;
 };
 
@@ -400,8 +415,7 @@ public:
   virtual void configure_prach_params() = 0;
   
   /* Start synchronization with strongest cell in the current carrier frequency */
-  virtual void sync_start() = 0; 
-  virtual void sync_stop() = 0;
+  virtual bool sync_status() = 0;
   
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
   virtual void set_crnti(uint16_t rnti) = 0; 
@@ -462,9 +476,14 @@ public:
   virtual void set_config_common(phy_cfg_common_t *common) = 0; 
   virtual void set_config_tdd(LIBLTE_RRC_TDD_CONFIG_STRUCT *tdd) = 0; 
   virtual void set_config_64qam_en(bool enable) = 0;
-  
+
+  /* Cell search and selection procedures */
+  virtual void cell_search_start() = 0;
+  virtual void cell_search_next() = 0;
+  virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
+
   /* Is the PHY downlink synchronized? */
-  virtual bool status_is_sync() = 0;
+  virtual bool sync_status() = 0;
 
   /* Configure UL using parameters written with set_param() */
   virtual void configure_ul_params(bool pregen_disabled = false) = 0;
diff --git a/lib/src/upper/gw.cc b/lib/src/upper/gw.cc
index beb003c51..4eba4b4d6 100644
--- a/lib/src/upper/gw.cc
+++ b/lib/src/upper/gw.cc
@@ -249,8 +249,8 @@ void gw::run_thread()
           {
             gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
 
-            while(run_enable && (!rrc->rrc_connected() || !rrc->have_drb())) {
-              rrc->rrc_connect();
+            while(run_enable && (!rrc->is_connected() || !rrc->have_drb())) {
+              rrc->connect();
               usleep(1000);
             }
             
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 838d49d30..59fa10ebc 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -51,12 +51,16 @@ public:
   void stop();
   void set_agc_enable(bool enable);
 
-  void     resync_sfn(); 
-  
+  void     resync_sfn();
+
+  void    set_earfcn(std::vector<uint32_t> earfcn);
+
+  void    cell_search_start();
+  void    cell_search_next();
+  bool    cell_select(uint32_t earfcn, srslte_cell_t cell);
+
   uint32_t get_current_tti();
   
-  void    sync_start(); 
-  void    sync_stop();
   bool    status_is_sync();
 
   void    set_time_adv_sec(float time_adv_sec);
@@ -65,7 +69,9 @@ public:
   const static int MUTEX_X_WORKER = 4; 
 
 private:
-  
+
+  std::vector<uint32_t> earfcn;
+
   void   set_ue_sync_opts(srslte_ue_sync_t *q); 
   void   run_thread();
   int    sync_sfn();
@@ -91,7 +97,7 @@ private:
   sync_metrics_t metrics;
 
   enum {
-    IDLE, CELL_SEARCH, SYNCING, SYNC_DONE
+    IDLE, CELL_SEARCH, CELL_SELECT, CAMPING
   } phy_state; 
 
   srslte_cell_t cell; 
@@ -111,6 +117,7 @@ private:
   uint32_t      sync_sfn_cnt;
   const static uint32_t SYNC_SFN_TIMEOUT = 5000;
   float ul_dl_factor;
+  int cur_earfcn_index;
   
   bool          cell_search(int force_N_id_2 = -1);
   bool          init_cell();
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index b478bbcdd..a2a234535 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -69,18 +69,21 @@ public:
   void enable_pregen_signals(bool enable); 
   
   void start_trace();
-  void write_trace(std::string filename); 
-  
+  void write_trace(std::string filename);
+
+  void set_earfcn(std::vector<uint32_t> earfcns);
+
   /********** RRC INTERFACE ********************/
   void    reset();
-  bool    status_is_sync();
   void    configure_ul_params(bool pregen_disabled = false);
-  void    resync_sfn(); 
-  
+  void    resync_sfn();
+  void    cell_search_start();
+  void    cell_search_next();
+  bool    cell_select(uint32_t earfcn, srslte_cell_t phy_cell);
+
   /********** MAC INTERFACE ********************/
   /* Functions to synchronize with a cell */
-  void    sync_start(); 
-  void    sync_stop();
+  bool    sync_status(); // this is also RRC interface
 
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
   void set_crnti(uint16_t rnti);
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 5e3a8b098..e21bff432 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -39,7 +39,7 @@ using srslte::byte_buffer_t;
 namespace srsue {
 
 // EMM states (3GPP 24.302 v10.0.0)
-typedef enum{
+  typedef enum {
     EMM_STATE_NULL = 0,
     EMM_STATE_DEREGISTERED,
     EMM_STATE_REGISTERED_INITIATED,
@@ -48,97 +48,133 @@ typedef enum{
     EMM_STATE_DEREGISTERED_INITIATED,
     EMM_STATE_TAU_INITIATED,
     EMM_STATE_N_ITEMS,
-}emm_state_t;
-static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
-                                                            "DEREGISTERED",
-                                                            "REGISTERED INITIATED",
-                                                            "REGISTERED",
-                                                            "SERVICE REQUEST INITIATED",
-                                                            "DEREGISTERED INITIATED",
-                                                            "TRACKING AREA UPDATE INITIATED"};
-
-class nas
-    :public nas_interface_rrc
-{
-public:
-  nas();
-  void init(usim_interface_nas  *usim_,
-            rrc_interface_nas   *rrc_,
-            gw_interface_nas    *gw_,
-            srslte::log         *nas_log_);
-  void stop();
-
-  emm_state_t get_state();
-
-  // RRC interface
-  void      notify_connection_setup();
-  void      write_pdu(uint32_t lcid, byte_buffer_t *pdu);
-  uint32_t  get_ul_count();
-  bool      is_attached();
-  bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
-  
-private:
-  srslte::byte_buffer_pool *pool;
-  srslte::log        *nas_log;
-  rrc_interface_nas  *rrc;
-  usim_interface_nas *usim;
-  gw_interface_nas   *gw;
-
-  emm_state_t        state;
-   
-  // Save short MAC
-  
-  // Identifiers
-  LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
-  bool                                 is_guti_set;
-
-  uint32_t ip_addr;
-  uint8_t  eps_bearer_id;
-
-  uint8_t  transaction_id;
-
-  // NAS counters - incremented for each security-protected message recvd/sent
-  uint32_t count_ul;
-  uint32_t count_dl;
-
-  // Security
-  uint8_t  ksi;
-  uint8_t  k_nas_enc[32];
-  uint8_t  k_nas_int[32];
-
-  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
-  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
-
-  void integrity_generate(uint8_t  *key_128,
-                          uint32_t  count,
-                          uint8_t   rb_id,
-                          uint8_t   direction,
-                          uint8_t  *msg,
-                          uint32_t  msg_len,
-                          uint8_t  *mac);
-  void integrity_check();
-  void cipher_encrypt();
-  void cipher_decrypt();
-
-  // Parsers
-  void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
-
-  // Senders
-  void send_attach_request();
-  void send_identity_response();
-  void send_service_request();
-  void send_esm_information_response();
-
-  void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
-};
+  } emm_state_t;
+  static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
+                                                              "DEREGISTERED",
+                                                              "REGISTERED INITIATED",
+                                                              "REGISTERED",
+                                                              "SERVICE REQUEST INITIATED",
+                                                              "DEREGISTERED INITIATED",
+                                                              "TRACKING AREA UPDATE INITIATED"};
+
+  typedef enum {
+    PLMN_NOT_SELECTED = 0,
+    PLMN_SELECTED
+  } plmn_selection_state_t;
+
+  class nas
+    : public nas_interface_rrc, public nas_interface_ue {
+  public:
+    nas();
+
+    void init(usim_interface_nas *usim_,
+              rrc_interface_nas *rrc_,
+              gw_interface_nas *gw_,
+              srslte::log *nas_log_);
+
+    void stop();
+
+    emm_state_t get_state();
+
+    // RRC interface
+    void notify_connection_setup();
+
+    void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+
+    uint32_t get_ul_count();
+
+    bool is_attached();
+
+    bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
+
+    void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
+
+    void cell_selected();
+
+    // UE interface
+    void attach_request();
+
+    void deattach_request();
+
+  private:
+    srslte::byte_buffer_pool *pool;
+    srslte::log *nas_log;
+    rrc_interface_nas *rrc;
+    usim_interface_nas *usim;
+    gw_interface_nas *gw;
+
+    emm_state_t state;
+
+    plmn_selection_state_t plmn_selection;
+    LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
+
+    // Save short MAC
+
+    // Identifiers
+    LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
+    bool is_guti_set;
+
+    uint32_t ip_addr;
+    uint8_t eps_bearer_id;
+
+    uint8_t transaction_id;
+
+    // NAS counters - incremented for each security-protected message recvd/sent
+    uint32_t count_ul;
+    uint32_t count_dl;
+
+    // Security
+    uint8_t ksi;
+    uint8_t k_nas_enc[32];
+    uint8_t k_nas_int[32];
+
+    srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
+    srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
+
+    void integrity_generate(uint8_t *key_128,
+                            uint32_t count,
+                            uint8_t rb_id,
+                            uint8_t direction,
+                            uint8_t *msg,
+                            uint32_t msg_len,
+                            uint8_t *mac);
+
+    void integrity_check();
+
+    void cipher_encrypt();
+
+    void cipher_decrypt();
+
+    // Parsers
+    void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
+
+    // Senders
+    void send_attach_request();
+
+    void send_identity_response();
+
+    void send_service_request();
+
+    void send_esm_information_response();
+
+    void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
+  };
 
 } // namespace srsue
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index cc1a22fd1..f33043d10 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -34,6 +34,7 @@
 #include "srslte/common/common.h"
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/security.h"
+#include "srslte/common/threads.h"
 
 #include <map>
 
@@ -42,168 +43,231 @@ using srslte::byte_buffer_t;
 namespace srsue {
 
 // RRC states (3GPP 36.331 v10.0.0)
-typedef enum{
+  typedef enum {
     RRC_STATE_IDLE = 0,
-    RRC_STATE_SIB1_SEARCH,
-    RRC_STATE_SIB2_SEARCH,
-    RRC_STATE_WAIT_FOR_CON_SETUP,
-    RRC_STATE_COMPLETING_SETUP,
-    RRC_STATE_RRC_CONNECTED,
+    RRC_STATE_PLMN_SELECTION,
+    RRC_STATE_CELL_SELECTING,
+    RRC_STATE_CELL_SELECTED,
+    RRC_STATE_CONNECTING,
+    RRC_STATE_CONNECTED,
     RRC_STATE_N_ITEMS,
-}rrc_state_t;
-static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
-                                                            "SIB1_SEARCH",
-                                                            "SIB2_SEARCH",
-                                                            "WAIT FOR CON SETUP",
-                                                            "COMPLETING SETUP",
-                                                            "RRC CONNECTED"};
-
-
-class rrc
-    :public rrc_interface_nas
-    ,public rrc_interface_phy
-    ,public rrc_interface_mac
-    ,public rrc_interface_gw
-    ,public rrc_interface_pdcp
-    ,public rrc_interface_rlc
-    ,public srslte::timer_callback
-{
-public:
-  rrc();
-  void init(phy_interface_rrc     *phy_,
-            mac_interface_rrc     *mac_,
-            rlc_interface_rrc     *rlc_,
-            pdcp_interface_rrc    *pdcp_,
-            nas_interface_rrc     *nas_,
-            usim_interface_rrc    *usim_,
-            srslte::mac_interface_timers *mac_timers_,
-            srslte::log           *rrc_log_);
-  void stop();
-
-  rrc_state_t get_state();
-  void set_ue_category(int category); 
-
-  // Timeout callback interface
-  void timer_expired(uint32_t timeout_id);
-
-  void test_con_restablishment(); 
-  void liblte_rrc_log(char* str);
-  
-private:
-  srslte::byte_buffer_pool  *pool;
-  srslte::log          *rrc_log;
-  phy_interface_rrc    *phy;
-  mac_interface_rrc    *mac;
-  rlc_interface_rrc    *rlc;
-  pdcp_interface_rrc   *pdcp;
-  nas_interface_rrc    *nas;
-  usim_interface_rrc   *usim;
-
-  srslte::bit_buffer_t  bit_buf;
-
-  pthread_mutex_t       mutex;
-  
-  rrc_state_t           state;
-  uint8_t               transaction_id;
-  bool                  drb_up;
-
-  uint8_t               k_rrc_enc[32];
-  uint8_t               k_rrc_int[32];
-  uint8_t               k_up_enc[32];
-  uint8_t               k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
-
-  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
-  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
-
-  LIBLTE_RRC_MIB_STRUCT                                 mib;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT               sib1;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT               sib2;
-
-  std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT>  srbs;
-  std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT>  drbs;
-
-  LIBLTE_RRC_DL_CCCH_MSG_STRUCT                         dl_ccch_msg;
-  LIBLTE_RRC_DL_DCCH_MSG_STRUCT                         dl_dcch_msg;
-
-  pthread_t             sib_search_thread;
-
-  // RRC constants and timers 
-  srslte::mac_interface_timers *mac_timers;
-  uint32_t n310_cnt, N310; 
-  uint32_t n311_cnt, N311; 
-  uint32_t t301, t310, t311;
-  uint32_t safe_reset_timer; 
-  int ue_category;
-    
-  
-  // NAS interface
-  void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
-  uint16_t get_mcc();
-  uint16_t get_mnc();
-  void enable_capabilities();
-
-  // PHY interface
-  void in_sync();
-  void out_of_sync();
-
-  // MAC interface
-  void release_pucch_srs();
-  void ra_problem(); 
-
-  // GW interface
-  bool rrc_connected();
-  void rrc_connect(); 
-  bool have_drb();
-
-  // PDCP interface
-  void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
-  void write_pdu_bcch_bch(byte_buffer_t *pdu);
-  void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
-  void write_pdu_pcch(byte_buffer_t *pdu);
-
-  // RLC interface
-  void max_retx_attempted();
-
-  // Senders
-  void send_con_request();
-  void send_con_restablish_request(); 
-  void send_con_restablish_complete();
-  void send_con_setup_complete(byte_buffer_t *nas_msg);
-  void send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
-  void send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
-  void send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
-  void send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
-
-  // Parsers
-  void parse_dl_ccch(byte_buffer_t *pdu);
-  void parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
-  void parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
-
-  // Helpers
-  void          reset_ue(); 
-  void          rrc_connection_release();
-  void          radio_link_failure(); 
-  static void*  start_sib_thread(void *rrc_);
-  void          sib_search();
-  uint32_t      sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
-  void          apply_sib2_configs();
-  void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
-  void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
-  void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
-  void          add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
-  void          add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
-  void          release_drb(uint8_t lcid);
-  void          apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg);
-  void          apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults); 
-  void          apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cfg, bool apply_defaults); 
-  
-  // Helpers for setting default values 
-  void          set_phy_default_pucch_srs();
-  void          set_phy_default();
-  void          set_mac_default();
-  void          set_rrc_default(); 
-  
-};
+  } rrc_state_t;
+  static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
+                                                              "PLMN SELECTION",
+                                                              "CELL SELECTION",
+                                                              "CONNECTING",
+                                                              "CONNECTED",
+                                                              "RRC CONNECTED"};
+  typedef enum {
+    SI_ACQUIRE_IDLE = 0,
+    SI_ACQUIRE_SIB1,
+    SI_ACQUIRE_SIB2,
+    SI_ACQUIRE_DONE
+  } si_acquire_state_t;
+
+
+  class rrc
+    : public rrc_interface_nas,
+      public rrc_interface_phy,
+      public rrc_interface_mac,
+      public rrc_interface_gw,
+      public rrc_interface_pdcp,
+      public rrc_interface_rlc,
+      public srslte::timer_callback,
+      public thread
+  {
+  public:
+    rrc();
+
+    void init(phy_interface_rrc *phy_,
+              mac_interface_rrc *mac_,
+              rlc_interface_rrc *rlc_,
+              pdcp_interface_rrc *pdcp_,
+              nas_interface_rrc *nas_,
+              usim_interface_rrc *usim_,
+              srslte::mac_interface_timers *mac_timers_,
+              srslte::log *rrc_log_);
+
+    void stop();
+
+    rrc_state_t get_state();
+
+    void set_ue_category(int category);
+
+    // Timeout callback interface
+    void timer_expired(uint32_t timeout_id);
+
+    void test_con_restablishment();
+
+    void liblte_rrc_log(char *str);
+
+  private:
+    srslte::byte_buffer_pool *pool;
+    srslte::log *rrc_log;
+    phy_interface_rrc *phy;
+    mac_interface_rrc *mac;
+    rlc_interface_rrc *rlc;
+    pdcp_interface_rrc *pdcp;
+    nas_interface_rrc *nas;
+    usim_interface_rrc *usim;
+
+    srslte::bit_buffer_t bit_buf;
+
+    pthread_mutex_t mutex;
+
+    rrc_state_t state;
+    uint8_t transaction_id;
+    bool drb_up;
+
+
+    uint8_t k_rrc_enc[32];
+    uint8_t k_rrc_int[32];
+    uint8_t k_up_enc[32];
+    uint8_t k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
+
+    srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
+    srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
+
+    std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT> srbs;
+    std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT> drbs;
+
+    LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
+    LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
+
+    // RRC constants and timers
+    srslte::mac_interface_timers *mac_timers;
+    uint32_t n310_cnt, N310;
+    uint32_t n311_cnt, N311;
+    uint32_t t301, t310, t311;
+    uint32_t safe_reset_timer;
+    int ue_category;
+
+    typedef struct {
+      uint32_t earfcn;
+      srslte_cell_t phy_cell;
+      float    rsrp;
+      bool     has_valid_sib1;
+      bool     has_valid_sib2;
+      LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
+      LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
+    } cell_t;
+
+    std::vector<cell_t> known_cells;
+    cell_t *current_cell;
+
+    si_acquire_state_t si_acquire_state;
+
+    void select_next_cell_in_plmn();
+    LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
+    int last_selected_cell;
+
+    bool thread_running;
+    void run_thread();
+
+    // NAS interface
+    void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
+
+    uint16_t get_mcc();
+
+    uint16_t get_mnc();
+
+    void enable_capabilities();
+    void plmn_search();
+    void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
+    void connect();
+
+    // PHY interface
+    void in_sync();
+
+    void out_of_sync();
+    void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
+
+    // MAC interface
+    void release_pucch_srs();
+
+    void ra_problem();
+
+    // GW interface
+    bool is_connected();
+
+    bool have_drb();
+
+    // PDCP interface
+    void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+
+    void write_pdu_bcch_bch(byte_buffer_t *pdu);
+
+    void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
+
+    void write_pdu_pcch(byte_buffer_t *pdu);
+
+    // RLC interface
+    void max_retx_attempted();
+
+    // Senders
+    void send_con_request();
+
+    void send_con_restablish_request();
+
+    void send_con_restablish_complete();
+
+    void send_con_setup_complete(byte_buffer_t *nas_msg);
+
+    void send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
+
+    void send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
+
+    void send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
+
+    void send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
+
+    // Parsers
+    void parse_dl_ccch(byte_buffer_t *pdu);
+
+    void parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
+
+    void parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
+
+    // Helpers
+    void reset_ue();
+
+    void rrc_connection_release();
+
+    void radio_link_failure();
+
+    uint32_t sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
+
+    void apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
+
+    void handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
+
+    void handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
+
+    void
+    handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
+
+    void add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
+
+    void add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
+
+    void release_drb(uint8_t lcid);
+
+    void apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg);
+
+    void apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults);
+
+    void apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cfg, bool apply_defaults);
+
+    // Helpers for setting default values
+    void set_phy_default_pucch_srs();
+
+    void set_phy_default();
+
+    void set_mac_default();
+
+    void set_rrc_default();
+
+  };
 
 } // namespace srsue
 
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index f583dd2df..316320f31 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -138,24 +138,23 @@ void mac::reset()
 
 void mac::run_thread() {
   int cnt=0;
-  
-  Info("Waiting PHY to synchronize with cell\n");  
-  phy_h->sync_start();
-  while(!phy_h->get_current_tti() && started) {
-    usleep(50000);
-  }
-  Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
-  ttisync.set_producer_cntr(phy_h->get_current_tti());
-     
+
   while(started) {
 
-    /* Warning: Here order of invocation of procedures is important!! */
-    ttisync.wait();
-    tti = phy_h->get_current_tti();
-    
-    if (started) {
-      log_h->step(tti);
+    while (!phy_h->sync_status()) {
+      usleep(5000);
+      if (phy_h->sync_status()) {
+        Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
+        ttisync.set_producer_cntr(phy_h->get_current_tti());
+      }
+    }
 
+    if (started && phy_h->sync_status()) {
+      /* Warning: Here order of invocation of procedures is important!! */
+      ttisync.wait();
+      tti = phy_h->get_current_tti();
+
+      log_h->step(tti);
       timers_db.step_all();
       
       // Step all procedures 
@@ -488,7 +487,7 @@ void mac::upper_timers::reset()
 /********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from
- * DEMU unit
+ * DEMUX unit
  *
  *******************************************************/
 mac::pdu_process::pdu_process(demux *demux_unit_)
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 251bfd144..7895ca66a 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -37,466 +37,510 @@
 #define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 
 namespace srsue {
- 
 
-phch_recv::phch_recv() { 
-  running = false; 
-}
 
-void phch_recv::init(srslte::radio_multi* _radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
-                     prach* _prach_buffer, srslte::thread_pool* _workers_pool,
-                     phch_common* _worker_com, srslte::log* _log_h, uint32_t nof_rx_antennas_, uint32_t prio, int sync_cpu_affinity)
-{
-  radio_h      = _radio_handler;
-  log_h        = _log_h;     
-  mac          = _mac;
-  rrc          = _rrc; 
-  workers_pool = _workers_pool;
-  worker_com   = _worker_com;
-  prach_buffer = _prach_buffer; 
-  nof_rx_antennas = nof_rx_antennas_;
-
-  tx_mutex_cnt = 0; 
-  running      = true; 
-  phy_state    = IDLE; 
-  time_adv_sec = 0; 
-  cell_is_set  = false; 
-  sync_sfn_cnt = 0; 
-  
-  for (uint32_t i=0;i<nof_rx_antennas;i++) {
-    sf_buffer_sfn[i] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*3*SRSLTE_SF_LEN_PRB(100));
-  }
-  
-  nof_tx_mutex = MUTEX_X_WORKER*workers_pool->get_nof_workers();
-  worker_com->set_nof_mutex(nof_tx_mutex);
-  if(sync_cpu_affinity < 0){
-    start(prio);
-  } else {
-    start_cpu(prio, sync_cpu_affinity);
+  phch_recv::phch_recv() {
+    running = false;
   }
-  
-  
-}
 
-void phch_recv::stop() {
-  running = false; 
-  wait_thread_finish();
-  for (uint32_t i=0;i<nof_rx_antennas;i++) {
-    if (sf_buffer_sfn[i]) {
-      free(sf_buffer_sfn[i]);
+  void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
+                       prach *_prach_buffer, srslte::thread_pool *_workers_pool,
+                       phch_common *_worker_com, srslte::log *_log_h, uint32_t nof_rx_antennas_, uint32_t prio,
+                       int sync_cpu_affinity) {
+    radio_h = _radio_handler;
+    log_h = _log_h;
+    mac = _mac;
+    rrc = _rrc;
+    workers_pool = _workers_pool;
+    worker_com = _worker_com;
+    prach_buffer = _prach_buffer;
+    nof_rx_antennas = nof_rx_antennas_;
+
+    tx_mutex_cnt = 0;
+    running = true;
+    phy_state = IDLE;
+    time_adv_sec = 0;
+    cell_is_set = false;
+    sync_sfn_cnt = 0;
+
+    for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+      sf_buffer_sfn[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
     }
-  }
-}
 
-void phch_recv::set_agc_enable(bool enable)
-{
-  do_agc = enable;
-}
+    nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
+    worker_com->set_nof_mutex(nof_tx_mutex);
+    if (sync_cpu_affinity < 0) {
+      start(prio);
+    } else {
+      start_cpu(prio, sync_cpu_affinity);
+    }
 
-int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time)
-{
-  srslte::radio_multi *radio_h = (srslte::radio_multi*) h;
-  if (radio_h->rx_now(data, nsamples, rx_time)) {
-    int offset = nsamples-radio_h->get_tti_len();
-    if (abs(offset)<10 && offset != 0) {
-      radio_h->tx_offset(offset);
-    } else if (nsamples<10) {
-      radio_h->tx_offset(nsamples);
+
+  }
+
+  void phch_recv::stop() {
+    running = false;
+    wait_thread_finish();
+    for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+      if (sf_buffer_sfn[i]) {
+        free(sf_buffer_sfn[i]);
+      }
     }
-    return nsamples;
-  } else {
-    return -1;
   }
-}
 
-double callback_set_rx_gain(void *h, double gain) {
-  srslte::radio_multi *radio_handler = (srslte::radio_multi*) h;
-  return radio_handler->set_rx_gain_th(gain);
-}
+  void phch_recv::set_agc_enable(bool enable) {
+    do_agc = enable;
+  }
 
-void phch_recv::set_time_adv_sec(float _time_adv_sec) {
- time_adv_sec = _time_adv_sec;
-}
+  int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
+    srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
+    if (radio_h->rx_now(data, nsamples, rx_time)) {
+      int offset = nsamples - radio_h->get_tti_len();
+      if (abs(offset) < 10 && offset != 0) {
+        radio_h->tx_offset(offset);
+      } else if (nsamples < 10) {
+        radio_h->tx_offset(nsamples);
+      }
+      return nsamples;
+    } else {
+      return -1;
+    }
+  }
 
-void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
-  if (worker_com->args->cfo_integer_enabled) {
-    srslte_ue_sync_cfo_i_detec_en(q, true); 
+  double callback_set_rx_gain(void *h, double gain) {
+    srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
+    return radio_handler->set_rx_gain_th(gain);
   }
-  
-  float cfo_tol = worker_com->args->cfo_correct_tol_hz; 
-  srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol/(15000*q->fft_size));
-  srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol/(15000*q->fft_size));
-
-  int time_correct_period = worker_com->args->time_correct_period; 
-  if (time_correct_period > 0) {
-    srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);     
+
+  void phch_recv::set_time_adv_sec(float _time_adv_sec) {
+    time_adv_sec = _time_adv_sec;
   }
-  
-  sss_alg_t sss_alg = SSS_FULL; 
-  if (!worker_com->args->sss_algorithm.compare("diff")) {
-    sss_alg = SSS_DIFF;
-  } else if (!worker_com->args->sss_algorithm.compare("partial")) {
-    sss_alg = SSS_PARTIAL_3;
-  } else if (!worker_com->args->sss_algorithm.compare("full")){
-    sss_alg = SSS_FULL; 
-  } else {
-    Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());     
+
+  void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
+    if (worker_com->args->cfo_integer_enabled) {
+      srslte_ue_sync_cfo_i_detec_en(q, true);
+    }
+
+    float cfo_tol = worker_com->args->cfo_correct_tol_hz;
+    srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol / (15000 * q->fft_size));
+    srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol / (15000 * q->fft_size));
+
+    int time_correct_period = worker_com->args->time_correct_period;
+    if (time_correct_period > 0) {
+      srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);
+    }
+
+    sss_alg_t sss_alg = SSS_FULL;
+    if (!worker_com->args->sss_algorithm.compare("diff")) {
+      sss_alg = SSS_DIFF;
+    } else if (!worker_com->args->sss_algorithm.compare("partial")) {
+      sss_alg = SSS_PARTIAL_3;
+    } else if (!worker_com->args->sss_algorithm.compare("full")) {
+      sss_alg = SSS_FULL;
+    } else {
+      Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
+    }
+    srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg);
+    srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
   }
-  srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg); 
-  srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);  
-}
 
-bool phch_recv::init_cell() {
-  cell_is_set = false;
-  if (!srslte_ue_mib_init(&ue_mib, cell)) 
-  {
-    if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) 
-    {
-
-      // Set options defined in expert section 
-      set_ue_sync_opts(&ue_sync); 
-      
-      for (uint32_t i=0;i<workers_pool->get_nof_workers();i++) {
-        if (!((phch_worker*) workers_pool->get_worker(i))->init_cell(cell)) {
-          Error("Error setting cell: initiating PHCH worker\n");
-          return false; 
+  bool phch_recv::init_cell() {
+    cell_is_set = false;
+    if (!srslte_ue_mib_init(&ue_mib, cell)) {
+      if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+
+        // Set options defined in expert section
+        set_ue_sync_opts(&ue_sync);
+
+        for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+          if (!((phch_worker *) workers_pool->get_worker(i))->init_cell(cell)) {
+            Error("Error setting cell: initiating PHCH worker\n");
+            return false;
+          }
         }
+        radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
+        if (do_agc) {
+          srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
+        }
+        srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
+        cell_is_set = true;
+      } else {
+        Error("Error setting cell: initiating ue_sync");
       }
-      radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
-      if (do_agc) {
-        srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);    
-      }
-      srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
-      cell_is_set = true;                             
     } else {
-      Error("Error setting cell: initiating ue_sync");      
+      Error("Error setting cell: initiating ue_mib\n");
     }
-  } else {
-    Error("Error setting cell: initiating ue_mib\n"); 
-  }      
-  return cell_is_set; 
-}
+    return cell_is_set;
+  }
 
-void phch_recv::free_cell()
-{
-  if (cell_is_set) {
-    for (uint32_t i=0;i<workers_pool->get_nof_workers();i++) {
-      ((phch_worker*) workers_pool->get_worker(i))->free_cell();
+  void phch_recv::free_cell() {
+    if (phy_state != IDLE) {
+      phy_state = IDLE;
+      usleep(2000);
+    }
+
+    if (cell_is_set) {
+      for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+        ((phch_worker *) workers_pool->get_worker(i))->free_cell();
+      }
+      prach_buffer->free_cell();
+      cell_is_set = false;
     }
-    prach_buffer->free_cell();
   }
-}
 
 
-bool phch_recv::cell_search(int force_N_id_2) 
-{
-  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
-  uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN/8];
-  
-  srslte_ue_cellsearch_result_t found_cells[3];
-  srslte_ue_cellsearch_t        cs; 
+  bool phch_recv::cell_search(int force_N_id_2) {
+    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+    uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN / 8];
 
-  bzero(found_cells, 3*sizeof(srslte_ue_cellsearch_result_t));
+    srslte_ue_cellsearch_result_t found_cells[3];
+    srslte_ue_cellsearch_t cs;
 
-  log_h->console("Searching for cell...\n");
-  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating UE cell search\n");
-    return false; 
-  }
-  
-  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
-  
-  // Set options defined in expert section 
-  set_ue_sync_opts(&cs.ue_sync); 
-  
-  if (do_agc) {
-    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
-  }
-  
-  radio_h->set_rx_srate(1.92e6);
-  radio_h->start_rx();
-  
-  /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
-  uint32_t max_peak_cell = 0;
-  int ret = SRSLTE_ERROR; 
-  
-  if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
-    ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
-    max_peak_cell = force_N_id_2;
-  } else {
-    ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell); 
-  }
+    bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
+
+    log_h->console("Searching for cell...\n");
+    if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
+                                        radio_h)) {
+      Error("Initiating UE cell search\n");
+      return false;
+    }
 
-  last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
+    srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
 
-  radio_h->stop_rx();
-  srslte_ue_cellsearch_free(&cs);
-  
-  if (ret < 0) {
-    Error("Error decoding MIB: Error searching PSS\n");
-    return false;
-  } else if (ret == 0) {
-    Error("Error decoding MIB: Could not find any PSS in this frequency\n");
-    return false;
-  }
-    
-  // Save result 
-  cell.id   = found_cells[max_peak_cell].cell_id;
-  cell.cp   = found_cells[max_peak_cell].cp; 
-  cellsearch_cfo = found_cells[max_peak_cell].cfo;
-  
-  log_h->console("Found CELL ID: %d CP: %s, CFO: %.1f KHz.\nTrying to decode MIB...\n", 
-                 cell.id, srslte_cp_string(cell.cp), cellsearch_cfo/1000);
-  
-  srslte_ue_mib_sync_t ue_mib_sync; 
-
-  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating UE MIB synchronization\n");
-    return false; 
+    // Set options defined in expert section
+    set_ue_sync_opts(&cs.ue_sync);
+
+    if (do_agc) {
+      srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
+    }
+
+    radio_h->set_rx_srate(1.92e6);
+    radio_h->start_rx();
+
+    /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
+    uint32_t max_peak_cell = 0;
+    int ret = SRSLTE_ERROR;
+
+    if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
+      ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
+      max_peak_cell = force_N_id_2;
+    } else {
+      ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell);
+    }
+
+    last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
+
+    radio_h->stop_rx();
+    srslte_ue_cellsearch_free(&cs);
+
+    if (ret < 0) {
+      Error("Error decoding MIB: Error searching PSS\n");
+      return false;
+    } else if (ret == 0) {
+      Error("Error decoding MIB: Could not find any PSS in this frequency\n");
+      return false;
+    }
+
+    // Save result
+    cell.id = found_cells[max_peak_cell].cell_id;
+    cell.cp = found_cells[max_peak_cell].cp;
+    cellsearch_cfo = found_cells[max_peak_cell].cfo;
+
+    log_h->console("Found CELL ID: %d CP: %s, CFO: %.1f KHz.\nTrying to decode MIB...\n",
+                   cell.id, srslte_cp_string(cell.cp), cellsearch_cfo / 1000);
+
+    srslte_ue_mib_sync_t ue_mib_sync;
+
+    if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas,
+                                      radio_h)) {
+      Error("Initiating UE MIB synchronization\n");
+      return false;
+    }
+
+    // Set options defined in expert section
+    set_ue_sync_opts(&ue_mib_sync.ue_sync);
+
+    if (do_agc) {
+      srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
+    }
+
+    srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
+
+    /* Find and decode MIB */
+    uint32_t sfn;
+    int sfn_offset;
+    radio_h->start_rx();
+    ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
+                                    SRSLTE_DEFAULT_MAX_FRAMES_PBCH,
+                                    bch_payload, &cell.nof_ports, &sfn_offset);
+    radio_h->stop_rx();
+    last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
+    cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
+    srslte_ue_mib_sync_free(&ue_mib_sync);
+
+    if (ret == 1) {
+      srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
+      worker_com->set_cell(cell);
+      srslte_cell_fprint(stdout, &cell, 0);
+
+      srslte_bit_pack_vector(bch_payload, bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN);
+      mac->bch_decoded_ok(bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN / 8);
+      return true;
+    } else {
+      Warning("Error decoding MIB: Error decoding PBCH\n");
+      return false;
+    }
   }
-  
-  // Set options defined in expert section 
-  set_ue_sync_opts(&ue_mib_sync.ue_sync); 
 
-  if (do_agc) {
-    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);    
+
+  int phch_recv::sync_sfn(void) {
+
+    int ret = SRSLTE_ERROR;
+    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+
+    srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+    ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer_sfn);
+    if (ret < 0) {
+      Error("Error calling ue_sync_get_buffer");
+      return -1;
+    }
+
+    if (ret == 1) {
+      if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
+        int sfn_offset = 0;
+        Info("SYNC:  Decoding MIB...\n");
+        int n = srslte_ue_mib_decode(&ue_mib, sf_buffer_sfn[0], bch_payload, NULL, &sfn_offset);
+        if (n < 0) {
+          Error("Error decoding MIB while synchronising SFN");
+          return -1;
+        } else if (n == SRSLTE_UE_MIB_FOUND) {
+          uint32_t sfn;
+          srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
+
+          sfn = (sfn+sfn_offset)%1024;
+          tti = sfn * 10;
+
+          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+          Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
+          srslte_ue_mib_reset(&ue_mib);
+          return 1;
+        }
+      }
+    } else {
+      Debug("SYNC:  PSS/SSS not found...\n");
+    }
+    return 0;
   }
 
-  srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
-
-  /* Find and decode MIB */
-  uint32_t sfn; 
-  int sfn_offset; 
-  radio_h->start_rx();
-  ret = srslte_ue_mib_sync_decode(&ue_mib_sync, 
-                                  SRSLTE_DEFAULT_MAX_FRAMES_PBCH, 
-                                  bch_payload, &cell.nof_ports, &sfn_offset); 
-  radio_h->stop_rx();
-  last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
-  cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
-  srslte_ue_mib_sync_free(&ue_mib_sync);
-
-  if (ret == 1) {
-    srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
-    worker_com->set_cell(cell);
-    srslte_cell_fprint(stdout, &cell, 0);
-
-    srslte_bit_pack_vector(bch_payload, bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN);
-    mac->bch_decoded_ok(bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN/8);
-    return true;     
-  } else {
-    Warning("Error decoding MIB: Error decoding PBCH\n");      
-    return false;
+  void phch_recv::resync_sfn() {
+    sync_sfn_cnt = 0;
+    phy_state = CELL_SELECT;
   }
-}
 
+  void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
+    this->earfcn = earfcn;
+  }
 
-int phch_recv::sync_sfn(void) {
-  
-  int ret = SRSLTE_ERROR; 
-  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+  void phch_recv::cell_search_next() {
+    cur_earfcn_index++;
+    if (cur_earfcn_index >= 0) {
+      if ((uint32_t) cur_earfcn_index >= earfcn.size() - 1) {
+        cur_earfcn_index = 0;
+      }
+      // If PHY is running, stop and free resources
+      free_cell();
+
+      float dl_freq = srslte_band_fd(earfcn[cur_earfcn_index]);
+      if (dl_freq >= 0) {
+        log_h->console("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz\n", earfcn[cur_earfcn_index], dl_freq / 1e6);
+        log_h->info("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz, channel_index=%d\n", earfcn[cur_earfcn_index],
+                    dl_freq / 1e6, cur_earfcn_index);
+        radio_h->set_rx_freq(dl_freq);
+
+        // Start PHY cell search (finds maximum cell in frequency)
+        phy_state = CELL_SEARCH;
+      } else {
+        log_h->error("Cell Search: Invalid EARFCN=%d, channel_index=%d\n", earfcn[cur_earfcn_index], cur_earfcn_index);
+      }
+    }
+  }
 
-  srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-  ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer_sfn);
-  if (ret < 0) {
-    Error("Error calling ue_sync_get_buffer");      
-    return -1;
+  void phch_recv::cell_search_start() {
+    cur_earfcn_index = -1;
+    log_h->console("Cell Search: Starting procedure...\n");
+    log_h->info("Cell Search: Starting procedure...\n");
+    cell_search_next();
   }
-    
-  if (ret == 1) {
-    if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
-      int sfn_offset=0;
-      Info("SYNC:  Decoding MIB...\n");
-      int n = srslte_ue_mib_decode(&ue_mib, sf_buffer_sfn[0], bch_payload, NULL, &sfn_offset);
-      if (n < 0) {
-        Error("Error decoding MIB while synchronising SFN");      
-        return -1; 
-      } else if (n == SRSLTE_UE_MIB_FOUND) {  
-        uint32_t sfn; 
-        srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
-
-        sfn = (sfn + sfn_offset)%1024;         
-        tti = sfn*10;
-        
-        srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-        Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
-        srslte_ue_mib_reset(&ue_mib);
-        return 1;
+
+  bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
+    free_cell();
+
+    float dl_freq = srslte_band_fd(earfcn);
+    float ul_freq = srslte_band_ul_earfcn(earfcn);
+    if (dl_freq >= 0 || ul_freq <= 0) {
+      log_h->console("Cell Select: Set EARFCN=%d, DL frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn,
+                     dl_freq / 1e6, ul_freq / 1e6);
+      log_h->info("Cell Select: Set EARFCN=%d, frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn, dl_freq / 1e6,
+                  ul_freq / 1e6);
+      radio_h->set_rx_freq(dl_freq);
+      radio_h->set_tx_freq(ul_freq);
+
+      this->cell = cell;
+      if (init_cell()) {
+        phy_state = CELL_SELECT;
+        return true;
+      } else {
+        log_h->error("Cell Select: Initializing cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
       }
-    }    
-  } else {
-    Debug("SYNC:  PSS/SSS not found...\n");
+    } else {
+      log_h->error("Cell Select: Invalid EARFCN=%d\n", earfcn);
+    }
+    return false;
   }
-  return 0;
-}
 
-void phch_recv::resync_sfn() {
-  sync_sfn_cnt = 0; 
-  phy_state = SYNCING;
-}
+  void phch_recv::run_thread() {
+    int sync_res;
+    phch_worker *worker = NULL;
+    cf_t *buffer[SRSLTE_MAX_PORTS];
+    while (running) {
+      switch (phy_state) {
+        case CELL_SEARCH:
+          if (cell_search()) {
+            log_h->console("Initializating cell configuration...\n");
+            init_cell();
+            float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+
+            if (30720 % ((int) srate / 1000) == 0) {
+              radio_h->set_master_clock_rate(30.72e6);
+            } else {
+              radio_h->set_master_clock_rate(23.04e6);
+            }
 
-void phch_recv::run_thread()
-{
-  int sync_res; 
-  phch_worker *worker = NULL;
-  cf_t *buffer[SRSLTE_MAX_PORTS];
-  while(running) {
-    switch(phy_state) {
-      case CELL_SEARCH:
-        if (cell_search()) {
-          log_h->console("Initializating cell configuration...\n");
-          init_cell();
-          float srate = (float) srslte_sampling_freq_hz(cell.nof_prb); 
-          
-          if (30720%((int) srate/1000) == 0) {
-            radio_h->set_master_clock_rate(30.72e6);        
-          } else {
-            radio_h->set_master_clock_rate(23.04e6);        
+            log_h->console("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
+            radio_h->set_rx_srate(srate);
+            radio_h->set_tx_srate(srate);
+
+            ul_dl_factor = radio_h->get_tx_freq() / radio_h->get_rx_freq();
+
+            Info("SYNC:  Cell found. Synchronizing...\n");
+            phy_state = CELL_SELECT;
+            sync_sfn_cnt = 0;
+            srslte_ue_mib_reset(&ue_mib);
           }
-          
-          log_h->console("Setting Sampling frequency %.2f MHz\n", (float) srate/1000000);          
-          radio_h->set_rx_srate(srate);
-          radio_h->set_tx_srate(srate);
+          break;
+        case CELL_SELECT:
 
-          ul_dl_factor = radio_h->get_tx_freq()/radio_h->get_rx_freq();
+          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
 
-          Info("SYNC:  Cell found. Synchronizing...\n");
-          phy_state = SYNCING;
-          sync_sfn_cnt = 0; 
-          srslte_ue_mib_reset(&ue_mib);
-        }
-        break;
-      case SYNCING:
-        
-        srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-        
-        if (!radio_is_streaming) {
-          // Start streaming
-          radio_h->start_rx();
-          radio_is_streaming = true; 
-        }
-          
-        switch(sync_sfn()) {
-          default:
-            log_h->console("Going IDLE\n");
-            phy_state = IDLE; 
-            break; 
-          case 1:
-            srslte_ue_sync_set_agc_period(&ue_sync, 20);
-            phy_state = SYNC_DONE;  
-            break;        
-          case 0:
-            break;        
-        } 
-        sync_sfn_cnt++;
-        if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
-          sync_sfn_cnt = 0; 
-          radio_h->stop_rx();
-          radio_is_streaming = false; 
-          log_h->console("Timeout while synchronizing SFN\n");
-          log_h->warning("Timeout while synchronizing SFN\n");
-        }
-       break;
-      case SYNC_DONE:        
-        tti = (tti+1)%10240;        
-        worker = (phch_worker*) workers_pool->wait_worker(tti);
-        sync_res = 0; 
-        if (worker) {       
-          for (uint32_t i=0;i<nof_rx_antennas;i++) {
-            buffer[i] = worker->get_buffer(i);
+          if (!radio_is_streaming) {
+            // Start streaming
+            radio_h->start_rx();
+            radio_is_streaming = true;
+          }
+
+          switch (sync_sfn()) {
+            default:
+              log_h->console("Going IDLE\n");
+              phy_state = IDLE;
+              break;
+            case 1:
+              srslte_ue_sync_set_agc_period(&ue_sync, 20);
+              phy_state = CAMPING;
+              break;
+            case 0:
+              break;
+          }
+          sync_sfn_cnt++;
+          if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
+            sync_sfn_cnt = 0;
+            radio_h->stop_rx();
+            radio_is_streaming = false;
+            log_h->console("Timeout while synchronizing SFN\n");
+            log_h->warning("Timeout while synchronizing SFN\n");
           }
+          break;
+        case CAMPING:
+          tti = (tti+1) % 10240;
+          worker = (phch_worker *) workers_pool->wait_worker(tti);
+          sync_res = 0;
+          if (worker) {
+            for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+              buffer[i] = worker->get_buffer(i);
+            }
 
-          sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer); 
-          if (sync_res == 1) {
-            
-            log_h->step(tti);
-
-            Debug("Worker %d synchronized\n", worker->get_id());
-            
-            metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
-            metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
-            worker->set_cfo(ul_dl_factor*metrics.cfo/15000);
-            worker_com->set_sync_metrics(metrics);
-    
-            float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync)/1000; 
-            worker->set_sample_offset(sample_offset);
-            
-            /* Compute TX time: Any transmission happens in TTI+4 thus advance 4 ms the reception time */
-            srslte_timestamp_t rx_time, tx_time, tx_time_prach; 
-            srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time); 
-            srslte_timestamp_copy(&tx_time, &rx_time);
-            srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
-            worker->set_tx_time(tx_time);
-            
-            Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
-            worker->set_tti(tti, tx_mutex_cnt);
-            tx_mutex_cnt = (tx_mutex_cnt+1)%nof_tx_mutex;
-
-            // Check if we need to TX a PRACH 
-            if (prach_buffer->is_ready_to_send(tti)) {
-              srslte_timestamp_copy(&tx_time_prach, &rx_time);
-              srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf*1e-3);
-              prach_buffer->send(radio_h, ul_dl_factor*metrics.cfo/15000, worker_com->pathloss, tx_time_prach);
-              radio_h->tx_end();            
-              worker_com->p0_preamble = prach_buffer->get_p0_preamble();
-              worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss + worker_com->p0_preamble);
-            }            
-            workers_pool->start_worker(worker);             
-            // Notify RRC in-sync every 1 frame
-            if ((tti%10) == 0) {
-              rrc->in_sync();
-              log_h->debug("Sending in-sync to RRC\n");
+            sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer);
+            if (sync_res == 1) {
+
+              log_h->step(tti);
+
+              Debug("Worker %d synchronized\n", worker->get_id());
+
+              metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
+              metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
+              worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
+              worker_com->set_sync_metrics(metrics);
+
+              float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync) / 1000;
+              worker->set_sample_offset(sample_offset);
+
+              /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
+              srslte_timestamp_t rx_time, tx_time, tx_time_prach;
+              srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
+              srslte_timestamp_copy(&tx_time, &rx_time);
+              srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
+              worker->set_tx_time(tx_time);
+
+              Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+              worker->set_tti(tti, tx_mutex_cnt);
+              tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
+
+              // Check if we need to TX a PRACH
+              if (prach_buffer->is_ready_to_send(tti)) {
+                srslte_timestamp_copy(&tx_time_prach, &rx_time);
+                srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
+                prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
+                radio_h->tx_end();
+                worker_com->p0_preamble = prach_buffer->get_p0_preamble();
+                worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
+              }
+              workers_pool->start_worker(worker);
+              // Notify RRC in-sync every 1 frame
+              if ((tti % 10) == 0) {
+                rrc->in_sync();
+                log_h->debug("Sending in-sync to RRC\n");
+              }
+            } else {
+              log_h->console("Sync error.\n");
+              log_h->error("Sync error. Sending out-of-sync to RRC\n");
+              // Notify RRC of out-of-sync frame
+              rrc->out_of_sync();
+              worker->release();
+              worker_com->reset_ul();
+              phy_state = CELL_SELECT;
             }
           } else {
-            log_h->console("Sync error.\n");
-            log_h->error("Sync error. Sending out-of-sync to RRC\n");
-            // Notify RRC of out-of-sync frame
-            rrc->out_of_sync();
-            worker->release();
-            worker_com->reset_ul();            
-            phy_state = SYNCING;
+            // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
+            running = false;
           }
-        } else {
-          // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
-          running = false; 
-        }
-        break;
-      case IDLE:
-        usleep(1000);
-        break;
+          break;
+        case IDLE:
+          usleep(1000);
+          break;
+      }
     }
   }
-}
-
-uint32_t phch_recv::get_current_tti()
-{
-  return tti; 
-}
-
-bool phch_recv::status_is_sync()
-{
-  return phy_state == SYNC_DONE;
-}
 
-void phch_recv::get_current_cell(srslte_cell_t* cell_)
-{
-  if (cell_) {
-    memcpy(cell_, &cell, sizeof(srslte_cell_t));
+  uint32_t phch_recv::get_current_tti() {
+    return tti;
   }
-}
-
-void phch_recv::sync_start()
-{
-  radio_h->set_master_clock_rate(30.72e6);        
-  phy_state = CELL_SEARCH;
-}
 
-void phch_recv::sync_stop()
-{
-  free_cell();
-  radio_h->stop_rx();
-  radio_is_streaming = false; 
-  phy_state = IDLE; 
-}
+  bool phch_recv::status_is_sync() {
+    return phy_state == CAMPING;
+  }
 
+  void phch_recv::get_current_cell(srslte_cell_t *cell_) {
+    if (cell_) {
+      memcpy(cell_, &cell, sizeof(srslte_cell_t));
+    }
+  }
 }
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index cf673cce6..9b03ab013 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -205,6 +205,21 @@ void phy::configure_ul_params(bool pregen_disabled)
   }
 }
 
+void phy::cell_search_start()
+{
+  sf_recv.cell_search_start();
+}
+
+void phy::cell_search_next()
+{
+  sf_recv.cell_search_next();
+}
+
+bool phy::cell_select(uint32_t earfcn, srslte_cell_t phy_cell)
+{
+  return sf_recv.cell_select(earfcn, phy_cell);
+}
+
 float phy::get_phr()
 {
   float phr = radio_handler->get_max_tx_power() - workers_common.cur_pusch_power; 
@@ -280,23 +295,18 @@ int phy::sr_last_tx_tti()
   return workers_common.sr_last_tx_tti;
 }
 
-bool phy::status_is_sync()
-{
-  return sf_recv.status_is_sync();
-}
-
 void phy::resync_sfn() {
   sf_recv.resync_sfn();
 }
 
-void phy::sync_start()
+void phy::set_earfcn(vector< uint32_t > earfcns)
 {
-  sf_recv.sync_start();
+  sf_recv.set_earfcn(earfcns);
 }
 
-void phy::sync_stop()
+bool phy::sync_status()
 {
-  sf_recv.sync_stop();
+  return sf_recv.status_is_sync();
 }
 
 void phy::set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN])
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 97f59c15e..ac98aeea1 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -178,11 +178,6 @@ bool ue::init(all_args_t *args_)
 
   radio.register_error_handler(rf_msg);
 
-  radio.set_rx_freq(args->rf.dl_freq);
-  radio.set_tx_freq(args->rf.ul_freq);
-
-  phy_log.console("Setting frequency: DL=%.1f Mhz, UL=%.1f MHz\n", args->rf.dl_freq/1e6, args->rf.ul_freq/1e6);
-
   mac.init(&phy, &rlc, &rrc, &mac_log);
   rlc.init(&pdcp, &rrc, this, &rlc_log, &mac);
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, SECURITY_DIRECTION_UPLINK);
@@ -258,7 +253,7 @@ bool ue::get_metrics(ue_metrics_t &m)
   rf_metrics.rf_error = false; // Reset error flag
 
   if(EMM_STATE_REGISTERED == nas.get_state()) {
-    if(RRC_STATE_RRC_CONNECTED == rrc.get_state()) {
+    if(RRC_STATE_CONNECTED == rrc.get_state()) {
       phy.get_metrics(m.phy);
       mac.get_metrics(m.mac);
       rlc.get_metrics(m.rlc);
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 15dc154ae..be03ec840 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -29,42 +29,72 @@
 
 using namespace srslte;
 
-namespace srsue{
-
-nas::nas()
-  :state(EMM_STATE_DEREGISTERED)
-  ,is_guti_set(false)
-  ,ip_addr(0)
-  ,eps_bearer_id(0)
-  ,count_ul(0)
-  ,count_dl(0)
-{}
-
-void nas::init(usim_interface_nas *usim_,
-               rrc_interface_nas  *rrc_,
-               gw_interface_nas   *gw_,
-               srslte::log        *nas_log_)
-{
-  pool    = byte_buffer_pool::get_instance();
-  usim    = usim_;
-  rrc     = rrc_;
-  gw      = gw_;
-  nas_log = nas_log_;
-}
+namespace srsue {
+
+  nas::nas()
+    : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), is_guti_set(false), ip_addr(0), eps_bearer_id(0),
+      count_ul(0), count_dl(0) {}
+
+  void nas::init(usim_interface_nas *usim_,
+                 rrc_interface_nas *rrc_,
+                 gw_interface_nas *gw_,
+                 srslte::log *nas_log_) {
+    pool = byte_buffer_pool::get_instance();
+    usim = usim_;
+    rrc = rrc_;
+    gw = gw_;
+    nas_log = nas_log_;
+    state = EMM_STATE_DEREGISTERED;
 
-void nas::stop()
-{}
+    // Manual PLMN selection procedure
+    current_plmn.mcc = 1;
+    current_plmn.mnc = 1;
+    plmn_selection = PLMN_SELECTED;
+  }
 
-emm_state_t nas::get_state()
-{
-  return state;
-}
+  void nas::stop() {}
+
+  emm_state_t nas::get_state() {
+    return state;
+  }
+
+/*******************************************************************************
+  UE interface
+*******************************************************************************/
+  void nas::attach_request() {
+    if (state == EMM_STATE_DEREGISTERED) {
+      state = EMM_STATE_REGISTERED_INITIATED;
+      if (plmn_selection == PLMN_NOT_SELECTED) {
+        rrc->plmn_search();
+      } else if (plmn_selection == PLMN_SELECTED) {
+        rrc->plmn_select(current_plmn);
+      }
+    } else {
+      nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
+    }
+  }
 
+  void nas::deattach_request() {
+    state = EMM_STATE_DEREGISTERED_INITIATED;
+    nas_log->info("Dettach request not supported\n");
+  }
 
 /*******************************************************************************
   RRC interface
 *******************************************************************************/
 
+void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
+  // if it's the plmn we want rrc->plmn_select() and plmn_selection = PLMN_SELECTED
+}
+
+void nas::cell_selected() {
+  if (state == EMM_STATE_REGISTERED_INITIATED) {
+    rrc->connect();
+  } else {
+    nas_log->info("Cell selcted in invalid state = %s\n", emm_state_text[state]);
+  }
+}
+
 bool nas::is_attached()
 {
   return state == EMM_STATE_REGISTERED;
@@ -73,7 +103,7 @@ bool nas::is_attached()
 void nas::notify_connection_setup()
 {
   nas_log->debug("State = %s\n", emm_state_text[state]);
-  if(EMM_STATE_DEREGISTERED == state) {
+  if(EMM_STATE_REGISTERED_INITIATED == state) {
     send_attach_request();
   } else {
     send_service_request();
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index a1805a1e9..20a4db1a2 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -29,7 +29,6 @@
 #include <sstream>
 
 #include "upper/rrc.h"
-#include "srslte/phy/utils/bit.h"
 #include "srslte/common/security.h"
 #include "srslte/common/bcd_helpers.h"
 
@@ -39,6 +38,11 @@ using namespace srslte;
 
 namespace srsue{
 
+  
+/*******************************************************************************
+  Base functions 
+*******************************************************************************/
+    
 rrc::rrc()
   :state(RRC_STATE_IDLE)
   ,drb_up(false)
@@ -76,6 +80,11 @@ void rrc::init(phy_interface_rrc     *phy_,
   usim    = usim_;
   rrc_log = rrc_log_;
   mac_timers = mac_timers_;
+  state   = RRC_STATE_IDLE; 
+  si_acquire_state = SI_ACQUIRE_IDLE; 
+  
+  thread_running = true; 
+  start();
 
   pthread_mutex_init(&mutex, NULL); 
   
@@ -93,7 +102,10 @@ void rrc::init(phy_interface_rrc     *phy_,
 }
 
 void rrc::stop()
-{}
+{
+  thread_running = false; 
+  wait_thread_finish();
+}
 
 rrc_state_t rrc::get_state()
 {
@@ -110,66 +122,127 @@ void rrc::set_ue_category(int category)
 }
 
 
+
+/*******************************************************************************
+ * 
+ * 
+ * 
+ * PLMN selection, cell selection/reselection and acquisition of SI procedures
+ * 
+ * 
+ * 
+*******************************************************************************/
+
+
 /*******************************************************************************
   NAS interface
 *******************************************************************************/
 
-void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
+uint16_t rrc::get_mcc()
 {
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
-
-  switch(state)
-  {
-  case RRC_STATE_COMPLETING_SETUP:
-    send_con_setup_complete(sdu);
-    break;
-  case RRC_STATE_RRC_CONNECTED:
-    send_ul_info_transfer(lcid, sdu);
-    break;
-  default:
-    rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
-    break;
+  if (current_cell) {
+    if(current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mcc;
+    }
   }
+  return 0;
 }
 
-uint16_t rrc::get_mcc()
+uint16_t rrc::get_mnc()
 {
-  if(sib1.N_plmn_ids > 0)
-    return sib1.plmn_id[0].id.mcc;
-  else
-    return 0;
+  if (current_cell) {
+    if(current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mnc;      
+    }
+  }
+  return 0;
 }
 
-uint16_t rrc::get_mnc()
+void rrc::plmn_search()
 {
-  if(sib1.N_plmn_ids > 0)
-    return sib1.plmn_id[0].id.mnc;
-  else
-    return 0;
+  state = RRC_STATE_PLMN_SELECTION;
+  phy->cell_search_start();
 }
 
-/*******************************************************************************
-  MAC interface
-*******************************************************************************/
-/* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
-void rrc::release_pucch_srs()
+void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id)
 {
-  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
-  set_phy_default_pucch_srs();
+  bool sync_ok = false; 
   
-  // Configure RX signals without pregeneration because default option is release
-  phy->configure_ul_params(true);
+  state = RRC_STATE_CELL_SELECTING; 
   
+  // Sort cells according to RSRP
+
+  selected_plmn_id = plmn_id; 
+  last_selected_cell = -1; 
+
+  select_next_cell_in_plmn();  
 }
 
-void rrc::ra_problem() {
-  radio_link_failure();
+void rrc::connect()
+{
+  pthread_mutex_lock(&mutex);
+  if(RRC_STATE_CELL_SELECTED == state) {
+    if (si_acquire_state == SI_ACQUIRE_IDLE) {
+      rrc_log->info("RRC in IDLE state - sending connection request.\n");
+      state = RRC_STATE_CONNECTING;
+      send_con_request();
+    } else {
+      rrc_log->warning("Received connect() but SI not acquired\n");
+    }
+  } else {
+    rrc_log->warning("Received connect() but cell is not selected\n");
+  }
+  pthread_mutex_unlock(&mutex);  
+}
+
+void rrc::select_next_cell_in_plmn() {
+  for (uint32_t i=last_selected_cell+1;i<known_cells.size();i++) {
+    for (uint32_t j=0;j<known_cells[i].sib1.N_plmn_ids;j++) {
+      if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc || 
+          known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) 
+      {
+        // Check that cell satisfies S criteria 
+        if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
+          si_acquire_state = SI_ACQUIRE_SIB1; 
+          last_selected_cell = i; 
+          return;           
+        }
+      }      
+    }
+  }
 }
 
 /*******************************************************************************
   PHY interface
 *******************************************************************************/
 
+void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
+    
+  // find if cell_id-earfcn combination already exists 
+  for (uint32_t i=0;i<known_cells.size();i++) {
+    if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
+      known_cells[i].rsrp = rsrp; 
+      current_cell = &known_cells[i];
+      rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%d dBm\n", known_cells[i].earfcn, known_cells[i].phy_cell.id, known_cells[i].rsrp);            
+      return; 
+    }
+  }
+  // add to list of known cells 
+  cell_t cell; 
+  cell.phy_cell = phy_cell; 
+  cell.rsrp     = rsrp; 
+  cell.earfcn   = earfcn;
+  cell.has_valid_sib1 = false; 
+  cell.has_valid_sib2 = false; 
+  known_cells.push_back(cell);  
+  
+  // save current cell 
+  current_cell = &known_cells.back(); 
+
+  rrc_log->info("Found new cell EARFCN=%d, PCI=%d, RSRP=%d dBm\n", cell.earfcn, cell.phy_cell.id, cell.rsrp);
+  
+}
+
 // Detection of physical layer problems (5.3.11.1)
 void rrc::out_of_sync()
 {
@@ -198,22 +271,236 @@ void rrc::in_sync()
 }
 
 /*******************************************************************************
-  GW interface
+  PDCP interface
 *******************************************************************************/
+void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu)
+{
+  pool->deallocate(pdu);
+  if (state == RRC_STATE_PLMN_SELECTION) {
+    // Do we need to do something with BCH?
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
+    si_acquire_state = SI_ACQUIRE_SIB1;       
+  } else {
+    rrc_log->warning("Received BCCH BCH in incorrect state\n");
+  }
+}
 
-bool rrc::rrc_connected()
+void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu)
 {
-  return (RRC_STATE_RRC_CONNECTED == state);
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
+  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
+  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
+  bit_buf.N_bits = pdu->N_bytes*8;
+  pool->deallocate(pdu);
+  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dlsch_msg);
+
+  if (dlsch_msg.N_sibs > 0) {
+    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) 
+    {
+      mac->bcch_stop_rx();
+
+      // Handle SIB1
+      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+
+      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
+                    current_cell->sib1.cell_id&0xfff,
+                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+
+      // Send PLMN and TAC to NAS
+      std::stringstream ss;
+      for(uint32_t i=0;i<current_cell->sib1.N_plmn_ids;i++){
+        std::string mcc;
+        std::string mnc;
+        mcc_to_string(current_cell->sib1.plmn_id[i].id.mcc, &mcc);
+        mnc_to_string(current_cell->sib1.plmn_id[i].id.mnc, &mnc);
+        ss << " PLMN Id: MCC " << mcc << " MNC " << mnc;
+
+        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+      }
+            
+      // Set TDD Config 
+      if (current_cell->sib1.tdd) {
+        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+      }
+      
+      rrc_log->console("SIB1 received, CellID=%d, %s\n",
+                       current_cell->sib1.cell_id&0xfff,
+                       ss.str().c_str());
+
+      current_cell->has_valid_sib1 = true; 
+      si_acquire_state = SI_ACQUIRE_SIB2;
+
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB2 == si_acquire_state) 
+    {
+      mac->bcch_stop_rx();      
+
+      // Handle SIB2
+      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+      rrc_log->console("SIB2 received\n");
+      rrc_log->info("SIB2 received\n");
+      
+      apply_sib2_configs(&current_cell->sib2);
+
+      current_cell->has_valid_sib2 = true; 
+      si_acquire_state = SI_ACQUIRE_IDLE; 
+    }
+  }
 }
 
-void rrc::rrc_connect() {
-  pthread_mutex_lock(&mutex);
-  if(RRC_STATE_IDLE == state) {
-    rrc_log->info("RRC in IDLE state - sending connection request.\n");
-    state = RRC_STATE_WAIT_FOR_CON_SETUP;
-    send_con_request();
+
+// Right now, this thread only controls System Information acquisition procedure
+void rrc::run_thread()
+{
+  uint32_t  tti ;
+  uint32_t  si_win_start, si_win_len;
+  uint16_t  period;
+  uint32_t  nof_sib1_trials = 0; 
+  const int SIB1_SEARCH_TIMEOUT = 30; 
+
+  while(thread_running)
+  {
+    switch(si_acquire_state)
+    {
+    case SI_ACQUIRE_SIB1:
+      // Instruct MAC to look for SIB1
+      if (!current_cell->has_valid_sib1) {
+        tti          = mac->get_current_tti();
+        si_win_start = sib_start_tti(tti, 2, 5);
+        mac->bcch_start_rx(si_win_start, 1);
+        rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                      si_win_start, 1);
+        nof_sib1_trials++;
+        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+          if (state == RRC_STATE_CELL_SELECTING) {
+            select_next_cell_in_plmn();
+            si_acquire_state = SI_ACQUIRE_IDLE;
+          } else if (state == RRC_STATE_PLMN_SELECTION) {
+            phy->cell_search_next();
+          } 
+          nof_sib1_trials = 0; 
+        }
+      } else {
+        si_acquire_state = SI_ACQUIRE_SIB2; 
+      }
+      break;
+    case SI_ACQUIRE_SIB2:
+      // Instruct MAC to look for SIB2 only when selecting a cell 
+      if (state == RRC_STATE_CELL_SELECTING && !current_cell->has_valid_sib2) {
+        tti          = mac->get_current_tti();
+        period       = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
+        si_win_start = sib_start_tti(tti, period, 0);
+        si_win_len   = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+
+        mac->bcch_start_rx(si_win_start, si_win_len);
+        rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                      si_win_start, si_win_len);
+      } else {
+        si_acquire_state = SI_ACQUIRE_DONE; 
+      }
+      break;
+    case SI_ACQUIRE_DONE: 
+      
+      // After acquiring SI, tell NAS that the cell is selected or go to next cell in case of PLMN selection 
+      
+      if (state == RRC_STATE_CELL_SELECTING) {        
+        nas->cell_selected();
+        state = RRC_STATE_CELL_SELECTED;
+      } else if (state == RRC_STATE_PLMN_SELECTION) {
+        phy->cell_search_next();
+      }
+      si_acquire_state = SI_ACQUIRE_IDLE; 
+      break;
+    default:
+      break;
+    }      
+    usleep(10000);
   }
-  pthread_mutex_unlock(&mutex);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/*******************************************************************************
+ * 
+ * 
+ * 
+ * Connection control and establishment/reestablishment procedures 
+ * 
+ * 
+ * 
+*******************************************************************************/
+
+
+
+
+
+
+/*******************************************************************************
+  NAS interface
+*******************************************************************************/
+
+void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
+{
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
+
+  switch(state)
+  {
+  case RRC_STATE_CONNECTING:
+    send_con_setup_complete(sdu);
+    break;
+  case RRC_STATE_CONNECTED:
+    send_ul_info_transfer(lcid, sdu);
+    break;
+  default:
+    rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
+    break;
+  }
+}
+
+
+
+/*******************************************************************************
+  MAC interface
+*******************************************************************************/
+/* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
+void rrc::release_pucch_srs()
+{
+  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
+  set_phy_default_pucch_srs();
+  
+  // Configure RX signals without pregeneration because default option is release
+  phy->configure_ul_params(true);  
+}
+
+void rrc::ra_problem() {
+  radio_link_failure();
+}
+
+
+/*******************************************************************************
+  GW interface
+*******************************************************************************/
+
+bool rrc::is_connected()
+{
+  return (RRC_STATE_CONNECTED == state);
 }
 
 bool rrc::have_drb()
@@ -246,75 +533,6 @@ void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 
 }
 
-void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu)
-{
-  // Unpack the MIB
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
-  rrc_log->info("BCCH BCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_bch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &mib);
-  rrc_log->info("MIB received BW=%s MHz\n", liblte_rrc_dl_bandwidth_text[mib.dl_bw]);
-  rrc_log->console("MIB received BW=%s MHz\n", liblte_rrc_dl_bandwidth_text[mib.dl_bw]);
-
-  // Start the SIB search state machine
-  state = RRC_STATE_SIB1_SEARCH;
-  pthread_create(&sib_search_thread, NULL, &rrc::start_sib_thread, this);
-}
-
-void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu)
-{
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dlsch_msg);
-
-  if (dlsch_msg.N_sibs > 0) {
-    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && RRC_STATE_SIB1_SEARCH == state) {
-      // Handle SIB1
-      memcpy(&sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                    sib1.cell_id&0xfff,
-                    liblte_rrc_si_window_length_num[sib1.si_window_length],
-                    liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity]);
-      std::stringstream ss;
-      for(uint32_t i=0;i<sib1.N_plmn_ids;i++){
-        std::string mcc;
-        std::string mnc;
-        mcc_to_string(sib1.plmn_id[i].id.mcc, &mcc);
-        mnc_to_string(sib1.plmn_id[i].id.mnc, &mnc);
-        ss << " PLMN Id: MCC " << mcc << " MNC " << mnc;
-      }
-      
-      // Set TDD Config 
-      if (dlsch_msg.sibs[0].sib.sib1.tdd) {
-        phy->set_config_tdd(&dlsch_msg.sibs[0].sib.sib1.tdd_cnfg);
-      }
-      
-      rrc_log->console("SIB1 received, CellID=%d, %s\n",
-                       sib1.cell_id&0xfff,
-                       ss.str().c_str());
-      
-      state = RRC_STATE_SIB2_SEARCH;
-      mac->bcch_stop_rx();
-      //TODO: Use all SIB1 info
-
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type && RRC_STATE_SIB2_SEARCH == state) {
-      // Handle SIB2
-      memcpy(&sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-      rrc_log->console("SIB2 received\n");
-      rrc_log->info("SIB2 received\n");
-      state = RRC_STATE_WAIT_FOR_CON_SETUP;
-      mac->bcch_stop_rx();
-      apply_sib2_configs();
-      send_con_request();
-    }
-  }
-}
 
 void rrc::write_pdu_pcch(byte_buffer_t *pdu)
 {
@@ -352,7 +570,7 @@ void rrc::write_pdu_pcch(byte_buffer_t *pdu)
         mac->pcch_stop_rx();
         if(RRC_STATE_IDLE == state) {
           rrc_log->info("RRC in IDLE state - sending connection request.\n");
-          state = RRC_STATE_WAIT_FOR_CON_SETUP;
+          state = RRC_STATE_CONNECTING;
           send_con_request();
         }
       }
@@ -400,7 +618,7 @@ void rrc::send_con_request()
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
   pdcp_buf->N_bytes = bit_buf.N_bits/8;
   pdcp_buf->set_timestamp();
@@ -417,7 +635,6 @@ void rrc::send_con_request()
   mac->set_contention_id(uecri);
 
   rrc_log->info("Sending RRC Connection Request on SRB0\n");
-  state = RRC_STATE_WAIT_FOR_CON_SETUP;
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
 
@@ -478,7 +695,7 @@ void rrc::send_con_restablish_request()
   
   // Wait for cell re-synchronization  
   uint32_t timeout_cnt = 0; 
-  while(!phy->status_is_sync() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH){
+  while(!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH){
     usleep(10000);
     timeout_cnt++; 
   }
@@ -494,7 +711,7 @@ void rrc::send_con_restablish_request()
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
   pdcp_buf->N_bytes = bit_buf.N_bits/8;
 
@@ -509,7 +726,6 @@ void rrc::send_con_restablish_request()
   mac->set_contention_id(uecri);
 
   rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
-  state = RRC_STATE_WAIT_FOR_CON_SETUP;
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
 
@@ -531,11 +747,11 @@ void rrc::send_con_restablish_complete()
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
   pdcp_buf->N_bytes = bit_buf.N_bits/8;
 
-  state = RRC_STATE_RRC_CONNECTED;
+  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
@@ -562,12 +778,12 @@ void rrc::send_con_setup_complete(byte_buffer_t *nas_msg)
       bit_buf.msg[bit_buf.N_bits + i] = 0;
     bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;
+  byte_buffer_t *pdcp_buf = pool_allocate;;
   srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
   pdcp_buf->N_bytes = bit_buf.N_bits/8;
   pdcp_buf->set_timestamp();
 
-  state = RRC_STATE_RRC_CONNECTED;
+  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Setup Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
@@ -654,7 +870,7 @@ void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu)
 
 void rrc::enable_capabilities()
 {
-  bool enable_ul_64 = ue_category>=5 && sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+  bool enable_ul_64 = ue_category>=5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
   rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64?"Enabling":"Disabling");
   phy->set_config_64qam_en(enable_ul_64);  
 }
@@ -766,7 +982,6 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu)
     transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
     handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
     rrc_log->info("Notifying NAS of connection setup\n");
-    state = RRC_STATE_COMPLETING_SETUP;
     nas->notify_connection_setup();
     break;
   case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
@@ -904,108 +1119,44 @@ void rrc::radio_link_failure() {
   
   rrc_log->warning("Detected Radio-Link Failure\n");
   rrc_log->console("Warning: Detected Radio-Link Failure\n");
-  if (state != RRC_STATE_RRC_CONNECTED) {
+  if (state != RRC_STATE_CONNECTED) {
     rrc_connection_release();
   } else {    
     send_con_restablish_request();
   }
 }
 
-void* rrc::start_sib_thread(void *rrc_)
-{
-  rrc *r = (rrc*)rrc_;
-  r->sib_search();
-  return NULL;
-}
-
-void rrc::sib_search()
-{
-  bool      searching = true;
-  uint32_t  tti ;
-  uint32_t  si_win_start, si_win_len;
-  uint16_t  period;
-  uint32_t  nof_sib1_trials = 0; 
-  const int SIB1_SEARCH_TIMEOUT = 30; 
-
-  while(searching)
-  {
-    switch(state)
-    {
-    case RRC_STATE_SIB1_SEARCH:
-      // Instruct MAC to look for SIB1
-      while(!phy->status_is_sync()){
-        usleep(50000);
-      }
-      usleep(10000); 
-      tti          = mac->get_current_tti();
-      si_win_start = sib_start_tti(tti, 2, 5);
-      mac->bcch_start_rx(si_win_start, 1);
-      rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                     si_win_start, 1);
-      nof_sib1_trials++;
-      if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-        rrc_log->info("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-        rrc_log->console("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-        phy->resync_sfn();
-        nof_sib1_trials = 0; 
-      }
-      break;
-    case RRC_STATE_SIB2_SEARCH:
-      // Instruct MAC to look for SIB2
-      usleep(10000);
-      tti          = mac->get_current_tti();
-      period       = liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity];
-      si_win_start = sib_start_tti(tti, period, 0);
-      si_win_len   = liblte_rrc_si_window_length_num[sib1.si_window_length];
-
-      mac->bcch_start_rx(si_win_start, si_win_len);
-      rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                     si_win_start, si_win_len);
-
-      break;
-    default:
-      searching = false;
-      break;
-    }
-    usleep(100000);
-  }
-}
 
 // Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
 uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
   return (period*10*(1+tti/(period*10))+x)%10240; // the 1 means next opportunity
 }
 
-void rrc::apply_sib2_configs()
+void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2)
 {
-  if(RRC_STATE_WAIT_FOR_CON_SETUP != state){
-    rrc_log->error("State must be RRC_STATE_WAIT_FOR_CON_SETUP to handle SIB2. Actual state: %s\n",
-                   rrc_state_text[state]);
-    return;
-  }
   
   // Apply RACH timeAlginmentTimer configuration 
   mac_interface_rrc::mac_cfg_t cfg; 
   mac->get_config(&cfg);
-  cfg.main.time_alignment_timer = sib2.time_alignment_timer; 
-  memcpy(&cfg.rach, &sib2.rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
-  cfg.prach_config_index = sib2.rr_config_common_sib.prach_cnfg.root_sequence_index; 
+  cfg.main.time_alignment_timer = sib2->time_alignment_timer; 
+  memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
+  cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index; 
   mac->set_config(&cfg);
   
   rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
-         liblte_rrc_number_of_ra_preambles_num[sib2.rr_config_common_sib.rach_cnfg.num_ra_preambles],
-         liblte_rrc_ra_response_window_size_num[sib2.rr_config_common_sib.rach_cnfg.ra_resp_win_size],
-         liblte_rrc_mac_contention_resolution_timer_num[sib2.rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
+         liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
+         liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
+         liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
 
   // Apply PHY RR Config Common
   phy_interface_rrc::phy_cfg_common_t common; 
-  memcpy(&common.pdsch_cnfg,  &sib2.rr_config_common_sib.pdsch_cnfg,  sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pusch_cnfg,  &sib2.rr_config_common_sib.pusch_cnfg,  sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pucch_cnfg,  &sib2.rr_config_common_sib.pucch_cnfg,  sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.ul_pwr_ctrl, &sib2.rr_config_common_sib.ul_pwr_ctrl, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
-  memcpy(&common.prach_cnfg,  &sib2.rr_config_common_sib.prach_cnfg,  sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
-  if (sib2.rr_config_common_sib.srs_ul_cnfg.present) {
-    memcpy(&common.srs_ul_cnfg,  &sib2.rr_config_common_sib.srs_ul_cnfg, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pdsch_cnfg,  &sib2->rr_config_common_sib.pdsch_cnfg,  sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pusch_cnfg,  &sib2->rr_config_common_sib.pusch_cnfg,  sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pucch_cnfg,  &sib2->rr_config_common_sib.pucch_cnfg,  sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
+  memcpy(&common.prach_cnfg,  &sib2->rr_config_common_sib.prach_cnfg,  sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
+  if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
+    memcpy(&common.srs_ul_cnfg,  &sib2->rr_config_common_sib.srs_ul_cnfg, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
   } else {
     // default is release
     common.srs_ul_cnfg.present = false; 
@@ -1015,34 +1166,34 @@ void rrc::apply_sib2_configs()
   phy->configure_ul_params();
 
   rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
-                sib2.rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
-                sib2.rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
-                sib2.rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
-                sib2.rr_config_common_sib.pusch_cnfg.n_sb);
+                sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
+                sib2->rr_config_common_sib.pusch_cnfg.n_sb);
 
   rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
-                liblte_rrc_delta_pucch_shift_num[sib2.rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
-                sib2.rr_config_common_sib.pucch_cnfg.n_cs_an,
-                sib2.rr_config_common_sib.pucch_cnfg.n1_pucch_an,
-                sib2.rr_config_common_sib.pucch_cnfg.n_rb_cqi);
+                liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
+                sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
   
   rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
-                 sib2.rr_config_common_sib.prach_cnfg.root_sequence_index,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag?"yes":"no",
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
-                 sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
+                 sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
+                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag?"yes":"no",
+                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
+                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
+                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
 
   rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
-                 liblte_rrc_srs_bw_config_num[sib2.rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
-                 liblte_rrc_srs_subfr_config_num[sib2.rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
-                 sib2.rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx?"yes":"no");
-
-  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2.ue_timers_and_constants.t301]);
-  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2.ue_timers_and_constants.t310]);
-  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2.ue_timers_and_constants.t311]);
-  N310 = liblte_rrc_n310_num[sib2.ue_timers_and_constants.n310];
-  N311 = liblte_rrc_n311_num[sib2.ue_timers_and_constants.n311];
+                 liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
+                 liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
+                 sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx?"yes":"no");
+
+  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
+  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
+  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+  N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
+  N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
   
   rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n", 
     N310, N311, mac_timers->get(t301)->get_timeout(), 
@@ -1309,7 +1460,7 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
   byte_buffer_t *nas_sdu;
   for(i=0;i<reconfig->N_ded_info_nas;i++)
   {
-    nas_sdu = pool_allocate;
+    nas_sdu = pool_allocate;;
     memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
     nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
     nas->write_pdu(lcid, nas_sdu);
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index 91df6a2fc..2eadba220 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -363,9 +363,7 @@ int main(int argc, char *argv[])
   radio.set_tx_freq(prog_args.rf_tx_freq);
   
   // Instruct the PHY to configure PRACH parameters and sync to current cell 
-  my_phy.sync_start();
-  
-  while(!my_phy.status_is_sync()) {
+  while(!my_phy.sync_status()) {
     usleep(20000);
   }
 
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index 08116b22c..834dbf75e 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -182,11 +182,9 @@ int main(int argc, char *argv[])
   // Set RX freq and gain
   radio.set_rx_freq(prog_args.rf_freq);
   
-  my_phy.sync_start();
-  
-  bool running = true; 
+  bool running = true;
   while(running) {
-    if (bch_decoded && my_phy.status_is_sync()) {
+    if (bch_decoded && my_phy.sync_status()) {
       uint32_t tti = my_phy.get_current_tti();
       
       // SIB1 is scheduled in subframe #5 of even frames, try to decode next frame SIB1
@@ -196,7 +194,7 @@ int main(int argc, char *argv[])
       total_pkts++;       
     }
     usleep(30000);    
-    if (bch_decoded && my_phy.status_is_sync() && total_pkts > 0) {
+    if (bch_decoded && my_phy.sync_status() && total_pkts > 0) {
       if (srslte_verbose == SRSLTE_VERBOSE_NONE && srsapps_verbose == 0) {
         float gain = prog_args.rf_gain; 
         if (gain < 0) {
diff --git a/srsue/test/upper/CMakeLists.txt b/srsue/test/upper/CMakeLists.txt
index 324ae4913..cbffe1cc8 100644
--- a/srsue/test/upper/CMakeLists.txt
+++ b/srsue/test/upper/CMakeLists.txt
@@ -18,17 +18,6 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-# IP traffic over RLC test 
-add_executable(ip_test ip_test.cc)
-target_link_libraries(ip_test srsue_mac
-                              srsue_phy
-                              srslte_common
-                              srslte_phy
-                              srslte_radio
-                              srslte_upper
-                              ${CMAKE_THREAD_LIBS_INIT}
-                              ${Boost_LIBRARIES})
-
 add_executable(usim_test usim_test.cc)
 target_link_libraries(usim_test srsue_upper srslte_upper srslte_phy)
 add_test(usim_test usim_test)
diff --git a/srsue/test/upper/ip_test.cc b/srsue/test/upper/ip_test.cc
deleted file mode 100644
index a26f273a8..000000000
--- a/srsue/test/upper/ip_test.cc
+++ /dev/null
@@ -1,645 +0,0 @@
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <arpa/inet.h>
-#include <linux/ip.h>
-#include <linux/if.h>
-#include <linux/if_tun.h>
-#include <sys/ioctl.h>
-#include <sys/socket.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <signal.h>
-#include <iostream>
-#include <sstream>
-#include <iomanip>
-#include <assert.h>
-
-#include "srslte/phy/utils/debug.h"
-#include "mac/mac.h"
-#include "phy/phy.h"
-#include "srslte/common/threads.h"
-#include "srslte/common/common.h"
-#include "srslte/common/buffer_pool.h"
-#include "srslte/common/logger.h"
-#include "srslte/common/log_filter.h"
-#include "srslte/upper/rlc.h"
-#include "upper/rrc.h"
-#include "srslte/radio/radio_multi.h"
-
-#define START_TUNTAP
-#define USE_RADIO
-#define PRINT_GW 0
-
-/**********************************************************************
- *  Program arguments processing
- ***********************************************************************/
-
-#define LCID 3
-
-typedef struct {
-  float rx_freq; 
-  float tx_freq; 
-  float rx_gain;
-  float tx_gain;
-  int time_adv;
-  std::string ip_address;
-}prog_args_t;
-
-uint32_t srsapps_verbose = 1; 
-
-prog_args_t prog_args; 
-
-void args_default(prog_args_t *args) {
-  args->tx_freq = 2.505e9;
-  args->rx_freq = 2.625e9;
-  args->rx_gain = 50.0; 
-  args->tx_gain = 70.0; 
-  args->time_adv = -1; // calibrated for b210
-  args->ip_address = "192.168.3.2";
-}
-
-void usage(prog_args_t *args, char *prog) {
-  printf("Usage: %s [gGIrfFtv]\n", prog);
-  printf("\t-f RX frequency [Default %.1f MHz]\n", args->rx_freq/1e6);
-  printf("\t-F TX frequency [Default %.1f MHz]\n", args->tx_freq/1e6);
-  printf("\t-g RX gain [Default %.1f]\n", args->rx_gain);
-  printf("\t-G TX gain [Default %.1f]\n", args->tx_gain);
-  printf("\t-I IP address [Default %s]\n", args->ip_address.c_str());
-  printf("\t-t time advance (in samples) [Default %d]\n", args->time_adv);
-  printf("\t-v [increase verbosity, default none]\n");
-}
-
-void parse_args(prog_args_t *args, int argc, char **argv) {
-  int opt;
-  args_default(args);
-  while ((opt = getopt(argc, argv, "gGfFItv")) != -1) {
-    switch (opt) {
-    case 'g':
-      args->rx_gain = atof(argv[optind]);
-      break;
-    case 'G':
-      args->tx_gain = atof(argv[optind]);
-      break;
-    case 'f':
-      args->rx_freq = atof(argv[optind]);
-      break;
-    case 'F':
-      args->tx_freq = atof(argv[optind]);
-      break;
-    case 'I':
-      args->ip_address = argv[optind];
-      break;
-    case 't':
-      args->time_adv = atoi(argv[optind]);
-      break;
-    case 'v':
-      srsapps_verbose++;
-      break;
-    default:
-      usage(args, argv[0]);
-      exit(-1);
-    }
-  }
-  if (args->rx_freq < 0 || args->tx_freq < 0) {
-    usage(args, argv[0]);
-    exit(-1);
-  }
-}
-                            
-int setup_if_addr(char *ip_addr);
-  
-// Define dummy RLC always transmitts
-class tester : public srsue::pdcp_interface_rlc, 
-               public srsue::rrc_interface_rlc,
-               public srsue::rrc_interface_phy,
-               public srsue::rrc_interface_mac,
-               public srsue::ue_interface,
-               public thread
-{
-public:
-  
-  tester() {
-    state = srsue::RRC_STATE_SIB1_SEARCH;
-    read_enable = true; 
-  }
-  
-  void init(srsue::phy *phy_, srsue::mac *mac_, srslte::rlc *rlc_, srslte::log *log_h_, std::string ip_address) {
-    log_h = log_h_; 
-    rlc   = rlc_; 
-    mac   = mac_; 
-    phy   = phy_; 
-    
-#ifdef START_TUNTAP
-    if (init_tuntap((char*) ip_address.c_str())) {
-      log_h->error("Initiating IP address\n");
-    }
-#endif
-
-    pool = srslte::byte_buffer_pool::get_instance();
-    
-    // Start reader thread
-    running=true; 
-    start();
-
-  }
-  
-
-  void sib_search()
-  {
-    bool      searching = true;
-    uint32_t  tti ;
-    uint32_t  si_win_start, si_win_len;
-    uint16_t  period;
-    uint32_t  nof_sib1_trials = 0; 
-    const int SIB1_SEARCH_TIMEOUT = 30; 
-
-    while(searching)
-    {
-      switch(state)
-      {
-      case srsue::RRC_STATE_SIB1_SEARCH:
-        // Instruct MAC to look for SIB1
-        while(!phy->status_is_sync()){
-          usleep(50000);
-        }
-        usleep(10000); 
-        tti          = mac->get_current_tti();
-        si_win_start = sib_start_tti(tti, 2, 5);
-        mac->bcch_start_rx(si_win_start, 1);
-        log_h->info("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                      si_win_start, 1);
-        nof_sib1_trials++;
-        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-          log_h->info("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-          log_h->console("Timeout while searching for SIB1. Resynchronizing SFN...\n");
-          phy->resync_sfn();
-          nof_sib1_trials = 0; 
-        }
-        break;
-      case srsue::RRC_STATE_SIB2_SEARCH:
-        // Instruct MAC to look for SIB2
-        usleep(10000);
-        tti          = mac->get_current_tti();
-        period       = liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity];
-        si_win_start = sib_start_tti(tti, period, 0);
-        si_win_len   = liblte_rrc_si_window_length_num[sib1.si_window_length];
-
-        mac->bcch_start_rx(si_win_start, si_win_len);
-        log_h->info("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                      si_win_start, si_win_len);
-
-        break;
-      default:
-        searching = false;
-        break;
-      }
-      usleep(100000);
-    }
-  }
-  
-  bool is_sib_received() {
-    return state == srsue::RRC_STATE_WAIT_FOR_CON_SETUP; 
-  }
-
-
-  void release_pucch_srs() {}
-  void ra_problem() {}
-  void write_pdu_bcch_bch(srslte::byte_buffer_t *pdu) {}
-  void write_pdu_bcch_dlsch(srslte::byte_buffer_t *pdu) 
-  {
-    log_h->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-    log_h->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-    LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-    bit_buf.N_bits = pdu->N_bytes*8;
-    pool->deallocate(pdu);
-    liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dlsch_msg);
-
-    if (dlsch_msg.N_sibs > 0) {
-      if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && srsue::RRC_STATE_SIB1_SEARCH == state) {
-        // Handle SIB1
-        memcpy(&sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-        log_h->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                      sib1.cell_id&0xfff,
-                      liblte_rrc_si_window_length_num[sib1.si_window_length],
-                      liblte_rrc_si_periodicity_num[sib1.sched_info[0].si_periodicity]);
-        std::stringstream ss;
-        for(uint32_t i=0;i<sib1.N_plmn_ids;i++){
-          ss << " PLMN Id: MCC " << sib1.plmn_id[i].id.mcc << " MNC " << sib1.plmn_id[i].id.mnc;
-        }
-        log_h->console("SIB1 received, CellID=%d, %s\n",
-                        sib1.cell_id&0xfff,
-                        ss.str().c_str());
-        
-        state = srsue::RRC_STATE_SIB2_SEARCH;
-        mac->bcch_stop_rx();
-        //TODO: Use all SIB1 info
-
-      } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type && srsue::RRC_STATE_SIB2_SEARCH == state) {
-        // Handle SIB2
-        memcpy(&sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-        log_h->console("SIB2 received\n");
-        log_h->info("SIB2 received\n");
-        state = srsue::RRC_STATE_WAIT_FOR_CON_SETUP;
-        mac->bcch_stop_rx();
-        apply_sib2_configs();
-        
-        srslte::byte_buffer_t *sdu = pool_allocate; 
-        assert(sdu); 
-        
-        // Send Msg3 
-        sdu->N_bytes = 10; 
-        for (uint32_t i=0;i<sdu->N_bytes;i++) {
-          sdu->msg[i] = i+1; 
-        }
-        uint64_t uecri = 0; 
-        uint8_t *ue_cri_ptr = (uint8_t*) &uecri;
-        uint32_t nbytes = 6;
-        for (uint32_t i=0;i<nbytes;i++) {
-          ue_cri_ptr[nbytes-i-1] = sdu->msg[i];
-        }
-        log_h->info("Setting UE contention resolution ID: %d\n", uecri);        
-        mac->set_contention_id(uecri);
-        
-        rlc->write_sdu(0, sdu);
-
-      }
-    }
-  }
-  void write_pdu_pcch(srslte::byte_buffer_t *sdu) {}
-  void max_retx_attempted(){}
-  void in_sync() {};
-  void out_of_sync() {};
-
-  void write_pdu(uint32_t lcid, srslte::byte_buffer_t *sdu)
-  {
-    uint32_t n=0;
-    switch(lcid) {
-      case LCID:
-        n = write(tun_fd, sdu->msg, sdu->N_bytes);
-        if (n != sdu->N_bytes) {
-          log_h->error("TUN/TAP write failure n=%d, nof_bytes=%d\n", n, sdu->N_bytes);
-          return; 
-        }
-        log_h->debug_hex(sdu->msg, sdu->N_bytes, 
-                        "Wrote %d bytes to TUN/TAP\n", 
-                        sdu->N_bytes);      
-        pool->deallocate(sdu);
-      break;
-      case 0:
-        log_h->info("Received ConnectionSetupComplete\n");
-        
-        // Setup a single UM bearer 
-        LIBLTE_RRC_RLC_CONFIG_STRUCT cfg; 
-        bzero(&cfg, sizeof(LIBLTE_RRC_RLC_CONFIG_STRUCT));
-        cfg.rlc_mode = LIBLTE_RRC_RLC_MODE_UM_BI;
-        cfg.dl_um_bi_rlc.t_reordering = LIBLTE_RRC_T_REORDERING_MS100; 
-        cfg.dl_um_bi_rlc.sn_field_len = LIBLTE_RRC_SN_FIELD_LENGTH_SIZE10;   
-        cfg.ul_um_bi_rlc.sn_field_len = LIBLTE_RRC_SN_FIELD_LENGTH_SIZE10;   
-        rlc->add_bearer(LCID, &cfg);
-        
-        mac->setup_lcid(LCID, 0, 1, -1, 100000);
-
-        LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT dedicated; 
-        bzero(&dedicated, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
-        dedicated.pusch_cnfg_ded.beta_offset_ack_idx = 5;
-        dedicated.pusch_cnfg_ded.beta_offset_ri_idx  = 12;
-        dedicated.pusch_cnfg_ded.beta_offset_cqi_idx = 15;
-        dedicated.pusch_cnfg_ded_present = true; 
-        dedicated.sched_request_cnfg.dsr_trans_max = LIBLTE_RRC_DSR_TRANS_MAX_N4; 
-        dedicated.sched_request_cnfg.sr_pucch_resource_idx = 0;
-        dedicated.sched_request_cnfg.sr_cnfg_idx = 35; 
-        dedicated.sched_request_cnfg.setup_present = true; 
-        dedicated.sched_request_cnfg_present = true; 
-        phy->set_config_dedicated(&dedicated);
-        phy->configure_ul_params();
-        
-        srsue::mac_interface_rrc::mac_cfg_t mac_cfg; 
-        mac->get_config(&mac_cfg);
-        memcpy(&mac_cfg.sr, &dedicated.sched_request_cnfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-        mac_cfg.main.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_SF40;
-        mac->set_config(&mac_cfg);
-    
-      break;
-      default:
-        log_h->error("Received message for lcid=%d\n", lcid);
-      break;
-    }
-  }
-  
-private:
-  int tun_fd;
-  bool running; 
-  srslte::log *log_h;
-  srslte::byte_buffer_pool *pool;
-  srslte::rlc *rlc; 
-  srsue::mac *mac; 
-  srsue::phy *phy;  
-  srslte::bit_buffer_t bit_buf;
-  srsue::rrc_state_t state; 
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
-  LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
-  bool read_enable;
-
-  
-  // Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
-  uint32_t sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
-    return (period*10*(1+tti/(period*10))+x)%10240; // the 1 means next opportunity
-  }
-    
-  int init_tuntap(char *ip_address) {
-    read_enable = true; 
-    tun_fd = setup_if_addr(ip_address);
-    if (tun_fd<0) {
-      fprintf(stderr, "Error setting up IP %s\n", ip_address);
-      return -1;
-    }
-    
-    printf("Created tun/tap interface at IP %s\n", ip_address);
-    return 0; 
-  }
-  
-  void run_thread() {
-    struct iphdr   *ip_pkt;
-    uint32_t        idx = 0;
-    int32_t         N_bytes;
-    srslte::byte_buffer_t  *pdu = pool_allocate;
-
-    log_h->info("TUN/TAP reader thread running\n");
-
-    while(running) {
-      N_bytes = read(tun_fd, &pdu->msg[idx], SRSLTE_MAX_BUFFER_SIZE_BYTES-SRSLTE_BUFFER_HEADER_OFFSET - idx);      
-      if(N_bytes > 0 && read_enable)
-      {
-        pdu->N_bytes = idx + N_bytes;
-        ip_pkt       = (struct iphdr*)pdu->msg;
-
-        log_h->debug_hex(pdu->msg, pdu->N_bytes, 
-                          "Read %d bytes from TUN/TAP\n", 
-                          N_bytes);
-                  
-        // Check if entire packet was received
-        if(ntohs(ip_pkt->tot_len) == pdu->N_bytes)
-        {
-          log_h->info_hex(pdu->msg, pdu->N_bytes, "UL PDU");
-
-          // Send PDU directly to PDCP
-          pdu->set_timestamp();
-          rlc->write_sdu(LCID, pdu);
-          
-          pdu = pool_allocate;
-          idx = 0;
-        } else{
-          idx += N_bytes;
-        }
-      }else{
-        log_h->error("Failed to read from TUN interface - gw receive thread exiting.\n");
-        break;
-      }
-    }
-  }
-  
-    
-  void apply_sib2_configs()
-  {
-    
-    // Apply RACH timeAlginmentTimer configuration 
-    srsue::mac_interface_rrc::mac_cfg_t cfg; 
-    mac->get_config(&cfg);
-    cfg.main.time_alignment_timer = sib2.time_alignment_timer; 
-    memcpy(&cfg.rach, &sib2.rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
-    cfg.prach_config_index = sib2.rr_config_common_sib.prach_cnfg.root_sequence_index; 
-    mac->set_config(&cfg);
-    
-    log_h->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
-          liblte_rrc_number_of_ra_preambles_num[sib2.rr_config_common_sib.rach_cnfg.num_ra_preambles],
-          liblte_rrc_ra_response_window_size_num[sib2.rr_config_common_sib.rach_cnfg.ra_resp_win_size],
-          liblte_rrc_mac_contention_resolution_timer_num[sib2.rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
-
-    // Apply PHY RR Config Common
-    srsue::phy_interface_rrc::phy_cfg_common_t common; 
-    memcpy(&common.pdsch_cnfg,  &sib2.rr_config_common_sib.pdsch_cnfg,  sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.pusch_cnfg,  &sib2.rr_config_common_sib.pusch_cnfg,  sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.pucch_cnfg,  &sib2.rr_config_common_sib.pucch_cnfg,  sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.ul_pwr_ctrl, &sib2.rr_config_common_sib.ul_pwr_ctrl, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
-    memcpy(&common.prach_cnfg,  &sib2.rr_config_common_sib.prach_cnfg,  sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
-    if (sib2.rr_config_common_sib.srs_ul_cnfg.present) {
-      memcpy(&common.srs_ul_cnfg,  &sib2.rr_config_common_sib.srs_ul_cnfg, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
-    } else {
-      // default is release
-      common.srs_ul_cnfg.present = false; 
-    }
-    phy->set_config_common(&common);
-
-    phy->configure_ul_params();
-
-    log_h->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
-                  sib2.rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
-                  sib2.rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
-                  sib2.rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
-                  sib2.rr_config_common_sib.pusch_cnfg.n_sb);
-
-    log_h->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
-                  liblte_rrc_delta_pucch_shift_num[sib2.rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
-                  sib2.rr_config_common_sib.pucch_cnfg.n_cs_an,
-                  sib2.rr_config_common_sib.pucch_cnfg.n1_pucch_an,
-                  sib2.rr_config_common_sib.pucch_cnfg.n_rb_cqi);
-    
-    log_h->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%d, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
-                  sib2.rr_config_common_sib.prach_cnfg.root_sequence_index,
-                  sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag?1:0,
-                  sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
-                  sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
-                  sib2.rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
-
-    log_h->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%d\n",
-                  sib2.rr_config_common_sib.srs_ul_cnfg.bw_cnfg,
-                  sib2.rr_config_common_sib.srs_ul_cnfg.subfr_cnfg,
-                  sib2.rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx);
-    
-  }
-};
-
-
-
-// Create classes
-srslte::logger logger;
-srslte::log_filter log_phy;
-srslte::log_filter log_mac;
-srslte::log_filter log_rlc;
-srslte::log_filter log_tester;
-srslte::mac_pcap   mac_pcap;
-srsue::phy my_phy;
-srsue::mac my_mac;
-srslte::rlc rlc;
-srslte::radio_multi my_radio; 
-
-// Local classes for testing
-tester my_tester; 
-
-
-bool running = true; 
-
-void sig_int_handler(int signo)
-{
-  running = false;
-}
-
-int main(int argc, char *argv[])
-{
-  
-  parse_args(&prog_args, argc, argv);
-
-  // set to null to disable pcap 
-  const char *pcap_filename = "/tmp/ip_test.pcap"; 
-  
-  logger.init("/tmp/ip_test_ue.log");
-  log_phy.init("PHY ", &logger, true);
-  log_mac.init("MAC ", &logger, true);
-  log_rlc.init("RLC ", &logger);
-  log_tester.init("TEST", &logger);
-  logger.log("\n\n");
- 
-  if (srsapps_verbose == 1) {
-    log_phy.set_level(srslte::LOG_LEVEL_INFO);
-    log_phy.set_hex_limit(100);
-    log_mac.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_mac.set_hex_limit(100);
-    log_rlc.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_rlc.set_hex_limit(1000);
-    log_tester.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_tester.set_hex_limit(100);
-    printf("Log level info\n");
-  }
-  if (srsapps_verbose == 2) {
-    log_phy.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_phy.set_hex_limit(100);
-    log_mac.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_mac.set_hex_limit(100);
-    log_rlc.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_rlc.set_hex_limit(100);
-    log_tester.set_level(srslte::LOG_LEVEL_DEBUG);
-    log_tester.set_hex_limit(100);
-    srslte_verbose = SRSLTE_VERBOSE_DEBUG;
-    printf("Log level debug\n");
-  }
-    
-  // Init Radio and PHY
-#ifdef USE_RADIO
-  my_radio.init();
-#else
-  my_radio.init(NULL, "dummy");
-#endif
-  
-  my_radio.set_tx_freq(prog_args.tx_freq);
-  my_radio.set_tx_gain(prog_args.tx_gain);
-  my_radio.set_rx_freq(prog_args.rx_freq);
-  my_radio.set_rx_gain(prog_args.rx_gain);
-  if (prog_args.time_adv >= 0) {
-    printf("Setting TA=%d samples\n",prog_args.time_adv); 
-    my_radio.set_tx_adv(prog_args.time_adv);
-  }
-    
-  my_phy.init(&my_radio, &my_mac, &my_tester, &log_phy, NULL);
-  my_mac.init(&my_phy, &rlc, &my_tester, &log_mac);
-  rlc.init(&my_tester, &my_tester, &my_tester, &log_rlc, &my_mac);  
-  my_tester.init(&my_phy, &my_mac, &rlc, &log_tester, prog_args.ip_address);
-
-  
-  if (pcap_filename) {
-    mac_pcap.open(pcap_filename);
-    my_mac.start_pcap(&mac_pcap);
-    signal(SIGINT, sig_int_handler);     
-  }
-  
-  // Set MAC defaults 
-  LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
-  bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
-  default_cfg.ulsch_cnfg.max_harq_tx        = LIBLTE_RRC_MAX_HARQ_TX_N5;
-  default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
-  default_cfg.ulsch_cnfg.retx_bsr_timer     = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;  
-  default_cfg.ulsch_cnfg.tti_bundling       = false;
-  default_cfg.drx_cnfg.setup_present        = false;
-  default_cfg.phr_cnfg.setup_present        = false;
-  default_cfg.time_alignment_timer          = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY; 
-  my_mac.set_config_main(&default_cfg);
-  
-  while(running) {
-    if (my_tester.is_sib_received()) {
-      printf("Main running\n");
-      sleep(1);
-    } else {
-      my_tester.sib_search();
-    }
-  }
-  
-  if (pcap_filename) {
-    mac_pcap.close();
-  }
-  
-  my_phy.stop();
-  my_mac.stop();
-}
-
-
-
-
-/******************* This is copied from srsue gw **********************/
-int setup_if_addr(char *ip_addr)
-{
-    
-  char *dev = (char*) "tun_srsue";
-
-  // Construct the TUN device
-  int tun_fd = open("/dev/net/tun", O_RDWR);
-  if(0 > tun_fd)
-  {
-    perror("open");
-    return(-1);
-  }
-
-  struct ifreq ifr;
-  
-  memset(&ifr, 0, sizeof(ifr));
-  ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
-  strncpy(ifr.ifr_ifrn.ifrn_name, dev, IFNAMSIZ);
-  if(0 > ioctl(tun_fd, TUNSETIFF, &ifr))
-  {
-    perror("ioctl");
-    return -1;
-  }
-
-  // Bring up the interface
-  int sock = socket(AF_INET, SOCK_DGRAM, 0);
-  if(0 > ioctl(sock, SIOCGIFFLAGS, &ifr))
-  {
-    perror("socket");
-    return -1;
-  }
-  ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
-  if(0 > ioctl(sock, SIOCSIFFLAGS, &ifr))
-  {
-    perror("ioctl");
-    return -1;
-  }
-
-  // Setup the IP address    
-  sock                                                   = socket(AF_INET, SOCK_DGRAM, 0);
-  ifr.ifr_addr.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr.s_addr = inet_addr(ip_addr);
-  if(0 > ioctl(sock, SIOCSIFADDR, &ifr))
-  {
-    perror("ioctl");
-    return -1;
-  }
-  ifr.ifr_netmask.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
-  if(0 > ioctl(sock, SIOCSIFNETMASK, &ifr))
-  {
-    perror("ioctl");
-    return -1;
-  }
-
-  return(tun_fd);
-}

From 12cbf026060b8d713e9a7a533d4aef16db9dc8f4 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 22 Jun 2017 18:52:12 +0200
Subject: [PATCH 009/269] changed name of vector_simd function

---
 lib/include/srslte/phy/utils/vector_simd.h | 18 ++----------------
 lib/src/phy/utils/vector.c                 |  2 +-
 lib/src/phy/utils/vector_simd.c            |  2 +-
 3 files changed, 4 insertions(+), 18 deletions(-)

diff --git a/lib/include/srslte/phy/utils/vector_simd.h b/lib/include/srslte/phy/utils/vector_simd.h
index 7e4197e4b..1894a0803 100644
--- a/lib/include/srslte/phy/utils/vector_simd.h
+++ b/lib/include/srslte/phy/utils/vector_simd.h
@@ -39,22 +39,14 @@ SRSLTE_API int srslte_vec_dot_prod_sss_sse(short *x, short *y, uint32_t len);
 
 SRSLTE_API int srslte_vec_dot_prod_sss_avx2(short *x, short *y, uint32_t len); 
 
-
-  
 SRSLTE_API void srslte_vec_sum_sss_sse(short *x, short *y, short *z, uint32_t len);
 
 SRSLTE_API void srslte_vec_sum_sss_avx2(short *x, short *y, short *z, uint32_t len);
 
-
-
 SRSLTE_API void srslte_vec_sub_sss_sse(short *x, short *y, short *z, uint32_t len); 
 
 SRSLTE_API void srslte_vec_sub_sss_avx2(short *x, short *y, short *z, uint32_t len);
 
-
-
-
-
 SRSLTE_API void srslte_vec_prod_sss_sse(short *x, short *y, short *z, uint32_t len); 
 
 SRSLTE_API void srslte_vec_prod_sss_avx2(short *x, short *y, short *z, uint32_t len);
@@ -64,17 +56,11 @@ SRSLTE_API void srslte_vec_sc_div2_sss_sse(short *x, int n_rightshift, short *z,
 
 SRSLTE_API  void srslte_vec_sc_div2_sss_avx2(short *x, int k, short *z, uint32_t len);
 
-
-
-
-
 SRSLTE_API void srslte_vec_lut_sss_sse(short *x, unsigned short *lut, short *y, uint32_t len); 
 
-SRSLTE_API void srslte_vec_convert_fi_sse(float *x, int16_t *z, float scale, uint32_t len); 
-
-
+SRSLTE_API void srslte_vec_convert_fi_sse(float *x, int16_t *z, float scale, uint32_t len);
 
-SRSLTE_API void srslte_vec_mult_scalar_cf_f_avx( cf_t *z,const cf_t *x,const float h,const uint32_t len);
+SRSLTE_API void srslte_vec_sc_prod_cfc_avx(const cf_t *x,const float h,cf_t *y,const uint32_t len);
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 7ce954c78..30527510d 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -240,7 +240,7 @@ void srslte_vec_norm_cfc(cf_t *x, float amplitude, cf_t *y, uint32_t len) {
 
 void srslte_vec_sc_prod_cfc(cf_t *x, float h, cf_t *z, uint32_t len) {
 #ifdef LV_HAVE_AVX
-  srslte_vec_mult_scalar_cf_f_avx(z,x, h, len);  
+  srslte_vec_sc_prod_cfc_avx(x,h,z,len);
 #else
   int i;
   for (i=0;i<len;i++) {
diff --git a/lib/src/phy/utils/vector_simd.c b/lib/src/phy/utils/vector_simd.c
index 1fcf2e33f..7be4afc44 100644
--- a/lib/src/phy/utils/vector_simd.c
+++ b/lib/src/phy/utils/vector_simd.c
@@ -469,7 +469,7 @@ void srslte_vec_convert_fi_sse(float *x, int16_t *z, float scale, uint32_t len)
 }
 
 //srslte_32fc_s32f_multiply_32fc_avx
- void srslte_vec_mult_scalar_cf_f_avx( cf_t *z,const cf_t *x,const float h,const uint32_t len)
+ void srslte_vec_sc_prod_cfc_avx( const cf_t *x,const float h,cf_t *z,const uint32_t len)
 {
 #ifdef LV_HAVE_AVX
    

From b71f7b70803c52fa2568747df400043ced230a62 Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Fri, 23 Jun 2017 14:49:21 +0100
Subject: [PATCH 010/269] Adding support for static builds

---
 CMakeLists.txt                   | 18 +++++++++++++++---
 cmake/modules/FindMbedTLS.cmake  | 18 ++++++++++++++++--
 cmake/modules/FindPolarssl.cmake | 18 ++++++++++++++++--
 lib/include/srslte/upper/pdcp.h  |  7 +++----
 lib/src/phy/CMakeLists.txt       |  6 +++---
 srsenb/CMakeLists.txt            |  4 ++--
 srsenb/src/CMakeLists.txt        |  4 ++--
 srsue/src/CMakeLists.txt         |  5 +++++
 8 files changed, 62 insertions(+), 18 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd2aaf433..17609e65f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -66,7 +66,7 @@ option(ENABLE_BLADERF  "Enable BladeRF"                           ON)
 option(BUILD_STATIC    "Attempt to statically link external deps" OFF)
 option(RPATH           "Enable RPATH"                             OFF)
 
-option(USE_LTE_RATES   "Use standard LTE sampling rates"        OFF)
+option(USE_LTE_RATES   "Use standard LTE sampling rates"          OFF)
 
 set(GCC_ARCH native CACHE STRING "GCC compile for specific architecture.")
 
@@ -79,13 +79,21 @@ find_package(Threads REQUIRED)
 find_package(Polarssl)
 if (POLARSSL_FOUND)
   set(SEC_INCLUDE_DIRS "${POLARSSL_INCLUDE_DIRS}")
-  set(SEC_LIBRARIES    "${POLARSSL_LIBRARIES}")
+  if(BUILD_STATIC)
+    set(SEC_LIBRARIES    "${POLARSSL_STATIC_LIBRARIES}")
+  else(BUILD_STATIC)
+    set(SEC_LIBRARIES    "${POLARSSL_LIBRARIES}")
+  endif(BUILD_STATIC)
   add_definitions(-DHAVE_POLARSSL)
 else(POLARSSL_FOUND)
   find_package(MbedTLS REQUIRED)
   if (MBEDTLS_FOUND)
     set(SEC_INCLUDE_DIRS "${MBEDTLS_INCLUDE_DIRS}")
-    set(SEC_LIBRARIES    "${MBEDTLS_LIBRARIES}")
+    if(BUILD_STATIC)
+      set(SEC_LIBRARIES    "${MBEDTLS_STATIC_LIBRARIES}")
+    else(BUILD_STATIC)
+      set(SEC_LIBRARIES    "${MBEDTLS_LIBRARIES}")
+    endif(BUILD_STATIC)
     add_definitions(-DHAVE_MBEDTLS)
   endif (MBEDTLS_FOUND)
 endif(POLARSSL_FOUND)
@@ -119,6 +127,10 @@ endif(BLADERF_FOUND OR UHD_FOUND OR SOAPYSDR_FOUND)
 
 if(ENABLE_SRSUE OR ENABLE_SRSENB)
   # Find Boost
+  if(BUILD_STATIC)
+    set(Boost_USE_STATIC_LIBS ON)
+  endif(BUILD_STATIC)
+
   set(BOOST_REQUIRED_COMPONENTS
       program_options
       system
diff --git a/cmake/modules/FindMbedTLS.cmake b/cmake/modules/FindMbedTLS.cmake
index 94d814874..11499bad4 100644
--- a/cmake/modules/FindMbedTLS.cmake
+++ b/cmake/modules/FindMbedTLS.cmake
@@ -32,9 +32,23 @@ FIND_LIBRARY(
           /usr/lib64
 )
 
+FIND_LIBRARY(
+    MBEDTLS_STATIC_LIBRARIES
+    NAMES libmbedcrypto.a
+    HINTS $ENV{MBEDTLS_DIR}/lib
+          ${PC_MBEDTLS_LIBDIR}
+          ${CMAKE_INSTALL_PREFIX}/lib
+          ${CMAKE_INSTALL_PREFIX}/lib64
+    PATHS /usr/local/lib
+          /usr/local/lib64
+          /usr/lib
+          /usr/lib64
+)
+
 message(STATUS "MBEDTLS LIBRARIES: " ${MBEDTLS_LIBRARIES})
+message(STATUS "MBEDTLS STATIC LIBRARIES: " ${MBEDTLS_STATIC_LIBRARIES})
 message(STATUS "MBEDTLS INCLUDE DIRS: " ${MBEDTLS_INCLUDE_DIRS})
 
 INCLUDE(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_INCLUDE_DIRS)
-MARK_AS_ADVANCED(MBEDTLS_LIBRARIES MBEDTLS_INCLUDE_DIRS)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
+MARK_AS_ADVANCED(MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
diff --git a/cmake/modules/FindPolarssl.cmake b/cmake/modules/FindPolarssl.cmake
index ccf0d6b91..6e6c5c2cb 100644
--- a/cmake/modules/FindPolarssl.cmake
+++ b/cmake/modules/FindPolarssl.cmake
@@ -31,9 +31,23 @@ FIND_LIBRARY(
           /usr/lib64
 )
 
+FIND_LIBRARY(
+    POLARSSL_STATIC_LIBRARIES
+    NAMES libpolarssl.a
+    HINTS $ENV{POLARSSL_DIR}/lib
+          ${PC_POLARSSL_LIBDIR}
+          ${CMAKE_INSTALL_PREFIX}/lib
+          ${CMAKE_INSTALL_PREFIX}/lib64
+    PATHS /usr/local/lib
+          /usr/local/lib64
+          /usr/lib
+          /usr/lib64
+)
+
 message(STATUS "POLARSSL LIBRARIES: " ${POLARSSL_LIBRARIES})
+message(STATUS "POLARSSL STATIC LIBRARIES: " ${POLARSSL_STATIC_LIBRARIES})
 message(STATUS "POLARSSL INCLUDE DIRS: " ${POLARSSL_INCLUDE_DIRS})
 
 INCLUDE(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(POLARSSL DEFAULT_MSG POLARSSL_LIBRARIES POLARSSL_INCLUDE_DIRS)
-MARK_AS_ADVANCED(POLARSSL_LIBRARIES POLARSSL_INCLUDE_DIRS)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(POLARSSL DEFAULT_MSG POLARSSL_LIBRARIES POLARSSL_STATIC_LIBRARIES POLARSSL_INCLUDE_DIRS)
+MARK_AS_ADVANCED(POLARSSL_STATIC_LIBRARIES POLARSSL_LIBRARIES POLARSSL_INCLUDE_DIRS)
diff --git a/lib/include/srslte/upper/pdcp.h b/lib/include/srslte/upper/pdcp.h
index 090f8f045..857806810 100644
--- a/lib/include/srslte/upper/pdcp.h
+++ b/lib/include/srslte/upper/pdcp.h
@@ -66,14 +66,13 @@ public:
   void write_pdu_pcch(byte_buffer_t *sdu);
 
 private:
-  log        *pdcp_log;
-  pdcp_entity         pdcp_array[SRSLTE_N_RADIO_BEARERS];
-
   srsue::rlc_interface_pdcp *rlc;
   srsue::rrc_interface_pdcp *rrc;
   srsue::gw_interface_pdcp  *gw;
 
-  uint8_t             direction;
+  log                       *pdcp_log;
+  pdcp_entity                pdcp_array[SRSLTE_N_RADIO_BEARERS];
+  uint8_t                    direction;
 
   bool valid_lcid(uint32_t lcid);
 };
diff --git a/lib/src/phy/CMakeLists.txt b/lib/src/phy/CMakeLists.txt
index 074ddec55..26271f2a7 100644
--- a/lib/src/phy/CMakeLists.txt
+++ b/lib/src/phy/CMakeLists.txt
@@ -72,11 +72,11 @@ endif(MKL_FOUND)
 
 
 if(MKL_FOUND)
-  if(STATIC_MKL)
+  if(BUILD_STATIC)
     target_link_libraries(srslte_phy ${MKL_STATIC_LIBRARIES})
-  else(STATIC_MKL)
+  else(BUILD_STATIC)
     target_link_libraries(srslte_phy ${MKL_LIBRARIES})
-  endif(STATIC_MKL)
+  endif(BUILD_STATIC)
 else(MKL_FOUND)
   target_link_libraries(srslte_phy ${FFTW3F_LIBRARIES})
 endif(MKL_FOUND)
diff --git a/srsenb/CMakeLists.txt b/srsenb/CMakeLists.txt
index 7fa60301a..c836f36d5 100644
--- a/srsenb/CMakeLists.txt
+++ b/srsenb/CMakeLists.txt
@@ -21,9 +21,9 @@
 find_package(LibConfig REQUIRED)
 find_package(SCTP REQUIRED)
 
-if(STATIC_LIBCONFIGPP)
+if(BUILD_STATIC)
   set(LIBCONFIGPP_LIBRARIES "${LIBCONFIGPP_STATIC_LIBRARY_PATH}")
-endif(STATIC_LIBCONFIGPP)
+endif(BUILD_STATIC)
 
 if(NOT Boost_FOUND)
     message(FATAL_ERROR "Boost required to compile srsENB")
diff --git a/srsenb/src/CMakeLists.txt b/srsenb/src/CMakeLists.txt
index 199650fac..23272ea85 100644
--- a/srsenb/src/CMakeLists.txt
+++ b/srsenb/src/CMakeLists.txt
@@ -5,9 +5,9 @@ add_subdirectory(upper)
 
 
 # Link libstdc++ and libgcc
-if(STATIC_LIB)
+if(BUILD_STATIC)
   set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -static-libstdc++ -static-libgcc")
-endif(STATIC_LIB)
+endif(BUILD_STATIC)
 
 
 if (RPATH)
diff --git a/srsue/src/CMakeLists.txt b/srsue/src/CMakeLists.txt
index 5a56bb057..1c6b87bc1 100644
--- a/srsue/src/CMakeLists.txt
+++ b/srsue/src/CMakeLists.txt
@@ -22,6 +22,11 @@ add_subdirectory(phy)
 add_subdirectory(mac)
 add_subdirectory(upper)
 
+# Link libstdc++ and libgcc
+if(BUILD_STATIC)
+  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -static-libstdc++ -static-libgcc")
+endif(BUILD_STATIC)
+
 if (RPATH)
   set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
 endif (RPATH)

From 6037a2f2bdc5f28242744a00c392f9387f4498a0 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 23 Jun 2017 16:29:46 +0200
Subject: [PATCH 011/269] changed logger classes

---
 lib/examples/CMakeLists.txt                  |   2 +-
 lib/include/srslte/common/log_filter.h       |   8 +-
 lib/include/srslte/common/log_stdout.h       |  82 ------
 lib/include/srslte/common/logger.h           |  34 +--
 lib/include/srslte/common/logger_file.h      |  76 +++++
 lib/include/srslte/common/logger_stdout.h    |  51 ++++
 lib/src/common/log_filter.cc                 |   5 +
 lib/src/common/log_stdout.cc                 | 290 -------------------
 lib/src/common/{logger.cc => logger_file.cc} |  21 +-
 lib/test/common/log_filter_test.cc           |   5 +-
 lib/test/common/logger_test.cc               |   8 +-
 lib/test/upper/rlc_am_test.cc                |  43 +--
 lib/test/upper/rlc_um_test.cc                |  10 +-
 srsenb/hdr/enb.h                             |  20 +-
 srsenb/test/mac/scheduler_test.cc            |   4 +-
 srsenb/test/upper/ip_test.cc                 |  12 +-
 srsue/hdr/ue.h                               |  22 +-
 srsue/test/mac/mac_test.cc                   |   4 +-
 srsue/test/phy/ue_itf_test_prach.cc          |   4 +-
 srsue/test/phy/ue_itf_test_sib1.cc           |   4 +-
 srsue/test/upper/ip_test.cc                  |  14 +-
 srsue/test/upper/rrc_reconfig_test.cc        |   6 +-
 srsue/test/upper/usim_test.cc                |   4 +-
 23 files changed, 235 insertions(+), 494 deletions(-)
 delete mode 100644 lib/include/srslte/common/log_stdout.h
 create mode 100644 lib/include/srslte/common/logger_file.h
 create mode 100644 lib/include/srslte/common/logger_stdout.h
 delete mode 100644 lib/src/common/log_stdout.cc
 rename lib/src/common/{logger.cc => logger_file.cc} (85%)

diff --git a/lib/examples/CMakeLists.txt b/lib/examples/CMakeLists.txt
index 652a2c195..9c6ee193e 100644
--- a/lib/examples/CMakeLists.txt
+++ b/lib/examples/CMakeLists.txt
@@ -65,7 +65,7 @@ if(RF_FOUND)
   add_executable(cell_search cell_search.c)
   target_link_libraries(cell_search srslte_phy srslte_rf)
 
-  add_executable(cell_measurement cell_measurement.c)
+  add_executable(cell_measurement cell_measurement.c ../include/srslte/common/logger_stdout.h)
   target_link_libraries(cell_measurement srslte_phy srslte_rf)
 
   add_executable(usrp_capture usrp_capture.c)
diff --git a/lib/include/srslte/common/log_filter.h b/lib/include/srslte/common/log_filter.h
index c1ed1998a..ae232f135 100644
--- a/lib/include/srslte/common/log_filter.h
+++ b/lib/include/srslte/common/log_filter.h
@@ -38,15 +38,19 @@
 #include <stdarg.h>
 #include <string>
 #include "srslte/common/log.h"
-#include "logger.h"
+#include "srslte/common/logger.h"
+#include "srslte/common/logger_stdout.h"
 
 namespace srslte {
 
+typedef std::string* str_ptr;
+
 class log_filter : public srslte::log
 {
 public:
 
   log_filter();
+  log_filter(std::string layer);
   log_filter(std::string layer, logger *logger_, bool tti=false);
 
   void init(std::string layer, logger *logger_, bool tti=false);
@@ -71,6 +75,8 @@ private:
   logger *logger_h;
   bool    do_tti;
 
+  logger_stdout def_logger_stdout;
+
   void all_log(srslte::LOG_LEVEL_ENUM level, uint32_t tti, char *msg);
   void all_log(srslte::LOG_LEVEL_ENUM level, uint32_t tti, char *msg, uint8_t *hex, int size);
   void all_log_line(srslte::LOG_LEVEL_ENUM level, uint32_t tti, std::string file, int line, char *msg);
diff --git a/lib/include/srslte/common/log_stdout.h b/lib/include/srslte/common/log_stdout.h
deleted file mode 100644
index df1b5b5fb..000000000
--- a/lib/include/srslte/common/log_stdout.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-/******************************************************************************
- *  File:         log_stout.h
- *
- *  Description:  Logging service through standard output. Inherits log interface
- *
- *  Reference:
- *****************************************************************************/
-
-#ifndef LOGSTDOUT_H
-#define LOGSTDOUT_H
-
-#include <stdarg.h>
-#include <string>
-#include "srslte/common/log.h"
-
-namespace srslte {
-  
-class log_stdout : public log
-{
-public:
-
-  log_stdout(std::string service_name_) : log(service_name_) { }
-  
-  void console(std::string message, ...);
-  void error(std::string message, ...);
-  void warning(std::string message, ...);
-  void info(std::string message, ...);
-  void debug(std::string message, ...);
-
-  // Same with hex dump
-  void error_hex(uint8_t *hex, int size, std::string message, ...);
-  void warning_hex(uint8_t *hex, int size, std::string message, ...);
-  void info_hex(uint8_t *hex, int size, std::string message, ...);
-  void debug_hex(uint8_t *hex, int size, std::string message, ...);
-
-  // Same with line and file info
-  void error_line(std::string file, int line, std::string message, ...);
-  void warning_line(std::string file, int line, std::string message, ...);
-  void info_line(std::string file, int line, std::string message, ...);
-  void debug_line(std::string file, int line, std::string message, ...);
-
-private:
-  void printlog(srslte::LOG_LEVEL_ENUM level, uint32_t tti, std::string file, int line, std::string message, va_list args);
-  void printlog(srslte::LOG_LEVEL_ENUM level, uint32_t tti, std::string message, va_list args);
-
-  void all_log(srslte::LOG_LEVEL_ENUM level, uint32_t tti, char *msg);
-  void all_log(srslte::LOG_LEVEL_ENUM level, uint32_t tti, char *msg, uint8_t *hex, int size);
-  void all_log_line(srslte::LOG_LEVEL_ENUM level, uint32_t tti, std::string file, int line, char *msg);
-  std::string now_time();
-  std::string hex_string(uint8_t *hex, int size);
-};
-
-}
-
-#endif
-  
diff --git a/lib/include/srslte/common/logger.h b/lib/include/srslte/common/logger.h
index 67a897824..95e52922c 100644
--- a/lib/include/srslte/common/logger.h
+++ b/lib/include/srslte/common/logger.h
@@ -26,49 +26,23 @@
 
 /******************************************************************************
  * File:        logger.h
- * Description: Common log object. Maintains a queue of log messages
- *              and runs a thread to read messages and write to file.
- *              Multiple producers, single consumer. If full, producers
- *              increase queue size. If empty, consumer blocks.
+ * Description: Interface for logging output
  *****************************************************************************/
 
 #ifndef LOGGER_H
 #define LOGGER_H
 
 #include <stdio.h>
-#include <deque>
 #include <string>
-#include "srslte/common/threads.h"
 
 namespace srslte {
 
-typedef std::string* str_ptr;
-
-class logger : public thread
+class logger
 {
 public:
-  logger();
-  logger(std::string file);
-  ~logger();
-  void init(std::string file);
-  void log(const char *msg);
-  void log(str_ptr msg);
-
-private:
-  void run_thread(); 
-  void flush();
-
-  FILE*                 logfile;
-  bool                  inited;
-  bool                  not_done;
-  std::string           filename;
-  pthread_cond_t        not_empty;
-  pthread_cond_t        not_full;
-  pthread_mutex_t       mutex;
-  pthread_t             thread;
-  std::deque<str_ptr> buffer;
+  virtual void log(std::string *msg) = 0;
 };
 
-} // namespace srsue
+} // namespace srslte
 
 #endif // LOGGER_H
diff --git a/lib/include/srslte/common/logger_file.h b/lib/include/srslte/common/logger_file.h
new file mode 100644
index 000000000..1bc41f883
--- /dev/null
+++ b/lib/include/srslte/common/logger_file.h
@@ -0,0 +1,76 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ * File:        logger_file.h
+ * Description: Common log object. Maintains a queue of log messages
+ *              and runs a thread to read messages and write to file.
+ *              Multiple producers, single consumer. If full, producers
+ *              increase queue size. If empty, consumer blocks.
+ *****************************************************************************/
+
+#ifndef LOGGER_FILE_H
+#define LOGGER_FILE_H
+
+#include <stdio.h>
+#include <deque>
+#include <string>
+#include "srslte/common/logger.h"
+#include "srslte/common/threads.h"
+
+namespace srslte {
+
+typedef std::string* str_ptr;
+
+class logger_file : public thread, public logger
+{
+public:
+  logger_file();
+  logger_file(std::string file);
+  ~logger_file();
+  void init(std::string file);
+  // Implementation of log_out
+  void log(str_ptr msg);
+  void log(const char *msg);
+
+private:
+  void run_thread(); 
+  void flush();
+
+  FILE*                 logfile;
+  bool                  inited;
+  bool                  not_done;
+  std::string           filename;
+  pthread_cond_t        not_empty;
+  pthread_cond_t        not_full;
+  pthread_mutex_t       mutex;
+  pthread_t             thread;
+  std::deque<str_ptr> buffer;
+};
+
+} // namespace srsue
+
+#endif // LOGGER_H
diff --git a/lib/include/srslte/common/logger_stdout.h b/lib/include/srslte/common/logger_stdout.h
new file mode 100644
index 000000000..00e4ffa7a
--- /dev/null
+++ b/lib/include/srslte/common/logger_stdout.h
@@ -0,0 +1,51 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ * File:        logger_stdout.h
+ * Description: Interface for logging output
+ *****************************************************************************/
+
+#ifndef LOGGER_STDOUT_H
+#define LOGGER_STDOUT_H
+
+#include <stdio.h>
+#include <string>
+#include "srslte/common/logger.h"
+
+namespace srslte {
+
+  class logger_stdout : public logger
+  {
+  public:
+    void log(std::string *msg) {
+      fprintf(stdout, "%s", msg->c_str());
+    }
+  };
+
+} // namespace srslte
+
+#endif // LOGGER_H
diff --git a/lib/src/common/log_filter.cc b/lib/src/common/log_filter.cc
index 12f004d5e..899a224d6 100644
--- a/lib/src/common/log_filter.cc
+++ b/lib/src/common/log_filter.cc
@@ -40,6 +40,11 @@ log_filter::log_filter()
   do_tti = false; 
 }
 
+log_filter::log_filter(std::string layer)
+{
+  init(layer, &def_logger_stdout, tti);
+}
+
 log_filter::log_filter(std::string layer, logger *logger_, bool tti)
 {
   init(layer, logger_, tti);
diff --git a/lib/src/common/log_stdout.cc b/lib/src/common/log_stdout.cc
deleted file mode 100644
index 2e50b755e..000000000
--- a/lib/src/common/log_stdout.cc
+++ /dev/null
@@ -1,290 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-#include <iostream>
-#include <sstream>
-#include <iomanip>
-#include <string>
-#include <string.h>
-#include <strings.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sys/time.h>
-#include <stdarg.h>
-#include <string>
-
-#include "srslte/common/log_stdout.h"
-
-
-using namespace std; 
-
-namespace srslte {
-
-void log_stdout::all_log(srslte::LOG_LEVEL_ENUM level,
-                         uint32_t               tti,
-                         char                  *msg)
-{
-  std::stringstream ss;
-
-  ss << now_time() << " ";
-  ss << "[" <<get_service_name() << "] ";
-  ss << log_level_text[level] << " ";
-  ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
-  ss << msg;
-
-  cout << ss.str();
-}
-
-void log_stdout::all_log(srslte::LOG_LEVEL_ENUM level,
-                         uint32_t               tti,
-                         char                  *msg,
-                         uint8_t               *hex,
-                         int                    size)
-{
-  std::stringstream ss;
-
-  ss << now_time() << " ";
-  ss << "[" <<get_service_name() << "] ";
-  ss << log_level_text[level] << " ";
-  ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
-  ss << msg << std::endl;
-  ss << hex_string(hex, size);
-
-  cout << ss.str();
-}
-
-void log_stdout::all_log_line(srslte::LOG_LEVEL_ENUM level,
-                              uint32_t               tti,
-                              std::string            file,
-                              int                    line,
-                              char                  *msg)
-{
-  std::stringstream ss;
-
-  ss << now_time() << " ";
-  ss << "[" <<get_service_name() << "] ";
-  ss << log_level_text[level] << " ";
-  ss << "[" << std::setfill('0') << std::setw(5) << tti << "] ";
-  ss << msg;
-
-  cout << ss.str();
-}
-
-void log_stdout::console(std::string message, ...) {
-  char     *args_msg;
-  va_list   args;
-  va_start(args, message);
-  if(vasprintf(&args_msg, message.c_str(), args) > 0)
-    printf("%s",args_msg); // Print directly to stdout
-  va_end(args);
-  free(args_msg);
-}
-
-void log_stdout::error(std::string message, ...) {
-  if (level >= LOG_LEVEL_ERROR) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_ERROR, tti, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::warning(std::string message, ...) {
-  if (level >= LOG_LEVEL_WARNING) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_WARNING, tti, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::info(std::string message, ...) {
-  if (level >= LOG_LEVEL_INFO) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_INFO, tti, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::debug(std::string message, ...) {
-  if (level >= LOG_LEVEL_DEBUG) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_DEBUG, tti, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_stdout::error_hex(uint8_t *hex, int size, std::string message, ...) {
-  if (level >= LOG_LEVEL_ERROR) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_ERROR, tti, args_msg, hex, size);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::warning_hex(uint8_t *hex, int size, std::string message, ...) {
-  if (level >= LOG_LEVEL_WARNING) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_WARNING, tti, args_msg, hex, size);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::info_hex(uint8_t *hex, int size, std::string message, ...) {
-  if (level >= LOG_LEVEL_INFO) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_INFO, tti, args_msg, hex, size);
-    va_end(args);
-    free(args_msg);
-  }
-}
-void log_stdout::debug_hex(uint8_t *hex, int size, std::string message, ...) {
-  if (level >= LOG_LEVEL_DEBUG) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log(LOG_LEVEL_DEBUG, tti, args_msg, hex, size);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_stdout::error_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_ERROR) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_ERROR, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_stdout::warning_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_WARNING) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_WARNING, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_stdout::info_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_INFO) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_INFO, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-void log_stdout::debug_line(std::string file, int line, std::string message, ...)
-{
-  if (level >= LOG_LEVEL_DEBUG) {
-    char     *args_msg;
-    va_list   args;
-    va_start(args, message);
-    if(vasprintf(&args_msg, message.c_str(), args) > 0)
-      all_log_line(LOG_LEVEL_DEBUG, tti, file, line, args_msg);
-    va_end(args);
-    free(args_msg);
-  }
-}
-
-
-
-std::string log_stdout::now_time()
-{
-  struct timeval rawtime;
-  struct tm * timeinfo;
-  char buffer[64];
-  char us[16];
-  
-  gettimeofday(&rawtime, NULL);
-  timeinfo = localtime(&rawtime.tv_sec);
-  
-  strftime(buffer,64,"%H:%M:%S",timeinfo);
-  strcat(buffer,".");
-  snprintf(us,16,"%ld",rawtime.tv_usec);
-  strcat(buffer,us);
-  
-  return std::string(buffer);
-}
-
-std::string log_stdout::hex_string(uint8_t *hex, int size)
-{
-  std::stringstream ss;
-  int c = 0;
-
-  ss << std::hex << std::setfill('0');
-  if(hex_limit >= 0) {
-    size = (size > hex_limit) ? hex_limit : size;
-  }
-  while(c < size) {
-    ss << "             " << std::setw(4) << static_cast<unsigned>(c) << ": ";
-    int tmp = (size-c < 16) ? size-c : 16;
-    for(int i=0;i<tmp;i++) {
-      ss << std::setw(2) << static_cast<unsigned>(hex[c++]) << " ";
-    }
-    ss << "\n";
-  }
-  return ss.str();
-}
-
-}
-
-  
diff --git a/lib/src/common/logger.cc b/lib/src/common/logger_file.cc
similarity index 85%
rename from lib/src/common/logger.cc
rename to lib/src/common/logger_file.cc
index b48f6e1c7..94e5e8405 100644
--- a/lib/src/common/logger.cc
+++ b/lib/src/common/logger_file.cc
@@ -27,20 +27,20 @@
 
 #define LOG_BUFFER_SIZE 1024*32
 
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 
 using namespace std;
 
 namespace srslte{
 
-logger::logger()
+logger_file::logger_file()
   :inited(false)
   ,not_done(true)
 {}
 
-logger::~logger() {
+logger_file::~logger_file() {
   not_done = false;
-  log("Closing log");
+  log(new std::string("Closing log"));
   if(inited) {
     wait_thread_finish();
     flush();
@@ -48,7 +48,7 @@ logger::~logger() {
   }
 }
 
-void logger::init(std::string file) {
+void logger_file::init(std::string file) {
   pthread_mutex_init(&mutex, NULL); 
   pthread_cond_init(&not_empty, NULL);
   pthread_cond_init(&not_full, NULL);
@@ -61,19 +61,18 @@ void logger::init(std::string file) {
   inited = true;
 }
 
-void logger::log(const char *msg) {
-  str_ptr s_ptr(new std::string(msg));
-  log(s_ptr);
+void logger_file::log(const char *msg) {
+  log(new std::string(msg));
 }
 
-void logger::log(str_ptr msg) {
+void logger_file::log(str_ptr msg) {
   pthread_mutex_lock(&mutex);
   buffer.push_back(msg);
   pthread_cond_signal(&not_empty);
   pthread_mutex_unlock(&mutex);
 }
 
-void logger::run_thread() {
+void logger_file::run_thread() {
   while(not_done) {
   pthread_mutex_lock(&mutex);
     while(buffer.empty()) {
@@ -89,7 +88,7 @@ void logger::run_thread() {
   }
 }
 
-void logger::flush() {
+void logger_file::flush() {
   std::deque<str_ptr>::iterator it;
   for(it=buffer.begin();it!=buffer.end();it++)
   {
diff --git a/lib/test/common/log_filter_test.cc b/lib/test/common/log_filter_test.cc
index d48821dac..350e42837 100644
--- a/lib/test/common/log_filter_test.cc
+++ b/lib/test/common/log_filter_test.cc
@@ -29,11 +29,12 @@
 
 #include <stdio.h>
 #include "srslte/common/log_filter.h"
+#include "srslte/common/logger_file.h"
 
 using namespace srslte;
 
 typedef struct {
-  logger *l;
+  logger_file *l;
   int thread_id;
 }args_t;
 
@@ -78,7 +79,7 @@ void* thread_loop_hex(void *a) {
 }
 
 void write(std::string filename) {
-  logger l;
+  logger_file l;
   l.init(filename);
   pthread_t threads[NTHREADS];
   args_t    args[NTHREADS];
diff --git a/lib/test/common/logger_test.cc b/lib/test/common/logger_test.cc
index 1baf5260f..845d4c104 100644
--- a/lib/test/common/logger_test.cc
+++ b/lib/test/common/logger_test.cc
@@ -29,12 +29,12 @@
 
 #include <stdlib.h>
 #include <stdio.h>
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 
 using namespace srslte;
 
 typedef struct {
-  logger *l;
+  logger_file *l;
   int thread_id;
 }args_t;
 
@@ -44,13 +44,13 @@ void* thread_loop(void *a) {
   for(int i=0;i<NMSGS;i++)
   {
     sprintf(buf, "Thread %d: %d", args->thread_id, i);
-    args->l->log(buf);
+    args->l->log(new std::string(buf));
   }
   return NULL;
 }
 
 void write(std::string filename) {
-  logger l;
+  logger_file l;
   l.init(filename);
   pthread_t threads[NTHREADS];
   args_t    args[NTHREADS];
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index bacc819ec..dfe62905e 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -25,7 +25,8 @@
  */
 
 #include <iostream>
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
+#include "srslte/common/logger_stdout.h"
 #include "srslte/upper/rlc_am.h"
 #include <assert.h>
 #define NBUFS 5
@@ -73,8 +74,8 @@ public:
 
 void basic_test()
 {
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -152,8 +153,8 @@ void basic_test()
 
 void concat_test()
 {
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -216,8 +217,8 @@ void concat_test()
 
 void segment_test()
 {
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -298,8 +299,8 @@ void segment_test()
 
 void retx_test()
 {
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -394,8 +395,8 @@ void resegment_test_1()
   // PDUs:                |  10  |  10  |  10  |  10  |  10  |
   // Retx PDU segments:                 | 5 | 5|
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -503,8 +504,8 @@ void resegment_test_2()
   // PDUs:              | 5 |  10  |     20     |  10  | 5 |
   // Retx PDU segments:            |  10  |  10 |
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -609,8 +610,8 @@ void resegment_test_3()
   // PDUs:              | 5 | 5|      20     |  10  |  10  |
   // Retx PDU segments:        |  10  |  10  |
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -711,8 +712,8 @@ void resegment_test_4()
   // PDUs:              | 5 | 5|         30         | 5 | 5|
   // Retx PDU segments:        |    15    |    15   |
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -813,8 +814,8 @@ void resegment_test_5()
   // PDUs:              |2|3|            40            |3|2|
   // Retx PDU segments:     |     20      |     20     |
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -914,8 +915,8 @@ void resegment_test_6()
   // PDUs:                |10|10|10|                270               | 54 |
   // Retx PDU segments:            |  120           |      150        |
 
-  srslte::log_stdout log1("RLC_AM_1");
-  srslte::log_stdout log2("RLC_AM_2");
+  srslte::log_filter log1("RLC_AM_1");
+  srslte::log_filter log2("RLC_AM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
diff --git a/lib/test/upper/rlc_um_test.cc b/lib/test/upper/rlc_um_test.cc
index dfc2883d8..491d816cc 100644
--- a/lib/test/upper/rlc_um_test.cc
+++ b/lib/test/upper/rlc_um_test.cc
@@ -25,7 +25,7 @@
  */
 
 #include <iostream>
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/upper/rlc_um.h"
 #include <assert.h>
 
@@ -78,8 +78,8 @@ public:
 
 void basic_test()
 {
-  srslte::log_stdout log1("RLC_UM_1");
-  srslte::log_stdout log2("RLC_UM_2");
+  srslte::log_filter log1("RLC_UM_1");
+  srslte::log_filter log2("RLC_UM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
@@ -146,8 +146,8 @@ void basic_test()
 
 void loss_test()
 {
-  srslte::log_stdout log1("RLC_UM_1");
-  srslte::log_stdout log2("RLC_UM_2");
+  srslte::log_filter log1("RLC_UM_1");
+  srslte::log_filter log2("RLC_UM_2");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log2.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index 8db5c85b6..b28d12ab4 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -50,7 +50,7 @@
 #include "srslte/common/bcd_helpers.h"
 #include "srslte/common/buffer_pool.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 #include "srslte/common/log_filter.h"
 #include "srslte/common/mac_pcap.h"
 #include "srslte/interfaces/sched_interface.h"
@@ -173,15 +173,15 @@ private:
   srsenb::gtpu       gtpu;
   srsenb::s1ap       s1ap;
 
-  srslte::logger     logger;
-  srslte::log_filter rf_log;
-  std::vector<void*> phy_log;
-  srslte::log_filter mac_log;
-  srslte::log_filter rlc_log;
-  srslte::log_filter pdcp_log;
-  srslte::log_filter rrc_log;
-  srslte::log_filter gtpu_log;
-  srslte::log_filter s1ap_log;
+  srslte::logger_file logger;
+  srslte::log_filter  rf_log;
+  std::vector<void*>  phy_log;
+  srslte::log_filter  mac_log;
+  srslte::log_filter  rlc_log;
+  srslte::log_filter  pdcp_log;
+  srslte::log_filter  rrc_log;
+  srslte::log_filter  gtpu_log;
+  srslte::log_filter  s1ap_log;
 
   srslte::byte_buffer_pool *pool;
 
diff --git a/srsenb/test/mac/scheduler_test.cc b/srsenb/test/mac/scheduler_test.cc
index 53bb28e18..e9afd61cb 100644
--- a/srsenb/test/mac/scheduler_test.cc
+++ b/srsenb/test/mac/scheduler_test.cc
@@ -6,7 +6,7 @@
 
 #include "srslte/interfaces/enb_interfaces.h"
 #include "srslte/interfaces/sched_interface.h"
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/radio/radio.h"
 #include "srslte/phy/utils/debug.h"
 
@@ -62,7 +62,7 @@ private:
 
 
 // Create classes
-srslte::log_stdout   log_out("ALL");
+srslte::log_filter   log_out("ALL");
 srsenb::sched        my_sched;
 srsenb::dl_metric_rr dl_metric;
 srsenb::ul_metric_rr ul_metric;
diff --git a/srsenb/test/upper/ip_test.cc b/srsenb/test/upper/ip_test.cc
index f2ec84f64..41c430f29 100644
--- a/srsenb/test/upper/ip_test.cc
+++ b/srsenb/test/upper/ip_test.cc
@@ -16,7 +16,7 @@
 #include "srslte/interfaces/enb_interfaces.h"
 #include "srslte/common/common.h"
 #include "srslte/common/buffer_pool.h"
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 #include "srslte/common/log_filter.h"
 #include "srslte/upper/rlc.h"
 #include "srslte/radio/radio.h"
@@ -320,11 +320,11 @@ private:
 
 
 // Create classes
-srslte::logger logger;
-srslte::log_filter log_phy;
-srslte::log_filter log_mac;
-srslte::log_filter log_rlc;
-srslte::log_filter log_tester;
+srslte::logger_file logger;
+srslte::log_filter  log_phy;
+srslte::log_filter  log_mac;
+srslte::log_filter  log_rlc;
+srslte::log_filter  log_tester;
 srsenb::phy my_phy;
 srsenb::mac my_mac;
 srslte::rlc  my_rlc;
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index f401697e9..6fb593d55 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -49,7 +49,7 @@
 
 #include "srslte/common/buffer_pool.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 #include "srslte/common/log_filter.h"
 
 #include "ue_metrics_interface.h"
@@ -173,16 +173,16 @@ private:
   srslte::gw         gw;
   srsue::usim        usim;
 
-  srslte::logger     logger;
-  srslte::log_filter rf_log;
-  srslte::log_filter phy_log;
-  srslte::log_filter mac_log;
-  srslte::log_filter rlc_log;
-  srslte::log_filter pdcp_log;
-  srslte::log_filter rrc_log;
-  srslte::log_filter nas_log;
-  srslte::log_filter gw_log;
-  srslte::log_filter usim_log;
+  srslte::logger_file logger;
+  srslte::log_filter  rf_log;
+  srslte::log_filter  phy_log;
+  srslte::log_filter  mac_log;
+  srslte::log_filter  rlc_log;
+  srslte::log_filter  pdcp_log;
+  srslte::log_filter  rrc_log;
+  srslte::log_filter  nas_log;
+  srslte::log_filter  gw_log;
+  srslte::log_filter  usim_log;
 
   srslte::byte_buffer_pool *pool;
 
diff --git a/srsue/test/mac/mac_test.cc b/srsue/test/mac/mac_test.cc
index 7c7e19cf0..cb2ad2945 100644
--- a/srsue/test/mac/mac_test.cc
+++ b/srsue/test/mac/mac_test.cc
@@ -32,7 +32,7 @@
 #include "srslte/radio/radio_multi.h"
 #include "phy/phy.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "mac/mac.h"
 #include "srslte/common/mac_pcap.h"
 
@@ -427,7 +427,7 @@ private:
 
 int main(int argc, char *argv[])
 {
-  srslte::log_stdout mac_log("MAC"), phy_log("PHY"); 
+  srslte::log_filter mac_log("MAC"), phy_log("PHY");
   rlctest my_rlc;  
   parse_args(&prog_args, argc, argv);
   
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index 91df6a2fc..76adcacdf 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -29,7 +29,7 @@
 #include "srslte/phy/utils/debug.h"
 #include "phy/phy.h"
 #include "srslte/interfaces/ue_interfaces.h"
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/radio/radio_multi.h"
 
 /**********************************************************************
@@ -330,7 +330,7 @@ srslte::radio_multi   radio;
   
 int main(int argc, char *argv[])
 {
-  srslte::log_stdout log("PHY");
+  srslte::log_filter log("PHY");
   
   parse_args(&prog_args, argc, argv);
 
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index 08116b22c..10e78b23a 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -28,7 +28,7 @@
 
 #include "srslte/phy/utils/debug.h"
 #include "phy/phy.h"
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/radio/radio_multi.h"
 
@@ -153,7 +153,7 @@ srslte::radio_multi radio;
 
 int main(int argc, char *argv[])
 {
-  srslte::log_stdout log("PHY");
+  srslte::log_filter log("PHY");
   
   parse_args(&prog_args, argc, argv);
 
diff --git a/srsue/test/upper/ip_test.cc b/srsue/test/upper/ip_test.cc
index a26f273a8..93021cd2b 100644
--- a/srsue/test/upper/ip_test.cc
+++ b/srsue/test/upper/ip_test.cc
@@ -21,7 +21,7 @@
 #include "srslte/common/threads.h"
 #include "srslte/common/common.h"
 #include "srslte/common/buffer_pool.h"
-#include "srslte/common/logger.h"
+#include "srslte/common/logger_file.h"
 #include "srslte/common/log_filter.h"
 #include "srslte/upper/rlc.h"
 #include "upper/rrc.h"
@@ -464,12 +464,12 @@ private:
 
 
 // Create classes
-srslte::logger logger;
-srslte::log_filter log_phy;
-srslte::log_filter log_mac;
-srslte::log_filter log_rlc;
-srslte::log_filter log_tester;
-srslte::mac_pcap   mac_pcap;
+srslte::logger_file logger;
+srslte::log_filter  log_phy;
+srslte::log_filter  log_mac;
+srslte::log_filter  log_rlc;
+srslte::log_filter  log_tester;
+srslte::mac_pcap    mac_pcap;
 srsue::phy my_phy;
 srsue::mac my_mac;
 srslte::rlc rlc;
diff --git a/srsue/test/upper/rrc_reconfig_test.cc b/srsue/test/upper/rrc_reconfig_test.cc
index 25629bcd7..c9df04e32 100644
--- a/srsue/test/upper/rrc_reconfig_test.cc
+++ b/srsue/test/upper/rrc_reconfig_test.cc
@@ -26,12 +26,12 @@
 
 #include <iostream>
 #include <srslte/srslte.h>
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include "srslte/asn1/liblte_rrc.h"
 #include "srslte/asn1/liblte_mme.h"
 
 void nas_test() {
-  srslte::log_stdout log1("NAS");
+  srslte::log_filter log1("NAS");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
 
@@ -91,7 +91,7 @@ void nas_test() {
 }
 
 void basic_test() {
-  srslte::log_stdout log1("RRC");
+  srslte::log_filter log1("RRC");
   log1.set_level(srslte::LOG_LEVEL_DEBUG);
   log1.set_hex_limit(-1);
 
diff --git a/srsue/test/upper/usim_test.cc b/srsue/test/upper/usim_test.cc
index 8e7d53bfe..c8a248184 100644
--- a/srsue/test/upper/usim_test.cc
+++ b/srsue/test/upper/usim_test.cc
@@ -26,7 +26,7 @@
 
 #include <iostream>
 #include "upper/usim.h"
-#include "srslte/common/log_stdout.h"
+#include "srslte/common/log_filter.h"
 #include <assert.h>
 
 using namespace srsue;
@@ -67,7 +67,7 @@ uint16 mnc = 93;
 
 int main(int argc, char **argv)
 {
-  srslte::log_stdout usim_log("USIM");
+  srslte::log_filter usim_log("USIM");
   bool    net_valid;
   uint8_t res[16];
 

From 776c1bbc9da8240adbeb4e98d6857ccad021e984 Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Fri, 23 Jun 2017 15:51:36 +0100
Subject: [PATCH 012/269] Moving fftw checks to top CMakeLists

---
 CMakeLists.txt             | 12 ++++++++++++
 lib/src/phy/CMakeLists.txt | 13 -------------
 srsenb/src/CMakeLists.txt  |  4 +++-
 srsue/src/CMakeLists.txt   |  2 ++
 4 files changed, 17 insertions(+), 14 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 17609e65f..62a56371b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,6 +76,18 @@ set(GCC_ARCH native CACHE STRING "GCC compile for specific architecture.")
 ########################################################################
 find_package(Threads REQUIRED)
 
+find_package(MKL)
+if(MKL_FOUND)
+  include_directories(${MKL_INCLUDE_DIRS})
+  link_directories(${MKL_LIBRARY_DIRS})
+else(MKL_FOUND)
+  find_package(FFTW3F REQUIRED)
+  if(FFTW3F_FOUND)
+    include_directories(${FFTW3F_INCLUDE_DIRS})
+    link_directories(${FFTW3F_LIBRARY_DIRS})
+  endif(FFTW3F_FOUND)
+endif(MKL_FOUND)
+
 find_package(Polarssl)
 if (POLARSSL_FOUND)
   set(SEC_INCLUDE_DIRS "${POLARSSL_INCLUDE_DIRS}")
diff --git a/lib/src/phy/CMakeLists.txt b/lib/src/phy/CMakeLists.txt
index 26271f2a7..7f26f9836 100644
--- a/lib/src/phy/CMakeLists.txt
+++ b/lib/src/phy/CMakeLists.txt
@@ -58,19 +58,6 @@ add_library(srslte_phy STATIC ${srslte_srcs})
 set_target_properties(srslte_phy PROPERTIES
                                  VERSION ${SRSLTE_VERSION_MAJOR}.${SRSLTE_VERSION_MINOR})
 
-find_package(MKL)
-if(MKL_FOUND)
-  include_directories(${MKL_INCLUDE_DIRS})
-  link_directories(${MKL_LIBRARY_DIRS})
-else(MKL_FOUND)
-  find_package(FFTW3F REQUIRED)
-  if(FFTW3F_FOUND)
-    include_directories(${FFTW3F_INCLUDE_DIRS})
-    link_directories(${FFTW3F_LIBRARY_DIRS})
-  endif(FFTW3F_FOUND)
-endif(MKL_FOUND)
-
-
 if(MKL_FOUND)
   if(BUILD_STATIC)
     target_link_libraries(srslte_phy ${MKL_STATIC_LIBRARIES})
diff --git a/srsenb/src/CMakeLists.txt b/srsenb/src/CMakeLists.txt
index 23272ea85..a83e974a8 100644
--- a/srsenb/src/CMakeLists.txt
+++ b/srsenb/src/CMakeLists.txt
@@ -31,7 +31,9 @@ target_link_libraries(srsenb  srsenb_upper
 
 if (RPATH)
   set_target_properties(srsenb PROPERTIES INSTALL_RPATH ".")
-endif (RPATH)                          
+endif (RPATH)
+
+install(TARGETS srsenb DESTINATION ${RUNTIME_DIR})
 
 ########################################################################
 # Option to run command after build (useful for remote builds)
diff --git a/srsue/src/CMakeLists.txt b/srsue/src/CMakeLists.txt
index 1c6b87bc1..4b2a7e282 100644
--- a/srsue/src/CMakeLists.txt
+++ b/srsue/src/CMakeLists.txt
@@ -46,6 +46,8 @@ if (RPATH)
   set_target_properties(srsue PROPERTIES INSTALL_RPATH ".")
 endif (RPATH)
 
+install(TARGETS srsue DESTINATION ${RUNTIME_DIR})
+
 ########################################################################
 # Option to run command after build (useful for remote builds)
 ########################################################################

From 14450827fa5ecd86aff4f3b89af2a3926b2f598d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 23 Jun 2017 17:47:23 +0200
Subject: [PATCH 013/269] added suppress_stdout to radio::init_multi

---
 lib/src/radio/radio_multi.cc | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/lib/src/radio/radio_multi.cc b/lib/src/radio/radio_multi.cc
index c8de57f46..0bb538c8d 100644
--- a/lib/src/radio/radio_multi.cc
+++ b/lib/src/radio/radio_multi.cc
@@ -14,9 +14,11 @@ bool radio_multi::init_multi(uint32_t nof_rx_antennas, char* args, char* devname
   burst_preamble_samples = 0; 
   burst_preamble_time_rounded = 0; 
   cur_tx_srate = 0; 
-  is_start_of_burst = true; 
-  
-  
+  is_start_of_burst = true;
+
+  // Suppress radio stdout
+  srslte_rf_suppress_stdout(&rf_device);
+
   tx_adv_auto = true; 
   // Set default preamble length each known device
   // We distinguish by device family, maybe we should calibrate per device

From fd08dd56bab4b61b6545ad7e86dfdf40a75bb794 Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Fri, 23 Jun 2017 16:47:54 +0100
Subject: [PATCH 014/269] Fix for security library find scripts

---
 CMakeLists.txt                   | 2 ++
 cmake/modules/FindMbedTLS.cmake  | 2 +-
 cmake/modules/FindPolarssl.cmake | 2 +-
 3 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 62a56371b..e40737900 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -107,6 +107,8 @@ else(POLARSSL_FOUND)
       set(SEC_LIBRARIES    "${MBEDTLS_LIBRARIES}")
     endif(BUILD_STATIC)
     add_definitions(-DHAVE_MBEDTLS)
+  else(MBEDTLS_FOUND)
+    message(FATAL_ERROR "Either polarssl or mbedtls is required to compile srsLTE")
   endif (MBEDTLS_FOUND)
 endif(POLARSSL_FOUND)
 
diff --git a/cmake/modules/FindMbedTLS.cmake b/cmake/modules/FindMbedTLS.cmake
index 11499bad4..2c9464e79 100644
--- a/cmake/modules/FindMbedTLS.cmake
+++ b/cmake/modules/FindMbedTLS.cmake
@@ -50,5 +50,5 @@ message(STATUS "MBEDTLS STATIC LIBRARIES: " ${MBEDTLS_STATIC_LIBRARIES})
 message(STATUS "MBEDTLS INCLUDE DIRS: " ${MBEDTLS_INCLUDE_DIRS})
 
 INCLUDE(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_INCLUDE_DIRS)
 MARK_AS_ADVANCED(MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
diff --git a/cmake/modules/FindPolarssl.cmake b/cmake/modules/FindPolarssl.cmake
index 6e6c5c2cb..58f21d13e 100644
--- a/cmake/modules/FindPolarssl.cmake
+++ b/cmake/modules/FindPolarssl.cmake
@@ -49,5 +49,5 @@ message(STATUS "POLARSSL STATIC LIBRARIES: " ${POLARSSL_STATIC_LIBRARIES})
 message(STATUS "POLARSSL INCLUDE DIRS: " ${POLARSSL_INCLUDE_DIRS})
 
 INCLUDE(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(POLARSSL DEFAULT_MSG POLARSSL_LIBRARIES POLARSSL_STATIC_LIBRARIES POLARSSL_INCLUDE_DIRS)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(POLARSSL DEFAULT_MSG POLARSSL_LIBRARIES POLARSSL_INCLUDE_DIRS)
 MARK_AS_ADVANCED(POLARSSL_STATIC_LIBRARIES POLARSSL_LIBRARIES POLARSSL_INCLUDE_DIRS)

From f48b1f3d66c39365ffb3752609b6b47a8192046f Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Fri, 23 Jun 2017 17:20:35 +0100
Subject: [PATCH 015/269] Adding ability to statically link FFTW

---
 cmake/modules/FindFFTW3F.cmake | 15 +++++++++++++--
 lib/src/phy/CMakeLists.txt     |  7 ++++++-
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/cmake/modules/FindFFTW3F.cmake b/cmake/modules/FindFFTW3F.cmake
index ebaa7c6df..ef7bc1397 100644
--- a/cmake/modules/FindFFTW3F.cmake
+++ b/cmake/modules/FindFFTW3F.cmake
@@ -15,6 +15,12 @@ find_path(FFTW3F_INCLUDE_DIR
             PATHS /usr/local/include 
                   /usr/include )
 
+find_library(FFTW3F_STATIC_LIBRARY
+            NAMES fftw3f.a libfftw3f.a libfftw3f-3.a
+            HINTS ${PC_FFTW3F_LIBDIR} ${PC_FFTW3F_LIBRARY_DIRS} $ENV{FFTW3_DIR}/lib
+            PATHS /usr/local/lib
+                  /usr/lib)
+
 find_library(FFTW3F_LIBRARY 
             NAMES fftw3f libfftw3f libfftw3f-3
             HINTS ${PC_FFTW3F_LIBDIR} ${PC_FFTW3F_LIBRARY_DIRS} $ENV{FFTW3_DIR}/lib
@@ -22,12 +28,17 @@ find_library(FFTW3F_LIBRARY
                   /usr/lib)
 
 set(FFTW3F_LIBRARIES ${FFTW3F_LIBRARY} )
+set(FFTW3F_STATIC_LIBRARIES ${FFTW3F_STATIC_LIBRARY} )
 set(FFTW3F_INCLUDE_DIRS ${FFTW3F_INCLUDE_DIR} )
 
+message(STATUS "FFTW3F LIBRARIES: " ${FFTW3F_LIBRARIES})
+message(STATUS "FFTW3F STATIC LIBRARIES: " ${FFTW3F_STATIC_LIBRARIES})
+message(STATUS "FFTW3F INCLUDE DIRS: " ${FFTW3F_INCLUDE_DIRS})
+
 include(FindPackageHandleStandardArgs)
 # handle the QUIETLY and REQUIRED arguments and set FFTW3F_FOUND to TRUE
 # if all listed variables are TRUE
 find_package_handle_standard_args(fftw3f  DEFAULT_MSG
-                                  FFTW3F_LIBRARY FFTW3F_INCLUDE_DIR)
+                                  FFTW3F_LIBRARY FFTW3F_STATIC_LIBRARY FFTW3F_INCLUDE_DIR)
 
-mark_as_advanced(FFTW3F_INCLUDE_DIR FFTW3F_LIBRARY )
+mark_as_advanced(FFTW3F_INCLUDE_DIR FFTW3F_STATIC_LIBRARY FFTW3F_LIBRARY )
diff --git a/lib/src/phy/CMakeLists.txt b/lib/src/phy/CMakeLists.txt
index 7f26f9836..bd5ae332d 100644
--- a/lib/src/phy/CMakeLists.txt
+++ b/lib/src/phy/CMakeLists.txt
@@ -65,7 +65,12 @@ if(MKL_FOUND)
     target_link_libraries(srslte_phy ${MKL_LIBRARIES})
   endif(BUILD_STATIC)
 else(MKL_FOUND)
-  target_link_libraries(srslte_phy ${FFTW3F_LIBRARIES})
+  if(BUILD_STATIC)
+    target_link_libraries(srslte_phy ${FFTW3F_STATIC_LIBRARIES})
+  else(BUILD_STATIC)
+    target_link_libraries(srslte_phy ${FFTW3F_LIBRARIES})
+  endif(BUILD_STATIC)
+
 endif(MKL_FOUND)
 
 

From fa9eaee498fda7184df4ee0f01f48c21a4c960e7 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 23 Jun 2017 19:15:30 +0200
Subject: [PATCH 016/269] new NAS/RRC structure. Attaching and ping working

---
 lib/include/srslte/common/bcd_helpers.h |    7 +
 lib/src/phy/rf/rf_uhd_imp.c             |    4 +-
 lib/src/radio/radio.cc                  |    4 +-
 srsue/hdr/phy/phch_recv.h               |   36 +-
 srsue/hdr/ue.h                          |   25 +-
 srsue/hdr/upper/nas.h                   |    3 +
 srsue/hdr/upper/rrc.h                   |    3 +-
 srsue/src/mac/mac.cc                    |    2 +-
 srsue/src/main.cc                       |    5 +-
 srsue/src/phy/phch_recv.cc              |  213 +-
 srsue/src/ue.cc                         |   12 +-
 srsue/src/upper/nas.cc                  |  948 ++++----
 srsue/src/upper/rrc.cc                  | 2696 +++++++++++------------
 13 files changed, 1997 insertions(+), 1961 deletions(-)

diff --git a/lib/include/srslte/common/bcd_helpers.h b/lib/include/srslte/common/bcd_helpers.h
index d44a66483..55411ae33 100644
--- a/lib/include/srslte/common/bcd_helpers.h
+++ b/lib/include/srslte/common/bcd_helpers.h
@@ -30,6 +30,7 @@
 #include <ctype.h>
 #include <stdint.h>
 #include <string>
+#include <srslte/asn1/liblte_rrc.h>
 
 namespace srslte {
 
@@ -112,6 +113,12 @@ inline bool mnc_to_string(uint16_t mnc, std::string *str)
   *str += (mnc & 0x000F) + '0';
   return true;
 }
+inline std::string plmn_id_to_c_str(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+  std::string mcc_str, mnc_str;
+  mnc_to_string(plmn_id.mnc, &mnc_str);
+  mcc_to_string(plmn_id.mcc, &mcc_str);
+  return mcc_str + mnc_str;
+}
 
 } // namespace srslte
 
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 319093994..5b80df0ec 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -623,7 +623,9 @@ int rf_uhd_recv_with_time_multi(void *h,
         log_overflow(handler);
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_LATE_COMMAND) {
         log_late(handler);
-      } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE) {
+      } else if (error_code == UHD_RX_METADATA_ERROR_CODE_TIMEOUT) {
+        fprintf(stderr, "Error timed out while receiving asynchronoous messages from UHD.\n");
+      } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE ) {
         fprintf(stderr, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
       }
       
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index bc74660b0..66f1a10f8 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -47,7 +47,7 @@ bool radio::init(char *args, char *devname)
   cur_tx_srate = 0; 
   is_start_of_burst = true; 
   
-  // Suppress radio stdout 
+  // Suppress radio stdout
   srslte_rf_suppress_stdout(&rf_device);
   
   tx_adv_auto = true; 
@@ -389,8 +389,6 @@ void radio::set_tx_srate(float srate)
   
   // Calculate TX advance in seconds from samples and sampling rate 
   tx_adv_sec = nsamples/cur_tx_srate;
-  
-  printf("Setting TX/RX offset %d samples, %.2f us\n", nsamples, tx_adv_sec*1e6);
 }
 
 void radio::start_rx()
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 59fa10ebc..0bfae26c9 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -74,13 +74,12 @@ private:
 
   void   set_ue_sync_opts(srslte_ue_sync_t *q); 
   void   run_thread();
-  int    sync_sfn();
-  
+
   bool   running; 
   
-  srslte::radio_multi        *radio_h;
+  srslte::radio_multi  *radio_h;
   mac_interface_phy    *mac;
-  rrc_interface_phy *rrc;
+  rrc_interface_phy    *rrc;
   srslte::log          *log_h;
   srslte::thread_pool  *workers_pool;
   phch_common          *worker_com;
@@ -91,16 +90,20 @@ private:
   
   uint32_t      nof_rx_antennas;
 
-  cf_t *sf_buffer_sfn[SRSLTE_MAX_PORTS]; 
+  cf_t *sf_buffer[SRSLTE_MAX_PORTS];
 
   // Sync metrics
   sync_metrics_t metrics;
 
   enum {
-    IDLE, CELL_SEARCH, CELL_SELECT, CAMPING
-  } phy_state; 
+    IDLE, CELL_SEARCH, CELL_MEASURE, CELL_SELECT, CELL_CAMP
+  } phy_state;
+
+  enum {
+    SRATE_NONE=0, SRATE_FIND, SRATE_CAMP
+  } srate_mode;
 
-  srslte_cell_t cell; 
+  srslte_cell_t cell;
   bool          cell_is_set;
   bool          is_sfn_synched; 
   bool          started; 
@@ -118,10 +121,19 @@ private:
   const static uint32_t SYNC_SFN_TIMEOUT = 5000;
   float ul_dl_factor;
   int cur_earfcn_index;
-  
-  bool          cell_search(int force_N_id_2 = -1);
-  bool          init_cell();
-  void          free_cell();
+  bool cell_search_in_progress;
+  uint32_t measure_cnt;
+  float    measure_rsrp;
+  srslte_ue_dl_t ue_dl_measure;
+
+
+  const static int RSRP_MEASURE_NOF_FRAMES = 20;
+
+  int    cell_sync_sfn();
+  int    cell_meas_rsrp();
+  bool   cell_search(int force_N_id_2 = -1);
+  bool   init_cell();
+  void   free_cell();
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 6fb593d55..60aff8469 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -61,15 +61,16 @@ namespace srsue {
 *******************************************************************************/
 
 typedef struct {
+  uint32_t      dl_earfcn;
   float         dl_freq;
   float         ul_freq;
   float         rx_gain;
   float         tx_gain;
-  uint32_t      nof_rx_ant; 
-  std::string   device_name; 
-  std::string   device_args; 
-  std::string   time_adv_nsamples; 
-  std::string   burst_preamble; 
+  uint32_t      nof_rx_ant;
+  std::string   device_name;
+  std::string   device_args;
+  std::string   time_adv_nsamples;
+  std::string   burst_preamble;
 }rf_args_t;
 
 typedef struct {
@@ -110,16 +111,16 @@ typedef struct {
 }gui_args_t;
 
 typedef struct {
-  phy_args_t phy; 
+  phy_args_t phy;
   float      metrics_period_secs;
   bool pregenerate_signals;
   int ue_cateogry;
-  
+
 }expert_args_t;
 
 typedef struct {
   rf_args_t     rf;
-  rf_cal_t      rf_cal; 
+  rf_cal_t      rf_cal;
   pcap_args_t   pcap;
   trace_args_t  trace;
   log_args_t    log;
@@ -144,7 +145,7 @@ public:
   void stop();
   bool is_attached();
   void start_plot();
-  
+
   static void rf_msg(srslte_rf_error_t error);
   void handle_rf_msg(srslte_rf_error_t error);
 
@@ -152,10 +153,10 @@ public:
   bool get_metrics(ue_metrics_t &m);
 
   void pregenerate_signals(bool enable);
-  
+
   // Testing
-  void test_con_restablishment(); 
-  
+  void test_con_restablishment();
+
 
 private:
   static ue *instance;
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index e21bff432..edf5dcf27 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -107,6 +107,9 @@ namespace srsue {
 
     plmn_selection_state_t plmn_selection;
     LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
+    LIBLTE_RRC_PLMN_IDENTITY_STRUCT home_plmn;
+
+    std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
 
     // Save short MAC
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index f33043d10..225659d6f 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -61,8 +61,7 @@ namespace srsue {
   typedef enum {
     SI_ACQUIRE_IDLE = 0,
     SI_ACQUIRE_SIB1,
-    SI_ACQUIRE_SIB2,
-    SI_ACQUIRE_DONE
+    SI_ACQUIRE_SIB2
   } si_acquire_state_t;
 
 
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 316320f31..5a74492e6 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -141,7 +141,7 @@ void mac::run_thread() {
 
   while(started) {
 
-    while (!phy_h->sync_status()) {
+    while (!phy_h->sync_status() && started) {
       usleep(5000);
       if (phy_h->sync_status()) {
         Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index f114c0606..965e6a918 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -61,8 +61,9 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
   // Command line or config file options
   bpo::options_description common("Configuration options");
   common.add_options()
-    ("rf.dl_freq", bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000), "Downlink centre frequency")
-    ("rf.ul_freq", bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000), "Uplink centre frequency")
+    ("rf.dl_earfcn", bpo::value<uint32_t>(&args->rf.dl_earfcn)->default_value(3400), "Downlink EARFCN")
+    ("rf.dl_freq", bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000), "(optional) Downlink centre frequency")
+    ("rf.ul_freq", bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000), "(optional) Uplink centre frequency")
     ("rf.rx_gain", bpo::value<float>(&args->rf.rx_gain)->default_value(-1), "Front-end receiver gain")
     ("rf.tx_gain", bpo::value<float>(&args->rf.tx_gain)->default_value(-1), "Front-end transmitter gain")
     ("rf.nof_rx_ant", bpo::value<uint32_t>(&args->rf.nof_rx_ant)->default_value(1), "Number of RX antennas")
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 7895ca66a..3dedcb606 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -25,6 +25,7 @@
  */
 
 #include <unistd.h>
+#include <srslte/srslte.h>
 #include "srslte/srslte.h"
 #include "srslte/common/log.h"
 #include "phy/phch_worker.h"
@@ -62,9 +63,11 @@ namespace srsue {
     time_adv_sec = 0;
     cell_is_set = false;
     sync_sfn_cnt = 0;
+    srate_mode = SRATE_NONE;
+    cell_search_in_progress = false;
 
     for (uint32_t i = 0; i < nof_rx_antennas; i++) {
-      sf_buffer_sfn[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
+      sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
     }
 
     nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
@@ -82,8 +85,8 @@ namespace srsue {
     running = false;
     wait_thread_finish();
     for (uint32_t i = 0; i < nof_rx_antennas; i++) {
-      if (sf_buffer_sfn[i]) {
-        free(sf_buffer_sfn[i]);
+      if (sf_buffer[i]) {
+        free(sf_buffer[i]);
       }
     }
   }
@@ -152,9 +155,14 @@ namespace srsue {
         // Set options defined in expert section
         set_ue_sync_opts(&ue_sync);
 
+        if (srslte_ue_dl_init_multi(&ue_dl_measure, cell, nof_rx_antennas)) {
+          Error("Setting cell: initiating ue_dl_measure\n");
+          return false;
+        }
+
         for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
           if (!((phch_worker *) workers_pool->get_worker(i))->init_cell(cell)) {
-            Error("Error setting cell: initiating PHCH worker\n");
+            Error("Setting cell: initiating PHCH worker\n");
             return false;
           }
         }
@@ -179,6 +187,10 @@ namespace srsue {
       usleep(2000);
     }
 
+    srslte_ue_sync_free(&ue_sync);
+
+    srslte_ue_dl_free(&ue_dl_measure);
+
     if (cell_is_set) {
       for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
         ((phch_worker *) workers_pool->get_worker(i))->free_cell();
@@ -198,7 +210,6 @@ namespace srsue {
 
     bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
 
-    log_h->console("Searching for cell...\n");
     if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
                                         radio_h)) {
       Error("Initiating UE cell search\n");
@@ -214,7 +225,10 @@ namespace srsue {
       srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
     }
 
-    radio_h->set_rx_srate(1.92e6);
+    if (srate_mode != SRATE_FIND) {
+      srate_mode = SRATE_FIND;
+      radio_h->set_rx_srate(1.92e6);
+    }
     radio_h->start_rx();
 
     /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
@@ -246,9 +260,6 @@ namespace srsue {
     cell.cp = found_cells[max_peak_cell].cp;
     cellsearch_cfo = found_cells[max_peak_cell].cfo;
 
-    log_h->console("Found CELL ID: %d CP: %s, CFO: %.1f KHz.\nTrying to decode MIB...\n",
-                   cell.id, srslte_cp_string(cell.cp), cellsearch_cfo / 1000);
-
     srslte_ue_mib_sync_t ue_mib_sync;
 
     if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas,
@@ -281,10 +292,6 @@ namespace srsue {
     if (ret == 1) {
       srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
       worker_com->set_cell(cell);
-      srslte_cell_fprint(stdout, &cell, 0);
-
-      srslte_bit_pack_vector(bch_payload, bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN);
-      mac->bch_decoded_ok(bch_payload_bits, SRSLTE_BCH_PAYLOAD_LEN / 8);
       return true;
     } else {
       Warning("Error decoding MIB: Error decoding PBCH\n");
@@ -293,45 +300,6 @@ namespace srsue {
   }
 
 
-  int phch_recv::sync_sfn(void) {
-
-    int ret = SRSLTE_ERROR;
-    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
-
-    srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-    ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer_sfn);
-    if (ret < 0) {
-      Error("Error calling ue_sync_get_buffer");
-      return -1;
-    }
-
-    if (ret == 1) {
-      if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
-        int sfn_offset = 0;
-        Info("SYNC:  Decoding MIB...\n");
-        int n = srslte_ue_mib_decode(&ue_mib, sf_buffer_sfn[0], bch_payload, NULL, &sfn_offset);
-        if (n < 0) {
-          Error("Error decoding MIB while synchronising SFN");
-          return -1;
-        } else if (n == SRSLTE_UE_MIB_FOUND) {
-          uint32_t sfn;
-          srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
-
-          sfn = (sfn+sfn_offset)%1024;
-          tti = sfn * 10;
-
-          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-          Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
-          srslte_ue_mib_reset(&ue_mib);
-          return 1;
-        }
-      }
-    } else {
-      Debug("SYNC:  PSS/SSS not found...\n");
-    }
-    return 0;
-  }
-
   void phch_recv::resync_sfn() {
     sync_sfn_cnt = 0;
     phy_state = CELL_SELECT;
@@ -342,17 +310,17 @@ namespace srsue {
   }
 
   void phch_recv::cell_search_next() {
+    cell_search_in_progress = true;
     cur_earfcn_index++;
     if (cur_earfcn_index >= 0) {
-      if ((uint32_t) cur_earfcn_index >= earfcn.size() - 1) {
+      if (cur_earfcn_index >= (int) earfcn.size() - 1) {
         cur_earfcn_index = 0;
       }
       // If PHY is running, stop and free resources
       free_cell();
 
-      float dl_freq = srslte_band_fd(earfcn[cur_earfcn_index]);
+      float dl_freq = 1e6*srslte_band_fd(earfcn[cur_earfcn_index]);
       if (dl_freq >= 0) {
-        log_h->console("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz\n", earfcn[cur_earfcn_index], dl_freq / 1e6);
         log_h->info("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz, channel_index=%d\n", earfcn[cur_earfcn_index],
                     dl_freq / 1e6, cur_earfcn_index);
         radio_h->set_rx_freq(dl_freq);
@@ -366,25 +334,39 @@ namespace srsue {
   }
 
   void phch_recv::cell_search_start() {
-    cur_earfcn_index = -1;
-    log_h->console("Cell Search: Starting procedure...\n");
-    log_h->info("Cell Search: Starting procedure...\n");
-    cell_search_next();
+    if (earfcn.size() > 0) {
+      cur_earfcn_index = -1;
+      log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+      log_h->info("Cell Search: Starting procedure...\n");
+      cell_search_next();
+    } else {
+      log_h->info("Empty EARFCN list. Stopping cell search...\n");
+      log_h->console("Empty EARFCN list. Stopping cell search...\n");
+    }
   }
 
   bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
     free_cell();
 
-    float dl_freq = srslte_band_fd(earfcn);
-    float ul_freq = srslte_band_ul_earfcn(earfcn);
+    int cnt=0;
+    while(phy_state == CELL_SEARCH && cnt<100) {
+      usleep(10000);
+      log_h->info("PHY in CELL_SEARCH. Waiting...\n");
+    }
+    if (phy_state==CELL_SEARCH) {
+      log_h->warning("PHY still in CELL_SEARCH, forcing CELL_SELECT...\n");
+    }
+    float dl_freq = 1e6*srslte_band_fd(earfcn);
+    float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(earfcn));
     if (dl_freq >= 0 || ul_freq <= 0) {
-      log_h->console("Cell Select: Set EARFCN=%d, DL frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn,
-                     dl_freq / 1e6, ul_freq / 1e6);
       log_h->info("Cell Select: Set EARFCN=%d, frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn, dl_freq / 1e6,
                   ul_freq / 1e6);
       radio_h->set_rx_freq(dl_freq);
       radio_h->set_tx_freq(ul_freq);
 
+      ul_dl_factor = ul_freq/dl_freq;
+
+      cell_search_in_progress = false;
       this->cell = cell;
       if (init_cell()) {
         phy_state = CELL_SELECT;
@@ -398,6 +380,77 @@ namespace srsue {
     return false;
   }
 
+
+  int phch_recv::cell_sync_sfn(void) {
+
+    int ret = SRSLTE_ERROR;
+    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+
+    srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+    ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
+    if (ret < 0) {
+      Error("Error calling ue_sync_get_buffer");
+      return -1;
+    }
+
+    if (ret == 1) {
+      if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
+        int sfn_offset = 0;
+        Info("SYNC:  Decoding MIB...\n");
+        int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
+        if (n < 0) {
+          Error("SYNC:  Error decoding MIB while synchronising SFN");
+          return -1;
+        } else if (n == SRSLTE_UE_MIB_FOUND) {
+          uint32_t sfn;
+          srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
+
+          sfn = (sfn+sfn_offset)%1024;
+          tti = sfn * 10;
+
+          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+          Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
+          srslte_ue_mib_reset(&ue_mib);
+          return 1;
+        }
+      }
+    } else {
+      Debug("SYNC:  PSS/SSS not found...\n");
+    }
+    return 0;
+  }
+
+  int phch_recv::cell_meas_rsrp() {
+
+    uint32_t cfi = 0;
+
+    tti = (tti+1) % 10240;
+    log_h->step(tti);
+
+    uint32_t sf_idx = tti%10;
+
+    int sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
+    if (sync_res == 1) {
+      if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
+        log_h->error("SYNC:  Measuring RSRP: Estimating channel\n");
+        return -1;
+      }
+      float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl_measure.chest);
+      measure_rsrp = SRSLTE_VEC_CMA(rsrp, measure_rsrp, measure_cnt);
+      measure_cnt++;
+      log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm\n",
+                  measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000));
+      if (measure_cnt >= RSRP_MEASURE_NOF_FRAMES) {
+        return 1;
+      }
+    } else {
+      log_h->error("SYNC:  Measuring RSRP: Sync error\n");
+      return -1;
+    }
+
+    return 0;
+  }
+
   void phch_recv::run_thread() {
     int sync_res;
     phch_worker *worker = NULL;
@@ -406,7 +459,6 @@ namespace srsue {
       switch (phy_state) {
         case CELL_SEARCH:
           if (cell_search()) {
-            log_h->console("Initializating cell configuration...\n");
             init_cell();
             float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
 
@@ -416,12 +468,11 @@ namespace srsue {
               radio_h->set_master_clock_rate(23.04e6);
             }
 
-            log_h->console("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
+            log_h->info("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
+            srate_mode = SRATE_CAMP;
             radio_h->set_rx_srate(srate);
             radio_h->set_tx_srate(srate);
 
-            ul_dl_factor = radio_h->get_tx_freq() / radio_h->get_rx_freq();
-
             Info("SYNC:  Cell found. Synchronizing...\n");
             phy_state = CELL_SELECT;
             sync_sfn_cnt = 0;
@@ -438,14 +489,21 @@ namespace srsue {
             radio_is_streaming = true;
           }
 
-          switch (sync_sfn()) {
+          switch (cell_sync_sfn()) {
             default:
               log_h->console("Going IDLE\n");
               phy_state = IDLE;
               break;
             case 1:
               srslte_ue_sync_set_agc_period(&ue_sync, 20);
-              phy_state = CAMPING;
+              if (!cell_search_in_progress) {
+                phy_state = CELL_CAMP;
+                log_h->console("Sync OK. Camping on cell PCI=%d...\n", cell.id);
+              } else {
+                measure_cnt  = 0;
+                measure_rsrp = 0;
+                phy_state    = CELL_MEASURE;
+              }
               break;
             case 0:
               break;
@@ -459,10 +517,21 @@ namespace srsue {
             log_h->warning("Timeout while synchronizing SFN\n");
           }
           break;
-        case CAMPING:
+        case CELL_MEASURE:
+          switch(cell_meas_rsrp()) {
+            case 1:
+              rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
+              phy_state = CELL_CAMP;
+            case 0:
+              break;
+            default:
+              log_h->error("SYNC:  Getting RSRP cell measurement.\n");
+              cell_search_next();
+          }
+          break;
+        case CELL_CAMP:
           tti = (tti+1) % 10240;
           worker = (phch_worker *) workers_pool->wait_worker(tti);
-          sync_res = 0;
           if (worker) {
             for (uint32_t i = 0; i < nof_rx_antennas; i++) {
               buffer[i] = worker->get_buffer(i);
@@ -535,7 +604,7 @@ namespace srsue {
   }
 
   bool phch_recv::status_is_sync() {
-    return phy_state == CAMPING;
+    return phy_state == CELL_CAMP;
   }
 
   void phch_recv::get_current_cell(srslte_cell_t *cell_) {
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index ac98aeea1..2fd38c3c8 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -33,6 +33,7 @@
 #include <string>
 #include <algorithm>
 #include <iterator>
+#include <ue.h>
 
 using namespace srslte;
 
@@ -74,7 +75,7 @@ ue::~ue()
 bool ue::init(all_args_t *args_)
 {
   args     = args_;
-  
+
   logger.init(args->log.filename);
   rf_log.init("RF  ", &logger);
   phy_log.init("PHY ", &logger, true);
@@ -169,6 +170,7 @@ bool ue::init(all_args_t *args_)
   }
   if (args->rf.tx_gain > 0) {
     radio.set_tx_gain(args->rf.tx_gain);    
+    printf("set tx gain %f\n", args->rf.tx_gain);
   } else {
     radio.set_tx_gain(args->rf.rx_gain);
     std::cout << std::endl << 
@@ -189,6 +191,14 @@ bool ue::init(all_args_t *args_)
   gw.init(&pdcp, &rrc, this, &gw_log);
   usim.init(&args->usim, &usim_log);
 
+  // Currently EARFCN list is set to only one frequency as indicated in ue.conf
+  std::vector<uint32_t> earfcn_list;
+  earfcn_list.push_back(args->rf.dl_earfcn);
+  phy.set_earfcn(earfcn_list);
+
+  printf("\n\nRequesting NAS Attach...\n");
+  nas.attach_request();
+
   started = true;
   return true;
 }
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index be03ec840..f9bc88064 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -25,7 +25,9 @@
  */
 
 
+#include "srslte/asn1/liblte_rrc.h"
 #include "upper/nas.h"
+#include "srslte/common/bcd_helpers.h"
 
 using namespace srslte;
 
@@ -45,11 +47,9 @@ namespace srsue {
     gw = gw_;
     nas_log = nas_log_;
     state = EMM_STATE_DEREGISTERED;
-
-    // Manual PLMN selection procedure
-    current_plmn.mcc = 1;
-    current_plmn.mnc = 1;
-    plmn_selection = PLMN_SELECTED;
+    plmn_selection = PLMN_NOT_SELECTED;
+    home_plmn.mcc = 61441; // This is 001
+    home_plmn.mnc = 65281; // This is 01
   }
 
   void nas::stop() {}
@@ -62,11 +62,14 @@ namespace srsue {
   UE interface
 *******************************************************************************/
   void nas::attach_request() {
+    nas_log->info("Attach Request\n");
     if (state == EMM_STATE_DEREGISTERED) {
       state = EMM_STATE_REGISTERED_INITIATED;
       if (plmn_selection == PLMN_NOT_SELECTED) {
+        nas_log->info("Starting PLMN Search...\n");
         rrc->plmn_search();
       } else if (plmn_selection == PLMN_SELECTED) {
+        nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
         rrc->plmn_select(current_plmn);
       }
     } else {
@@ -83,239 +86,227 @@ namespace srsue {
   RRC interface
 *******************************************************************************/
 
-void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
-  // if it's the plmn we want rrc->plmn_select() and plmn_selection = PLMN_SELECTED
-}
+  void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
+
+    // Store PLMN if not registered
+    for (uint32_t i=0;i<known_plmns.size();i++) {
+      if (plmn_id.mcc == known_plmns[i].mcc && plmn_id.mnc == known_plmns[i].mnc) {
+        nas_log->info("Detected known PLMN %s\n", plmn_id_to_c_str(plmn_id).c_str());
+        if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+          rrc->plmn_select(plmn_id);
+        }
+        return;
+      }
+    }
+    nas_log->info("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                  tracking_area_code);
+    nas_log->console("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                  tracking_area_code);
+    if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+      rrc->plmn_select(plmn_id);
+    }
+  }
+
+  void nas::cell_selected() {
+    if (state == EMM_STATE_REGISTERED_INITIATED) {
+      rrc->connect();
+    } else {
+      nas_log->info("Cell selected in invalid state = %s\n", emm_state_text[state]);
+    }
+  }
 
-void nas::cell_selected() {
-  if (state == EMM_STATE_REGISTERED_INITIATED) {
-    rrc->connect();
-  } else {
-    nas_log->info("Cell selcted in invalid state = %s\n", emm_state_text[state]);
+  bool nas::is_attached() {
+    return state == EMM_STATE_REGISTERED;
   }
-}
-
-bool nas::is_attached()
-{
-  return state == EMM_STATE_REGISTERED;
-}
-
-void nas::notify_connection_setup()
-{
-  nas_log->debug("State = %s\n", emm_state_text[state]);
-  if(EMM_STATE_REGISTERED_INITIATED == state) {
-    send_attach_request();
-  } else {
-    send_service_request();
+
+  void nas::notify_connection_setup() {
+    nas_log->debug("State = %s\n", emm_state_text[state]);
+    if (EMM_STATE_REGISTERED_INITIATED == state) {
+      send_attach_request();
+    } else {
+      send_service_request();
+    }
   }
-}
-
-void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
-{
-  uint8 pd;
-  uint8 msg_type;
-
-  nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rb_id_text[lcid]);
-
-  // Parse the message
-  liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT*)pdu, &pd, &msg_type);
-  switch(msg_type)
-  {
-  case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
-      parse_attach_accept(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
-      parse_attach_reject(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
-      parse_authentication_request(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
-      parse_authentication_reject(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
-      parse_identity_request(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
-      parse_security_mode_command(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
-      parse_service_reject(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
-      parse_esm_information_request(lcid, pdu);
-      break;
-  case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
-      parse_emm_information(lcid, pdu);
-      break;
-  default:
-      nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n",msg_type);
-      pool->deallocate(pdu);
-      break;
+
+  void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+    uint8 pd;
+    uint8 msg_type;
+
+    nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rb_id_text[lcid]);
+
+    // Parse the message
+    liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT *) pdu, &pd, &msg_type);
+    switch (msg_type) {
+      case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
+        parse_attach_accept(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
+        parse_attach_reject(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
+        parse_authentication_request(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
+        parse_authentication_reject(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
+        parse_identity_request(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
+        parse_security_mode_command(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
+        parse_service_reject(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
+        parse_esm_information_request(lcid, pdu);
+        break;
+      case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
+        parse_emm_information(lcid, pdu);
+        break;
+      default:
+        nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n", msg_type);
+        pool->deallocate(pdu);
+        break;
+    }
+  }
+
+  uint32_t nas::get_ul_count() {
+    return count_ul;
   }
-}
-
-uint32_t  nas::get_ul_count()
-{
-  return count_ul;
-}
-
-bool      nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi)
-{
-  if(is_guti_set) {
-    s_tmsi->mmec   = guti.mme_code;
-    s_tmsi->m_tmsi = guti.m_tmsi;
-    return true;
-  } else {
-    return false;
+
+  bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
+    if (is_guti_set) {
+      s_tmsi->mmec = guti.mme_code;
+      s_tmsi->m_tmsi = guti.m_tmsi;
+      return true;
+    } else {
+      return false;
+    }
   }
-}
 
 /*******************************************************************************
   Security
 *******************************************************************************/
 
-void nas::integrity_generate(uint8_t  *key_128,
-                             uint32_t  count,
-                             uint8_t   rb_id,
-                             uint8_t   direction,
-                             uint8_t  *msg,
-                             uint32_t  msg_len,
-                             uint8_t  *mac)
-{
-  switch(integ_algo)
-  {
-  case INTEGRITY_ALGORITHM_ID_EIA0:
-    break;
-  case INTEGRITY_ALGORITHM_ID_128_EIA1:
-    security_128_eia1(key_128,
-                      count,
-                      rb_id,
-                      direction,
-                      msg,
-                      msg_len,
-                      mac);
-    break;
-  case INTEGRITY_ALGORITHM_ID_128_EIA2:
-    security_128_eia2(key_128,
-                      count,
-                      rb_id,
-                      direction,
-                      msg,
-                      msg_len,
-                      mac);
-    break;
-  default:
-    break;
+  void nas::integrity_generate(uint8_t *key_128,
+                               uint32_t count,
+                               uint8_t rb_id,
+                               uint8_t direction,
+                               uint8_t *msg,
+                               uint32_t msg_len,
+                               uint8_t *mac) {
+    switch (integ_algo) {
+      case INTEGRITY_ALGORITHM_ID_EIA0:
+        break;
+      case INTEGRITY_ALGORITHM_ID_128_EIA1:
+        security_128_eia1(key_128,
+                          count,
+                          rb_id,
+                          direction,
+                          msg,
+                          msg_len,
+                          mac);
+        break;
+      case INTEGRITY_ALGORITHM_ID_128_EIA2:
+        security_128_eia2(key_128,
+                          count,
+                          rb_id,
+                          direction,
+                          msg,
+                          msg_len,
+                          mac);
+        break;
+      default:
+        break;
+    }
   }
-}
-
-void nas::integrity_check()
-{
 
-}
+  void nas::integrity_check() {
 
-void nas::cipher_encrypt()
-{
+  }
 
-}
+  void nas::cipher_encrypt() {
 
-void nas::cipher_decrypt()
-{
+  }
 
-}
+  void nas::cipher_decrypt() {
 
+  }
 
 
 /*******************************************************************************
   Parsers
 *******************************************************************************/
 
-void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
-{
-  LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT                                attach_accept;
-  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT  act_def_eps_bearer_context_req;
-  LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT                              attach_complete;
-  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT   act_def_eps_bearer_context_accept;
-
-  nas_log->info("Received Attach Accept\n");
-  count_dl++;
-
-  liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &attach_accept);
-
-  if(attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY)
-  {
-    //FIXME: Handle t3412.unit
-    //FIXME: Handle tai_list
-    if(attach_accept.guti_present)
-    {
-      memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
-      is_guti_set = true;
-      // TODO: log message to console
-    }
-    if(attach_accept.lai_present)
-    {
-    }
-    if(attach_accept.ms_id_present)
-    {}
-    if(attach_accept.emm_cause_present)
-    {}
-    if(attach_accept.t3402_present)
-    {}
-    if(attach_accept.t3423_present)
-    {}
-    if(attach_accept.equivalent_plmns_present)
-    {}
-    if(attach_accept.emerg_num_list_present)
-    {}
-    if(attach_accept.eps_network_feature_support_present)
-    {}
-    if(attach_accept.additional_update_result_present)
-    {}
-    if(attach_accept.t3412_ext_present)
-    {}
-
-    liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg, &act_def_eps_bearer_context_req);
-
-    if(LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type)
-    {
-      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[0] << 24;
-      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[1] << 16;
-      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
-      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[3];
-
-      nas_log->info("IP allocated by network %u.%u.%u.%u\n",
-                    act_def_eps_bearer_context_req.pdn_addr.addr[0],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[1],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[2],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[3]);
-
-      nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
-                    act_def_eps_bearer_context_req.pdn_addr.addr[0],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[1],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[2],
-                    act_def_eps_bearer_context_req.pdn_addr.addr[3]);
-      
-      // Setup GW
-      char *err_str = NULL;
-      if(gw->setup_if_addr(ip_addr, err_str))
-      {
-        nas_log->error("Failed to set gateway address - %s\n", err_str);
+  void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
+    LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
+    LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT act_def_eps_bearer_context_req;
+    LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT attach_complete;
+    LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT act_def_eps_bearer_context_accept;
+
+    nas_log->info("Received Attach Accept\n");
+    count_dl++;
+
+    liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_accept);
+
+    if (attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY) {
+      //FIXME: Handle t3412.unit
+      //FIXME: Handle tai_list
+      if (attach_accept.guti_present) {
+        memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
+        is_guti_set = true;
+        // TODO: log message to console
       }
-    }
-    else
-    {
-      nas_log->error("Not handling IPV6 or IPV4V6\n");
-      pool->deallocate(pdu);
-      return;
-    }
-    eps_bearer_id = act_def_eps_bearer_context_req.eps_bearer_id;
-    if(act_def_eps_bearer_context_req.transaction_id_present)
-    {
+      if (attach_accept.lai_present) {
+      }
+      if (attach_accept.ms_id_present) {}
+      if (attach_accept.emm_cause_present) {}
+      if (attach_accept.t3402_present) {}
+      if (attach_accept.t3423_present) {}
+      if (attach_accept.equivalent_plmns_present) {}
+      if (attach_accept.emerg_num_list_present) {}
+      if (attach_accept.eps_network_feature_support_present) {}
+      if (attach_accept.additional_update_result_present) {}
+      if (attach_accept.t3412_ext_present) {}
+
+      liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg,
+                                                                        &act_def_eps_bearer_context_req);
+
+      if (LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type) {
+        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[0] << 24;
+        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[1] << 16;
+        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
+        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[3];
+
+        nas_log->info("IP allocated by network %u.%u.%u.%u\n",
+                      act_def_eps_bearer_context_req.pdn_addr.addr[0],
+                      act_def_eps_bearer_context_req.pdn_addr.addr[1],
+                      act_def_eps_bearer_context_req.pdn_addr.addr[2],
+                      act_def_eps_bearer_context_req.pdn_addr.addr[3]);
+
+        nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
+                         act_def_eps_bearer_context_req.pdn_addr.addr[0],
+                         act_def_eps_bearer_context_req.pdn_addr.addr[1],
+                         act_def_eps_bearer_context_req.pdn_addr.addr[2],
+                         act_def_eps_bearer_context_req.pdn_addr.addr[3]);
+
+        // Setup GW
+        char *err_str = NULL;
+        if (gw->setup_if_addr(ip_addr, err_str)) {
+          nas_log->error("Failed to set gateway address - %s\n", err_str);
+        }
+      } else {
+        nas_log->error("Not handling IPV6 or IPV4V6\n");
+        pool->deallocate(pdu);
+        return;
+      }
+      eps_bearer_id = act_def_eps_bearer_context_req.eps_bearer_id;
+      if (act_def_eps_bearer_context_req.transaction_id_present) {
         transaction_id = act_def_eps_bearer_context_req.proc_transaction_id;
-    }
+      }
 
-    //FIXME: Handle the following parameters
+      //FIXME: Handle the following parameters
 //    act_def_eps_bearer_context_req.eps_qos.qci
 //    act_def_eps_bearer_context_req.eps_qos.br_present
 //    act_def_eps_bearer_context_req.eps_qos.br_ext_present
@@ -328,336 +319,315 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu)
 //    act_def_eps_bearer_context_req.protocol_cnfg_opts_present
 //    act_def_eps_bearer_context_req.connectivity_type_present
 
-    // FIXME: Setup the default EPS bearer context
-
-    state = EMM_STATE_REGISTERED;
+      // FIXME: Setup the default EPS bearer context
+
+      state = EMM_STATE_REGISTERED;
+
+      // Send EPS bearer context accept and attach complete
+      count_ul++;
+      act_def_eps_bearer_context_accept.eps_bearer_id = eps_bearer_id;
+      act_def_eps_bearer_context_accept.proc_transaction_id = transaction_id;
+      act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
+      liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept,
+                                                                     &attach_complete.esm_msg);
+      liblte_mme_pack_attach_complete_msg(&attach_complete,
+                                          LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
+                                          count_ul,
+                                          (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+      integrity_generate(&k_nas_int[16],
+                         count_ul,
+                         lcid - 1,
+                         SECURITY_DIRECTION_UPLINK,
+                         &pdu->msg[5],
+                         pdu->N_bytes - 5,
+                         &pdu->msg[1]);
 
-    // Send EPS bearer context accept and attach complete
-    count_ul++;
-    act_def_eps_bearer_context_accept.eps_bearer_id                 = eps_bearer_id;
-    act_def_eps_bearer_context_accept.proc_transaction_id           = transaction_id;
-    act_def_eps_bearer_context_accept.protocol_cnfg_opts_present    = false;
-    liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept, &attach_complete.esm_msg);
-    liblte_mme_pack_attach_complete_msg(&attach_complete,
-                                        LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                        count_ul,
-                                        (LIBLTE_BYTE_MSG_STRUCT*)pdu);
-    integrity_generate(&k_nas_int[16],
-                       count_ul,
-                       lcid-1,
-                       SECURITY_DIRECTION_UPLINK,
-                       &pdu->msg[5],
-                       pdu->N_bytes-5,
-                       &pdu->msg[1]);
+      // Instruct RRC to enable capabilities
+      rrc->enable_capabilities();
 
-    // Instruct RRC to enable capabilities
-    rrc->enable_capabilities();
+      nas_log->info("Sending Attach Complete\n");
+      rrc->write_sdu(lcid, pdu);
 
-    nas_log->info("Sending Attach Complete\n");
-    rrc->write_sdu(lcid, pdu);
-    
+    } else {
+      nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
+      state = EMM_STATE_DEREGISTERED;
+      pool->deallocate(pdu);
+    }
   }
-  else
-  {
-    nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
+
+  void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu) {
+    LIBLTE_MME_ATTACH_REJECT_MSG_STRUCT attach_rej;
+
+    liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_rej);
+    nas_log->warning("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
+    nas_log->console("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
     state = EMM_STATE_DEREGISTERED;
     pool->deallocate(pdu);
+    // FIXME: Command RRC to release?
   }
-}
-
-void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
-  LIBLTE_MME_ATTACH_REJECT_MSG_STRUCT attach_rej;
-
-  liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &attach_rej);
-  nas_log->warning("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
-  nas_log->console("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
-  state = EMM_STATE_DEREGISTERED;
-  pool->deallocate(pdu);
-  // FIXME: Command RRC to release?
-}
-
-void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu)
-{
-  LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT  auth_req;
-  LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
-
-  nas_log->info("Received Authentication Request\n");;
-  liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &auth_req);
-
-  // Reuse the pdu for the response message
-  pdu->reset();
-
-  // Generate authentication response using RAND, AUTN & KSI-ASME
-  uint16 mcc, mnc;
-  mcc = rrc->get_mcc();
-  mnc = rrc->get_mnc();
-
-  nas_log->info("MCC=%d, MNC=%d\n", mcc, mnc);
-
-  bool    net_valid;
-  uint8_t res[16];
-  usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
-
-  if(net_valid)
-  {
-    nas_log->info("Network authentication successful\n");
-    for(int i=0; i<8; i++)
-    {
-      auth_res.res[i] = res[i];
-    }
-    liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT*)pdu);
 
-    nas_log->info("Sending Authentication Response\n");
-    rrc->write_sdu(lcid, pdu);
+  void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
+    LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT auth_req;
+    LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
+
+    nas_log->info("Received Authentication Request\n");;
+    liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &auth_req);
+
+    // Reuse the pdu for the response message
+    pdu->reset();
+
+    // Generate authentication response using RAND, AUTN & KSI-ASME
+    uint16 mcc, mnc;
+    mcc = rrc->get_mcc();
+    mnc = rrc->get_mnc();
+
+    nas_log->info("MCC=%d, MNC=%d\n", mcc, mnc);
+
+    bool net_valid;
+    uint8_t res[16];
+    usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
+
+    if (net_valid) {
+      nas_log->info("Network authentication successful\n");
+      for (int i = 0; i < 8; i++) {
+        auth_res.res[i] = res[i];
+      }
+      liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+
+      nas_log->info("Sending Authentication Response\n");
+      rrc->write_sdu(lcid, pdu);
+    } else {
+      nas_log->warning("Network authentication failure\n");
+      nas_log->console("Warning: Network authentication failure\n");
+      pool->deallocate(pdu);
+    }
   }
-  else
-  {
-    nas_log->warning("Network authentication failure\n");
-    nas_log->console("Warning: Network authentication failure\n");
+
+  void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
+    nas_log->warning("Received Authentication Reject\n");
     pool->deallocate(pdu);
+    state = EMM_STATE_DEREGISTERED;
+    // FIXME: Command RRC to release?
   }
-}
-
-void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
-  nas_log->warning("Received Authentication Reject\n");
-  pool->deallocate(pdu);
-  state = EMM_STATE_DEREGISTERED;
-  // FIXME: Command RRC to release?
-}
-
-void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu)
-{
-  nas_log->error("TODO:parse_identity_request\n");
-}
-
-void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
-{
-  bool                                          success;
-  LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT   sec_mode_cmd;
-  LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT  sec_mode_comp;
-  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT    sec_mode_rej;
-
-  nas_log->info("Received Security Mode Command\n");
-  liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT*)pdu, &sec_mode_cmd);
-
-  ksi = sec_mode_cmd.nas_ksi.nas_ksi;
-  cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM)sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
-  integ_algo  = (INTEGRITY_ALGORITHM_ID_ENUM)sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
-  // FIXME: Handle nonce_ue, nonce_mme
-  // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
-  // FIXME: Use selected_nas_sec_algs to choose correct algos
-
-  nas_log->debug("Security details: ksi=%d, eea=%s, eia=%s\n",
-                 ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
-
-
-  if(CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
-     (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
-      INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
-     sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE)
-  {
-    sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
-    nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
-    success = false;
+
+  void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu) {
+    nas_log->error("TODO:parse_identity_request\n");
   }
-  else
-  {
-    // Generate NAS encryption key and integrity protection key
-    usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
-    nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
-    nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
-
-    // Check incoming MAC
-    uint8_t *inMAC = &pdu->msg[1];
-    uint8_t genMAC[4];
-    integrity_generate(&k_nas_int[16],
-                       count_dl,
-                       lcid-1,
-                       SECURITY_DIRECTION_DOWNLINK,
-                       &pdu->msg[5],
-                       pdu->N_bytes-5,
-                       genMAC);
-
-    nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
-    nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
-
-    bool match=true;
-    for(int i=0;i<4;i++) {
-      if(inMAC[i] != genMAC[i]) {
-        match = false;
-      }
-    }
-    if(!match) {
-      sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
-      nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
+
+  void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
+    bool success;
+    LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT sec_mode_cmd;
+    LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT sec_mode_comp;
+    LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
+
+    nas_log->info("Received Security Mode Command\n");
+    liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &sec_mode_cmd);
+
+    ksi = sec_mode_cmd.nas_ksi.nas_ksi;
+    cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
+    integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
+    // FIXME: Handle nonce_ue, nonce_mme
+    // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
+    // FIXME: Use selected_nas_sec_algs to choose correct algos
+
+    nas_log->debug("Security details: ksi=%d, eea=%s, eia=%s\n",
+                   ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
+
+
+    if (CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
+        (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
+         INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
+        sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
+      sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
+      nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
       success = false;
     } else {
+      // Generate NAS encryption key and integrity protection key
+      usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
+      nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
+      nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
+
+      // Check incoming MAC
+      uint8_t *inMAC = &pdu->msg[1];
+      uint8_t genMAC[4];
+      integrity_generate(&k_nas_int[16],
+                         count_dl,
+                         lcid - 1,
+                         SECURITY_DIRECTION_DOWNLINK,
+                         &pdu->msg[5],
+                         pdu->N_bytes - 5,
+                         genMAC);
+
+      nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
+      nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
+
+      bool match = true;
+      for (int i = 0; i < 4; i++) {
+        if (inMAC[i] != genMAC[i]) {
+          match = false;
+        }
+      }
+      if (!match) {
+        sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
+        nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
+        success = false;
+      } else {
 
-      if(sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req)
-      {
+        if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
           sec_mode_comp.imeisv_present = true;
           sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
           usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
           sec_mode_comp.imeisv.imeisv[14] = 5;
           sec_mode_comp.imeisv.imeisv[15] = 3;
-      }
-      else
-      {
+        } else {
           sec_mode_comp.imeisv_present = false;
+        }
+
+        // Reuse pdu for response
+        pdu->reset();
+        liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
+                                                   LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
+                                                   count_ul,
+                                                   (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+        integrity_generate(&k_nas_int[16],
+                           count_ul,
+                           lcid - 1,
+                           SECURITY_DIRECTION_UPLINK,
+                           &pdu->msg[5],
+                           pdu->N_bytes - 5,
+                           &pdu->msg[1]);
+        nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
+                      count_ul,
+                      rb_id_text[lcid]);
+        success = true;
       }
+    }
 
+    if (!success) {
       // Reuse pdu for response
       pdu->reset();
-      liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
-                                                 LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                                 count_ul,
-                                                 (LIBLTE_BYTE_MSG_STRUCT*)pdu);
-      integrity_generate(&k_nas_int[16],
-                         count_ul,
-                         lcid-1,
-                         SECURITY_DIRECTION_UPLINK,
-                         &pdu->msg[5],
-                         pdu->N_bytes-5,
-                         &pdu->msg[1]);
-      nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
-                   count_ul,
-                   rb_id_text[lcid]);
-      success = true;
+      liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
     }
+
+    rrc->write_sdu(lcid, pdu);
   }
 
-  if(!success) {
-    // Reuse pdu for response
-    pdu->reset();
-    liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT*)pdu);
+  void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
+    nas_log->error("TODO:parse_service_reject\n");
+  }
+
+  void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
+    nas_log->error("TODO:parse_esm_information_request\n");
   }
 
-  rrc->write_sdu(lcid, pdu);
-}
-
-void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu)
-{
-  nas_log->error("TODO:parse_service_reject\n");
-}
-void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu)
-{
-  nas_log->error("TODO:parse_esm_information_request\n");
-}
-void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu)
-{
-  nas_log->error("TODO:parse_emm_information\n");
-}
+  void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
+    nas_log->error("TODO:parse_emm_information\n");
+  }
 
 /*******************************************************************************
   Senders
 *******************************************************************************/
 
-void nas::send_attach_request()
-{
-  LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT  attach_req;
-  byte_buffer_t                  *msg = pool_allocate;
-  u_int32_t                             i;
+  void nas::send_attach_request() {
+    LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
+    byte_buffer_t *msg = pool_allocate;
+    u_int32_t i;
 
-  attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
+    attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
 
-  for(i=0; i<8; i++)
-  {
+    for (i = 0; i < 8; i++) {
       attach_req.ue_network_cap.eea[i] = false;
       attach_req.ue_network_cap.eia[i] = false;
+    }
+    attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
+    attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
+    attach_req.ue_network_cap.eia[1] = true; // EIA1 supported
+    attach_req.ue_network_cap.eia[2] = true; // EIA2 supported
+
+    attach_req.ue_network_cap.uea_present = false; // UMTS encryption algos
+    attach_req.ue_network_cap.uia_present = false; // UMTS integrity algos
+
+    attach_req.ms_network_cap_present = false; // A/Gb mode (2G) or Iu mode (3G)
+
+    attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
+    usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
+
+    // ESM message (PDN connectivity request) for first default bearer
+    gen_pdn_connectivity_request(&attach_req.esm_msg);
+
+    attach_req.old_p_tmsi_signature_present = false;
+    attach_req.additional_guti_present = false;
+    attach_req.last_visited_registered_tai_present = false;
+    attach_req.drx_param_present = false;
+    attach_req.ms_network_cap_present = false;
+    attach_req.old_lai_present = false;
+    attach_req.tmsi_status_present = false;
+    attach_req.ms_cm2_present = false;
+    attach_req.ms_cm3_present = false;
+    attach_req.supported_codecs_present = false;
+    attach_req.additional_update_type_present = false;
+    attach_req.voice_domain_pref_and_ue_usage_setting_present = false;
+    attach_req.device_properties_present = false;
+    attach_req.old_guti_type_present = false;
+
+    // Pack the message
+    liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
+
+    nas_log->info("Sending attach request\n");
+    rrc->write_sdu(RB_ID_SRB1, msg);
   }
-  attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
-  attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
-  attach_req.ue_network_cap.eia[1] = true; // EIA1 supported
-  attach_req.ue_network_cap.eia[2] = true; // EIA2 supported
-
-  attach_req.ue_network_cap.uea_present = false; // UMTS encryption algos
-  attach_req.ue_network_cap.uia_present = false; // UMTS integrity algos
-
-  attach_req.ms_network_cap_present = false; // A/Gb mode (2G) or Iu mode (3G)
-
-  attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
-  usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
-
-  // ESM message (PDN connectivity request) for first default bearer
-  gen_pdn_connectivity_request(&attach_req.esm_msg);
-
-  attach_req.old_p_tmsi_signature_present = false;
-  attach_req.additional_guti_present = false;
-  attach_req.last_visited_registered_tai_present = false;
-  attach_req.drx_param_present = false;
-  attach_req.ms_network_cap_present = false;
-  attach_req.old_lai_present = false;
-  attach_req.tmsi_status_present = false;
-  attach_req.ms_cm2_present = false;
-  attach_req.ms_cm3_present = false;
-  attach_req.supported_codecs_present = false;
-  attach_req.additional_update_type_present = false;
-  attach_req.voice_domain_pref_and_ue_usage_setting_present = false;
-  attach_req.device_properties_present = false;
-  attach_req.old_guti_type_present = false;
-
-  // Pack the message
-  liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT*)msg);
-
-  nas_log->info("Sending attach request\n");
-  rrc->write_sdu(RB_ID_SRB1, msg);
-}
-
-void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg)
-{
-    LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT  pdn_con_req;
+
+  void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
+    LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT pdn_con_req;
 
     nas_log->info("Generating PDN Connectivity Request\n");
 
     // Set the PDN con req parameters
-    pdn_con_req.eps_bearer_id       = 0x00; // Unassigned bearer ID
+    pdn_con_req.eps_bearer_id = 0x00; // Unassigned bearer ID
     pdn_con_req.proc_transaction_id = 0x01; // First transaction ID
-    pdn_con_req.pdn_type            = LIBLTE_MME_PDN_TYPE_IPV4;
-    pdn_con_req.request_type        = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
+    pdn_con_req.pdn_type = LIBLTE_MME_PDN_TYPE_IPV4;
+    pdn_con_req.request_type = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
 
     // Set the optional flags
-    pdn_con_req.esm_info_transfer_flag_present  = false; //FIXME: Check if this is needed
-    pdn_con_req.apn_present                     = false;
-    pdn_con_req.protocol_cnfg_opts_present      = false;
-    pdn_con_req.device_properties_present       = false;
+    pdn_con_req.esm_info_transfer_flag_present = false; //FIXME: Check if this is needed
+    pdn_con_req.apn_present = false;
+    pdn_con_req.protocol_cnfg_opts_present = false;
+    pdn_con_req.device_properties_present = false;
 
     // Pack the message
     liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
-}
+  }
 
-void nas::send_identity_response(){}
+  void nas::send_identity_response() {}
 
-void nas::send_service_request()
-{
-  byte_buffer_t *msg = pool_allocate;
-  count_ul++;
+  void nas::send_service_request() {
+    byte_buffer_t *msg = pool_allocate;
+    count_ul++;
 
-  // Pack the service request message directly
-  msg->msg[0]  = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
-  msg->N_bytes++;
-  msg->msg[1]  = (ksi & 0x07) << 5;
-  msg->msg[1] |= count_ul & 0x1F;
-  msg->N_bytes++;
+    // Pack the service request message directly
+    msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
+    msg->N_bytes++;
+    msg->msg[1] = (ksi & 0x07) << 5;
+    msg->msg[1] |= count_ul & 0x1F;
+    msg->N_bytes++;
 
-  uint8_t mac[4];
-  integrity_generate(&k_nas_int[16],
-                      count_ul,
-                      RB_ID_SRB1-1,
-                      SECURITY_DIRECTION_UPLINK,
-                      &msg->msg[0],
-                      2,
-                      &mac[0]);
-  // Set the short MAC
-  msg->msg[2] = mac[2];
-  msg->N_bytes++;
-  msg->msg[3] = mac[3];
-  msg->N_bytes++;
-  nas_log->info("Sending service request\n");
-  rrc->write_sdu(RB_ID_SRB1, msg);
-}
-
-void nas::send_esm_information_response(){}
+    uint8_t mac[4];
+    integrity_generate(&k_nas_int[16],
+                       count_ul,
+                       RB_ID_SRB1 - 1,
+                       SECURITY_DIRECTION_UPLINK,
+                       &msg->msg[0],
+                       2,
+                       &mac[0]);
+    // Set the short MAC
+    msg->msg[2] = mac[2];
+    msg->N_bytes++;
+    msg->msg[3] = mac[3];
+    msg->N_bytes++;
+    nas_log->info("Sending service request\n");
+    rrc->write_sdu(RB_ID_SRB1, msg);
+  }
+
+  void nas::send_esm_information_response() {}
 
 } // namespace srsue
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 20a4db1a2..3e8bfcab8 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -27,6 +27,9 @@
 
 #include <unistd.h>
 #include <sstream>
+#include <srslte/asn1/liblte_rrc.h>
+#include <upper/rrc.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "upper/rrc.h"
 #include "srslte/common/security.h"
@@ -36,90 +39,83 @@
 
 using namespace srslte;
 
-namespace srsue{
+namespace srsue {
+
 
-  
 /*******************************************************************************
   Base functions 
 *******************************************************************************/
-    
-rrc::rrc()
-  :state(RRC_STATE_IDLE)
-  ,drb_up(false)
-{}
-
-static void liblte_rrc_handler(void *ctx, char *str) {
-  rrc *r = (rrc*) ctx; 
-  r->liblte_rrc_log(str);
-}
-
-void rrc::liblte_rrc_log(char* str)
-{
-  if (rrc_log) {
-    rrc_log->warning("[ASN]: %s\n", str);
-  } else {
-    printf("[ASN]: %s\n", str);
+
+  rrc::rrc()
+    : state(RRC_STATE_IDLE), drb_up(false) {}
+
+  static void liblte_rrc_handler(void *ctx, char *str) {
+    rrc *r = (rrc *) ctx;
+    r->liblte_rrc_log(str);
+  }
+
+  void rrc::liblte_rrc_log(char *str) {
+    if (rrc_log) {
+      rrc_log->warning("[ASN]: %s\n", str);
+    } else {
+      printf("[ASN]: %s\n", str);
+    }
   }
-}
-
-void rrc::init(phy_interface_rrc     *phy_,
-               mac_interface_rrc     *mac_,
-               rlc_interface_rrc     *rlc_,
-               pdcp_interface_rrc    *pdcp_,
-               nas_interface_rrc     *nas_,
-               usim_interface_rrc    *usim_,
-               mac_interface_timers  *mac_timers_,
-               srslte::log           *rrc_log_)
-{
-  pool    = byte_buffer_pool::get_instance();
-  phy     = phy_;
-  mac     = mac_;
-  rlc     = rlc_;
-  pdcp    = pdcp_;
-  nas     = nas_;
-  usim    = usim_;
-  rrc_log = rrc_log_;
-  mac_timers = mac_timers_;
-  state   = RRC_STATE_IDLE; 
-  si_acquire_state = SI_ACQUIRE_IDLE; 
-  
-  thread_running = true; 
-  start();
-
-  pthread_mutex_init(&mutex, NULL); 
-  
-  ue_category = SRSLTE_UE_CATEGORY; 
-  
-  transaction_id = 0;
-  
-  // Register logging handler with liblte_rrc
-  liblte_rrc_log_register_handler(this, liblte_rrc_handler);
-  
-  // Set default values for all layers 
-  set_rrc_default();
-  set_phy_default();
-  set_mac_default();
-}
-
-void rrc::stop()
-{
-  thread_running = false; 
-  wait_thread_finish();
-}
-
-rrc_state_t rrc::get_state()
-{
-  return state;
-}
-
-void rrc::set_ue_category(int category)
-{
-  if(category >= 1 && category <= 5) {
-    ue_category = category; 
-  } else {
-    rrc_log->error("Unsupported UE category %d\n", category);
+
+  void rrc::init(phy_interface_rrc *phy_,
+                 mac_interface_rrc *mac_,
+                 rlc_interface_rrc *rlc_,
+                 pdcp_interface_rrc *pdcp_,
+                 nas_interface_rrc *nas_,
+                 usim_interface_rrc *usim_,
+                 mac_interface_timers *mac_timers_,
+                 srslte::log *rrc_log_) {
+    pool = byte_buffer_pool::get_instance();
+    phy = phy_;
+    mac = mac_;
+    rlc = rlc_;
+    pdcp = pdcp_;
+    nas = nas_;
+    usim = usim_;
+    rrc_log = rrc_log_;
+    mac_timers = mac_timers_;
+    state = RRC_STATE_IDLE;
+    si_acquire_state = SI_ACQUIRE_IDLE;
+
+    thread_running = true;
+    start();
+
+    pthread_mutex_init(&mutex, NULL);
+
+    ue_category = SRSLTE_UE_CATEGORY;
+
+    transaction_id = 0;
+
+    // Register logging handler with liblte_rrc
+    liblte_rrc_log_register_handler(this, liblte_rrc_handler);
+
+    // Set default values for all layers
+    set_rrc_default();
+    set_phy_default();
+    set_mac_default();
+  }
+
+  void rrc::stop() {
+    thread_running = false;
+    wait_thread_finish();
+  }
+
+  rrc_state_t rrc::get_state() {
+    return state;
+  }
+
+  void rrc::set_ue_category(int category) {
+    if (category >= 1 && category <= 5) {
+      ue_category = category;
+    } else {
+      rrc_log->error("Unsupported UE category %d\n", category);
+    }
   }
-}
 
 
 
@@ -138,286 +134,300 @@ void rrc::set_ue_category(int category)
   NAS interface
 *******************************************************************************/
 
-uint16_t rrc::get_mcc()
-{
-  if (current_cell) {
-    if(current_cell->sib1.N_plmn_ids > 0) {
-      return current_cell->sib1.plmn_id[0].id.mcc;
+  uint16_t rrc::get_mcc() {
+    if (current_cell) {
+      if (current_cell->sib1.N_plmn_ids > 0) {
+        return current_cell->sib1.plmn_id[0].id.mcc;
+      }
     }
+    return 0;
   }
-  return 0;
-}
-
-uint16_t rrc::get_mnc()
-{
-  if (current_cell) {
-    if(current_cell->sib1.N_plmn_ids > 0) {
-      return current_cell->sib1.plmn_id[0].id.mnc;      
+
+  uint16_t rrc::get_mnc() {
+    if (current_cell) {
+      if (current_cell->sib1.N_plmn_ids > 0) {
+        return current_cell->sib1.plmn_id[0].id.mnc;
+      }
     }
+    return 0;
+  }
+
+  void rrc::plmn_search() {
+    rrc_log->info("Starting PLMN search procedure\n");
+    state = RRC_STATE_PLMN_SELECTION;
+    phy->cell_search_start();
   }
-  return 0;
-}
-
-void rrc::plmn_search()
-{
-  state = RRC_STATE_PLMN_SELECTION;
-  phy->cell_search_start();
-}
-
-void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id)
-{
-  bool sync_ok = false; 
-  
-  state = RRC_STATE_CELL_SELECTING; 
-  
-  // Sort cells according to RSRP
-
-  selected_plmn_id = plmn_id; 
-  last_selected_cell = -1; 
-
-  select_next_cell_in_plmn();  
-}
-
-void rrc::connect()
-{
-  pthread_mutex_lock(&mutex);
-  if(RRC_STATE_CELL_SELECTED == state) {
-    if (si_acquire_state == SI_ACQUIRE_IDLE) {
+
+  void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+    rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
+
+    state = RRC_STATE_CELL_SELECTING;
+
+    // Sort cells according to RSRP
+
+    selected_plmn_id = plmn_id;
+    last_selected_cell = -1;
+
+    select_next_cell_in_plmn();
+  }
+
+  void rrc::connect() {
+    pthread_mutex_lock(&mutex);
+    if (RRC_STATE_CELL_SELECTED == state) {
       rrc_log->info("RRC in IDLE state - sending connection request.\n");
       state = RRC_STATE_CONNECTING;
       send_con_request();
     } else {
-      rrc_log->warning("Received connect() but SI not acquired\n");
+      rrc_log->warning("Received connect() but cell is not selected\n");
     }
-  } else {
-    rrc_log->warning("Received connect() but cell is not selected\n");
+    pthread_mutex_unlock(&mutex);
   }
-  pthread_mutex_unlock(&mutex);  
-}
-
-void rrc::select_next_cell_in_plmn() {
-  for (uint32_t i=last_selected_cell+1;i<known_cells.size();i++) {
-    for (uint32_t j=0;j<known_cells[i].sib1.N_plmn_ids;j++) {
-      if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc || 
-          known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) 
-      {
-        // Check that cell satisfies S criteria 
-        if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
-          si_acquire_state = SI_ACQUIRE_SIB1; 
-          last_selected_cell = i; 
-          return;           
+
+  void rrc::select_next_cell_in_plmn() {
+    for (uint32_t i = last_selected_cell + 1; i < known_cells.size(); i++) {
+      for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
+        if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc ||
+            known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) {
+          rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                        known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                        known_cells[i].sib1.cell_id);
+          rrc_log->console("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                        known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                        known_cells[i].sib1.cell_id);
+          // Check that cell satisfies S criteria
+          if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
+            // Give time to the PHY to sync on the new cell
+            int cnt=0;
+            while(!phy->sync_status() && cnt<100) {
+              usleep(1000);
+              cnt++;
+            }
+            if (phy->sync_status()) {
+              if (!known_cells[i].has_valid_sib1) {
+                si_acquire_state = SI_ACQUIRE_SIB1;
+              } else if (!known_cells[i].has_valid_sib2) {
+                si_acquire_state = SI_ACQUIRE_SIB2;
+              } else {
+                si_acquire_state = SI_ACQUIRE_IDLE;
+              }
+              last_selected_cell = i;
+            } else {
+              rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x: Could not synchronize\n",
+                             known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+            }
+            return;
+          } else {
+            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+                           known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+          }
         }
-      }      
+      }
     }
   }
-}
 
 /*******************************************************************************
   PHY interface
 *******************************************************************************/
 
-void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
-    
-  // find if cell_id-earfcn combination already exists 
-  for (uint32_t i=0;i<known_cells.size();i++) {
-    if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
-      known_cells[i].rsrp = rsrp; 
-      current_cell = &known_cells[i];
-      rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%d dBm\n", known_cells[i].earfcn, known_cells[i].phy_cell.id, known_cells[i].rsrp);            
-      return; 
+  void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
+
+    // find if cell_id-earfcn combination already exists
+    for (uint32_t i = 0; i < known_cells.size(); i++) {
+      if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
+        known_cells[i].rsrp = rsrp;
+        current_cell = &known_cells[i];
+        rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
+                      known_cells[i].phy_cell.id, known_cells[i].rsrp);
+        return;
+      }
     }
+    // add to list of known cells
+    cell_t cell;
+    cell.phy_cell = phy_cell;
+    cell.rsrp = rsrp;
+    cell.earfcn = earfcn;
+    cell.has_valid_sib1 = false;
+    cell.has_valid_sib2 = false;
+    known_cells.push_back(cell);
+
+    // save current cell
+    current_cell = &known_cells.back();
+
+    si_acquire_state = SI_ACQUIRE_SIB1;
+
+    rrc_log->info("Found Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+                  cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
+                  cell.earfcn, cell.rsrp);
+
+    rrc_log->console("Found Cell:  PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+                  cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
+                  cell.earfcn, cell.rsrp);
+
   }
-  // add to list of known cells 
-  cell_t cell; 
-  cell.phy_cell = phy_cell; 
-  cell.rsrp     = rsrp; 
-  cell.earfcn   = earfcn;
-  cell.has_valid_sib1 = false; 
-  cell.has_valid_sib2 = false; 
-  known_cells.push_back(cell);  
-  
-  // save current cell 
-  current_cell = &known_cells.back(); 
-
-  rrc_log->info("Found new cell EARFCN=%d, PCI=%d, RSRP=%d dBm\n", cell.earfcn, cell.phy_cell.id, cell.rsrp);
-  
-}
 
 // Detection of physical layer problems (5.3.11.1)
-void rrc::out_of_sync()
-{
-  if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
-    n310_cnt++;
-    if (n310_cnt == N310) {
-      mac_timers->get(t310)->reset();
-      mac_timers->get(t310)->run();
-      n310_cnt = 0; 
-      rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
+  void rrc::out_of_sync() {
+    if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
+      n310_cnt++;
+      if (n310_cnt == N310) {
+        mac_timers->get(t310)->reset();
+        mac_timers->get(t310)->run();
+        n310_cnt = 0;
+        rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
+      }
     }
   }
-}
 
 // Recovery of physical layer problems (5.3.11.2)
-void rrc::in_sync()
-{
-  if (mac_timers->get(t310)->is_running()) {
-    n311_cnt++;
-    if (n311_cnt == N311) {
-      mac_timers->get(t310)->stop();      
-      n311_cnt = 0; 
-      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
+  void rrc::in_sync() {
+    if (mac_timers->get(t310)->is_running()) {
+      n311_cnt++;
+      if (n311_cnt == N311) {
+        mac_timers->get(t310)->stop();
+        n311_cnt = 0;
+        rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
+      }
     }
   }
-}
 
 /*******************************************************************************
   PDCP interface
 *******************************************************************************/
-void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu)
-{
-  pool->deallocate(pdu);
-  if (state == RRC_STATE_PLMN_SELECTION) {
-    // Do we need to do something with BCH?
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
-    si_acquire_state = SI_ACQUIRE_SIB1;       
-  } else {
-    rrc_log->warning("Received BCCH BCH in incorrect state\n");
-  }
-}
-
-void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu)
-{
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dlsch_msg);
-
-  if (dlsch_msg.N_sibs > 0) {
-    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) 
-    {
-      mac->bcch_stop_rx();
-
-      // Handle SIB1
-      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-
-      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                    current_cell->sib1.cell_id&0xfff,
-                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
-                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
-
-      // Send PLMN and TAC to NAS
-      std::stringstream ss;
-      for(uint32_t i=0;i<current_cell->sib1.N_plmn_ids;i++){
-        std::string mcc;
-        std::string mnc;
-        mcc_to_string(current_cell->sib1.plmn_id[i].id.mcc, &mcc);
-        mnc_to_string(current_cell->sib1.plmn_id[i].id.mnc, &mnc);
-        ss << " PLMN Id: MCC " << mcc << " MNC " << mnc;
-
-        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
-      }
-            
-      // Set TDD Config 
-      if (current_cell->sib1.tdd) {
-        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
-      }
-      
-      rrc_log->console("SIB1 received, CellID=%d, %s\n",
-                       current_cell->sib1.cell_id&0xfff,
-                       ss.str().c_str());
-
-      current_cell->has_valid_sib1 = true; 
-      si_acquire_state = SI_ACQUIRE_SIB2;
-
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB2 == si_acquire_state) 
-    {
-      mac->bcch_stop_rx();      
-
-      // Handle SIB2
-      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-      rrc_log->console("SIB2 received\n");
-      rrc_log->info("SIB2 received\n");
-      
-      apply_sib2_configs(&current_cell->sib2);
-
-      current_cell->has_valid_sib2 = true; 
-      si_acquire_state = SI_ACQUIRE_IDLE; 
+  void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
+    pool->deallocate(pdu);
+    if (state == RRC_STATE_PLMN_SELECTION) {
+      // Do we need to do something with BCH?
+      rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
+    } else {
+      rrc_log->warning("Received BCCH BCH in incorrect state\n");
     }
   }
-}
 
+  void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
+    rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
+    LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
+    pool->deallocate(pdu);
+    liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
 
-// Right now, this thread only controls System Information acquisition procedure
-void rrc::run_thread()
-{
-  uint32_t  tti ;
-  uint32_t  si_win_start, si_win_len;
-  uint16_t  period;
-  uint32_t  nof_sib1_trials = 0; 
-  const int SIB1_SEARCH_TIMEOUT = 30; 
-
-  while(thread_running)
-  {
-    switch(si_acquire_state)
-    {
-    case SI_ACQUIRE_SIB1:
-      // Instruct MAC to look for SIB1
-      if (!current_cell->has_valid_sib1) {
-        tti          = mac->get_current_tti();
-        si_win_start = sib_start_tti(tti, 2, 5);
-        mac->bcch_start_rx(si_win_start, 1);
-        rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                      si_win_start, 1);
-        nof_sib1_trials++;
-        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-          if (state == RRC_STATE_CELL_SELECTING) {
-            select_next_cell_in_plmn();
+    if (dlsch_msg.N_sibs > 0) {
+      if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
+        mac->bcch_stop_rx();
+
+        // Handle SIB1
+        memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+
+        rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
+                      current_cell->sib1.cell_id & 0xfff,
+                      liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                      liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+
+
+        // Set TDD Config
+        if (current_cell->sib1.tdd) {
+          phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+        }
+
+        current_cell->has_valid_sib1 = true;
+
+        // Jump to next state
+        switch(state) {
+          case RRC_STATE_CELL_SELECTING:
+            si_acquire_state = SI_ACQUIRE_SIB2;
+            break;
+          case RRC_STATE_PLMN_SELECTION:
             si_acquire_state = SI_ACQUIRE_IDLE;
-          } else if (state == RRC_STATE_PLMN_SELECTION) {
+            rrc_log->info("SI Acquisition done. Searching next cell...\n");
+            usleep(5000);
             phy->cell_search_next();
-          } 
-          nof_sib1_trials = 0; 
+            break;
+          default:
+            si_acquire_state = SI_ACQUIRE_IDLE;
         }
-      } else {
-        si_acquire_state = SI_ACQUIRE_SIB2; 
-      }
-      break;
-    case SI_ACQUIRE_SIB2:
-      // Instruct MAC to look for SIB2 only when selecting a cell 
-      if (state == RRC_STATE_CELL_SELECTING && !current_cell->has_valid_sib2) {
-        tti          = mac->get_current_tti();
-        period       = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
-        si_win_start = sib_start_tti(tti, period, 0);
-        si_win_len   = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
-
-        mac->bcch_start_rx(si_win_start, si_win_len);
-        rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                      si_win_start, si_win_len);
-      } else {
-        si_acquire_state = SI_ACQUIRE_DONE; 
+
+        // Send PLMN and TAC to NAS
+        std::stringstream ss;
+        for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+          nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+        }
+
+      } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
+                 SI_ACQUIRE_SIB2 == si_acquire_state) {
+        mac->bcch_stop_rx();
+
+        // Handle SIB2
+        memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+        rrc_log->info("SIB2 received\n");
+
+        apply_sib2_configs(&current_cell->sib2);
+
+        current_cell->has_valid_sib2 = true;
+
+        // Jump to next state
+        switch(state) {
+          case RRC_STATE_CELL_SELECTING:
+            si_acquire_state = SI_ACQUIRE_IDLE;
+            state = RRC_STATE_CELL_SELECTED;
+            nas->cell_selected();
+            break;
+          default:
+            si_acquire_state = SI_ACQUIRE_IDLE;
+        }
+
       }
-      break;
-    case SI_ACQUIRE_DONE: 
-      
-      // After acquiring SI, tell NAS that the cell is selected or go to next cell in case of PLMN selection 
-      
-      if (state == RRC_STATE_CELL_SELECTING) {        
-        nas->cell_selected();
-        state = RRC_STATE_CELL_SELECTED;
-      } else if (state == RRC_STATE_PLMN_SELECTION) {
-        phy->cell_search_next();
+    }
+  }
+
+
+// Right now, this thread only controls System Information acquisition procedure
+  void rrc::run_thread() {
+    uint32_t tti;
+    uint32_t si_win_start, si_win_len;
+    uint16_t period;
+    uint32_t nof_sib1_trials = 0;
+    const int SIB1_SEARCH_TIMEOUT = 30;
+
+    while (thread_running) {
+      switch (si_acquire_state) {
+        case SI_ACQUIRE_SIB1:
+          // Instruct MAC to look for SIB1
+          tti = mac->get_current_tti();
+          si_win_start = sib_start_tti(tti, 2, 5);
+          mac->bcch_start_rx(si_win_start, 1);
+          rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                         si_win_start, 1);
+          nof_sib1_trials++;
+          if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+            if (state == RRC_STATE_CELL_SELECTING) {
+              select_next_cell_in_plmn();
+              si_acquire_state = SI_ACQUIRE_IDLE;
+            } else if (state == RRC_STATE_PLMN_SELECTION) {
+              phy->cell_search_next();
+            }
+            nof_sib1_trials = 0;
+          }
+          usleep(20000);
+          break;
+        case SI_ACQUIRE_SIB2:
+          // Instruct MAC to look for SIB2 only when selecting a cell
+          tti = mac->get_current_tti();
+          period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
+          si_win_start = sib_start_tti(tti, period, 0);
+          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+
+          mac->bcch_start_rx(si_win_start, si_win_len);
+          rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                         si_win_start, si_win_len);
+          usleep(current_cell->sib1.si_window_length*1000);
+          break;
+        default:
+          break;
       }
-      si_acquire_state = SI_ACQUIRE_IDLE; 
-      break;
-    default:
-      break;
-    }      
-    usleep(10000);
+      usleep(10000);
+    }
   }
-}
 
 
 
@@ -456,23 +466,21 @@ void rrc::run_thread()
   NAS interface
 *******************************************************************************/
 
-void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
-{
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
-
-  switch(state)
-  {
-  case RRC_STATE_CONNECTING:
-    send_con_setup_complete(sdu);
-    break;
-  case RRC_STATE_CONNECTED:
-    send_ul_info_transfer(lcid, sdu);
-    break;
-  default:
-    rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
-    break;
+  void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
+    rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
+
+    switch (state) {
+      case RRC_STATE_CONNECTING:
+        send_con_setup_complete(sdu);
+        break;
+      case RRC_STATE_CONNECTED:
+        send_ul_info_transfer(lcid, sdu);
+        break;
+      default:
+        rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
+        break;
+    }
   }
-}
 
 
 
@@ -480,1154 +488,1110 @@ void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
   MAC interface
 *******************************************************************************/
 /* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
-void rrc::release_pucch_srs()
-{
-  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
-  set_phy_default_pucch_srs();
-  
-  // Configure RX signals without pregeneration because default option is release
-  phy->configure_ul_params(true);  
-}
+  void rrc::release_pucch_srs() {
+    // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
+    set_phy_default_pucch_srs();
 
-void rrc::ra_problem() {
-  radio_link_failure();
-}
+    // Configure RX signals without pregeneration because default option is release
+    phy->configure_ul_params(true);
+  }
+
+  void rrc::ra_problem() {
+    radio_link_failure();
+  }
 
 
 /*******************************************************************************
   GW interface
 *******************************************************************************/
 
-bool rrc::is_connected()
-{
-  return (RRC_STATE_CONNECTED == state);
-}
+  bool rrc::is_connected() {
+    return (RRC_STATE_CONNECTED == state);
+  }
 
-bool rrc::have_drb()
-{
-  return drb_up;
-}
+  bool rrc::have_drb() {
+    return drb_up;
+  }
 
 /*******************************************************************************
   PDCP interface
 *******************************************************************************/
 
-void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
-{
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_text[lcid]);
-  rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
-
-  switch(lcid)
-  {
-  case RB_ID_SRB0:
-    parse_dl_ccch(pdu);
-    break;
-  case RB_ID_SRB1:
-  case RB_ID_SRB2:
-    parse_dl_dcch(lcid, pdu);
-    break;
-  default:
-    rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
-    break;
+  void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_text[lcid]);
+    rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
+
+    switch (lcid) {
+      case RB_ID_SRB0:
+        parse_dl_ccch(pdu);
+        break;
+      case RB_ID_SRB1:
+      case RB_ID_SRB2:
+        parse_dl_dcch(lcid, pdu);
+        break;
+      default:
+        rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
+        break;
+    }
+
   }
 
-}
 
+  void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
+    if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
+      rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
+      rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
+      rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
 
-void rrc::write_pdu_pcch(byte_buffer_t *pdu)
-{
-  if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
-    rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
-    rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
-    
-    LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-    bit_buf.N_bits = pdu->N_bytes*8;
-    pool->deallocate(pdu);
-    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &pcch_msg);
-    
-    if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
-      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;     
-    }
+      LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
+      srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+      bit_buf.N_bits = pdu->N_bytes * 8;
+      pool->deallocate(pdu);
+      liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
 
-    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-    if(!nas->get_s_tmsi(&s_tmsi)) {
-      rrc_log->info("No S-TMSI present in NAS\n");
-      return;
-    }
-    
-    LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
-    for (uint32_t i=0;i<pcch_msg.paging_record_list_size;i++) {
-      s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
-      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i+1, pcch_msg.paging_record_list_size,
-                    pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i+1, pcch_msg.paging_record_list_size,
+      if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
+        pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
+      }
+
+      LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+      if (!nas->get_s_tmsi(&s_tmsi)) {
+        rrc_log->info("No S-TMSI present in NAS\n");
+        return;
+      }
+
+      LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
+      for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
+        s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
+        rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
                       pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      if(s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
-        rrc_log->info("S-TMSI match in paging message\n");
-        rrc_log->console("S-TMSI match in paging message\n");
-        mac->pcch_stop_rx();
-        if(RRC_STATE_IDLE == state) {
-          rrc_log->info("RRC in IDLE state - sending connection request.\n");
-          state = RRC_STATE_CONNECTING;
-          send_con_request();
+        rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                         pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+        if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
+          rrc_log->info("S-TMSI match in paging message\n");
+          rrc_log->console("S-TMSI match in paging message\n");
+          mac->pcch_stop_rx();
+          if (RRC_STATE_IDLE == state) {
+            rrc_log->info("RRC in IDLE state - sending connection request.\n");
+            state = RRC_STATE_CONNECTING;
+            send_con_request();
+          }
         }
       }
     }
   }
-}
 
 /*******************************************************************************
   RLC interface
 *******************************************************************************/
 
-void rrc::max_retx_attempted()
-{
-  //TODO: Handle the radio link failure
-  rrc_log->warning("Max RLC reTx attempted\n");
-  //radio_link_failure();
-}
+  void rrc::max_retx_attempted() {
+    //TODO: Handle the radio link failure
+    rrc_log->warning("Max RLC reTx attempted\n");
+    //radio_link_failure();
+  }
 
 /*******************************************************************************
   Senders
 *******************************************************************************/
 
-void rrc::send_con_request()
-{
-  rrc_log->debug("Preparing RRC Connection Request\n");
-  LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-  LIBLTE_RRC_S_TMSI_STRUCT      s_tmsi;
-
-  // Prepare ConnectionRequest packet
-  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
-  if(nas->get_s_tmsi(&s_tmsi)) {
-    ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
-    ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
-  } else {
-    ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_RANDOM_VALUE;
-    ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
-  }
-  ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
-  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-  }
-  byte_buffer_t *pdcp_buf = pool_allocate;;
-  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
-  pdcp_buf->set_timestamp();
-
-  // Set UE contention resolution ID in MAC
-  uint64_t uecri=0;
-  uint8_t *ue_cri_ptr = (uint8_t*) &uecri;
-  uint32_t nbytes = 6;
-  for (uint32_t i=0;i<nbytes;i++) {
-    ue_cri_ptr[nbytes-i-1] = pdcp_buf->msg[i];
-  }
-  rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
-  
-  mac->set_contention_id(uecri);
+  void rrc::send_con_request() {
+    rrc_log->debug("Preparing RRC Connection Request\n");
+    LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
+    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+
+    // Prepare ConnectionRequest packet
+    ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
+    if (nas->get_s_tmsi(&s_tmsi)) {
+      ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
+      ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
+    } else {
+      ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_RANDOM_VALUE;
+      ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
+    }
+    ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
+    liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    byte_buffer_t *pdcp_buf = pool_allocate;;
+    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+    pdcp_buf->set_timestamp();
+
+    // Set UE contention resolution ID in MAC
+    uint64_t uecri = 0;
+    uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
+    uint32_t nbytes = 6;
+    for (uint32_t i = 0; i < nbytes; i++) {
+      ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
+    }
+    rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
 
-  rrc_log->info("Sending RRC Connection Request on SRB0\n");
-  pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
-}
+    mac->set_contention_id(uecri);
+
+    rrc_log->info("Sending RRC Connection Request on SRB0\n");
+    pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
+  }
 
 
 /* RRC connection re-establishment procedure (5.3.7) */
-void rrc::send_con_restablish_request()
-{
-  
-  srslte_cell_t cell; 
-  phy->get_current_cell(&cell);
-
-  LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-  LIBLTE_RRC_S_TMSI_STRUCT      s_tmsi;
-
-  // Compute shortMAC-I
-  uint8_t varShortMAC[128], varShortMAC_packed[16];
-  bzero(varShortMAC, 128);
-  bzero(varShortMAC_packed, 16);
-  uint8_t *msg_ptr = varShortMAC; 
-  liblte_rrc_pack_cell_identity_ie(0x1a2d0, &msg_ptr);
-  liblte_rrc_pack_phys_cell_id_ie(cell.id, &msg_ptr);
-  mac_interface_rrc::ue_rnti_t ue_rnti; 
-  mac->get_rntis(&ue_rnti);
-  liblte_rrc_pack_c_rnti_ie(ue_rnti.crnti, &msg_ptr);
-  srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, msg_ptr - varShortMAC);
-  
-  uint8_t mac_key[4];
-  security_128_eia2(&k_rrc_int[16],
-                     1,
-                     1,
-                     1,
-                     varShortMAC_packed,
-                     7,
-                     mac_key);
-  
-  mac_interface_rrc::ue_rnti_t uernti; 
-  mac->get_rntis(&uernti);
-  
-  // Prepare ConnectionRestalishmentRequest packet
-  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;  
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = uernti.crnti;
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = cell.id;
-  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2]<<8 | mac_key[3];
-  ul_ccch_msg.msg.rrc_con_reest_req.cause = LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE;
-  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
-  rrc_log->console("RRC Connection Reestablishment\n");
-  mac_timers->get(t310)->stop();
-  mac_timers->get(t311)->reset();
-  mac_timers->get(t311)->run();
-  
-  set_phy_default();
-  mac->reset();
-  
-  // FIXME: Cell selection should be different??
-  phy->resync_sfn();
-  
-  // Wait for cell re-synchronization  
-  uint32_t timeout_cnt = 0; 
-  while(!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH){
-    usleep(10000);
-    timeout_cnt++; 
-  }
-  mac_timers->get(t301)->reset();
-  mac_timers->get(t301)->run();
-  mac_timers->get(t311)->stop();
-  rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
-  
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-  }
-  byte_buffer_t *pdcp_buf = pool_allocate;;
-  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
-
-  // Set UE contention resolution ID in MAC
-  uint64_t uecri=0;
-  uint8_t *ue_cri_ptr = (uint8_t*) &uecri;
-  uint32_t nbytes = 6;
-  for (uint32_t i=0;i<nbytes;i++) {
-    ue_cri_ptr[nbytes-i-1] = pdcp_buf->msg[i];
+  void rrc::send_con_restablish_request() {
+
+    srslte_cell_t cell;
+    phy->get_current_cell(&cell);
+
+    LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
+    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+
+    // Compute shortMAC-I
+    uint8_t varShortMAC[128], varShortMAC_packed[16];
+    bzero(varShortMAC, 128);
+    bzero(varShortMAC_packed, 16);
+    uint8_t *msg_ptr = varShortMAC;
+    liblte_rrc_pack_cell_identity_ie(0x1a2d0, &msg_ptr);
+    liblte_rrc_pack_phys_cell_id_ie(cell.id, &msg_ptr);
+    mac_interface_rrc::ue_rnti_t ue_rnti;
+    mac->get_rntis(&ue_rnti);
+    liblte_rrc_pack_c_rnti_ie(ue_rnti.crnti, &msg_ptr);
+    srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, msg_ptr - varShortMAC);
+
+    uint8_t mac_key[4];
+    security_128_eia2(&k_rrc_int[16],
+                      1,
+                      1,
+                      1,
+                      varShortMAC_packed,
+                      7,
+                      mac_key);
+
+    mac_interface_rrc::ue_rnti_t uernti;
+    mac->get_rntis(&uernti);
+
+    // Prepare ConnectionRestalishmentRequest packet
+    ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;
+    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = uernti.crnti;
+    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = cell.id;
+    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2] << 8 | mac_key[3];
+    ul_ccch_msg.msg.rrc_con_reest_req.cause = LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE;
+    liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
+    rrc_log->console("RRC Connection Reestablishment\n");
+    mac_timers->get(t310)->stop();
+    mac_timers->get(t311)->reset();
+    mac_timers->get(t311)->run();
+
+    set_phy_default();
+    mac->reset();
+
+    // FIXME: Cell selection should be different??
+    phy->resync_sfn();
+
+    // Wait for cell re-synchronization
+    uint32_t timeout_cnt = 0;
+    while (!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH) {
+      usleep(10000);
+      timeout_cnt++;
+    }
+    mac_timers->get(t301)->reset();
+    mac_timers->get(t301)->run();
+    mac_timers->get(t311)->stop();
+    rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    byte_buffer_t *pdcp_buf = pool_allocate;;
+    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+
+    // Set UE contention resolution ID in MAC
+    uint64_t uecri = 0;
+    uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
+    uint32_t nbytes = 6;
+    for (uint32_t i = 0; i < nbytes; i++) {
+      ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
+    }
+    rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
+    mac->set_contention_id(uecri);
+
+    rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
+    pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
   }
-  rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
-  mac->set_contention_id(uecri);
-
-  rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
-  pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
-}
-
-
-void rrc::send_con_restablish_complete()
-{
-  rrc_log->debug("Preparing RRC Connection Reestablishment Complete\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-  // Prepare ConnectionSetupComplete packet
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_REEST_COMPLETE;
-  ul_dcch_msg.msg.rrc_con_reest_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+
+  void rrc::send_con_restablish_complete() {
+    rrc_log->debug("Preparing RRC Connection Reestablishment Complete\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+    // Prepare ConnectionSetupComplete packet
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_REEST_COMPLETE;
+    ul_dcch_msg.msg.rrc_con_reest_complete.rrc_transaction_id = transaction_id;
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    byte_buffer_t *pdcp_buf = pool_allocate;;
+    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+
+    state = RRC_STATE_CONNECTED;
+    rrc_log->console("RRC Connected\n");
+    rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
+    pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;;
-  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
-
-  state = RRC_STATE_CONNECTED;
-  rrc_log->console("RRC Connected\n");
-  rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
-  pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
-}
-
-void rrc::send_con_setup_complete(byte_buffer_t *nas_msg)
-{
-  rrc_log->debug("Preparing RRC Connection Setup Complete\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-  // Prepare ConnectionSetupComplete packet
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_SETUP_COMPLETE;
-  ul_dcch_msg.msg.rrc_con_setup_complete.registered_mme_present = false;
-  ul_dcch_msg.msg.rrc_con_setup_complete.rrc_transaction_id = transaction_id;
-  ul_dcch_msg.msg.rrc_con_setup_complete.selected_plmn_id = 1;
-  memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
-  ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+  void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
+    rrc_log->debug("Preparing RRC Connection Setup Complete\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+    // Prepare ConnectionSetupComplete packet
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_SETUP_COMPLETE;
+    ul_dcch_msg.msg.rrc_con_setup_complete.registered_mme_present = false;
+    ul_dcch_msg.msg.rrc_con_setup_complete.rrc_transaction_id = transaction_id;
+    ul_dcch_msg.msg.rrc_con_setup_complete.selected_plmn_id = 1;
+    memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
+    ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    byte_buffer_t *pdcp_buf = pool_allocate;;
+    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+    pdcp_buf->set_timestamp();
+
+    state = RRC_STATE_CONNECTED;
+    rrc_log->console("RRC Connected\n");
+    rrc_log->info("Sending RRC Connection Setup Complete\n");
+    pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
   }
-  byte_buffer_t *pdcp_buf = pool_allocate;;
-  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-  pdcp_buf->N_bytes = bit_buf.N_bits/8;
-  pdcp_buf->set_timestamp();
-
-  state = RRC_STATE_CONNECTED;
-  rrc_log->console("RRC Connected\n");
-  rrc_log->info("Sending RRC Connection Setup Complete\n");
-  pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
-}
-
-void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu)
-{
-  rrc_log->debug("Preparing RX Info Transfer\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-  // Prepare RX INFO packet
-  ul_dcch_msg.msg_type                                 = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
-  ul_dcch_msg.msg.ul_info_transfer.dedicated_info_type = LIBLTE_RRC_UL_INFORMATION_TRANSFER_TYPE_NAS;
-  memcpy(ul_dcch_msg.msg.ul_info_transfer.dedicated_info.msg, sdu->msg, sdu->N_bytes);
-  ul_dcch_msg.msg.ul_info_transfer.dedicated_info.N_bytes = sdu->N_bytes;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Reset and reuse sdu buffer
-  byte_buffer_t *pdu = sdu;
-  pdu->reset();
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+  void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu) {
+    rrc_log->debug("Preparing RX Info Transfer\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+    // Prepare RX INFO packet
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
+    ul_dcch_msg.msg.ul_info_transfer.dedicated_info_type = LIBLTE_RRC_UL_INFORMATION_TRANSFER_TYPE_NAS;
+    memcpy(ul_dcch_msg.msg.ul_info_transfer.dedicated_info.msg, sdu->msg, sdu->N_bytes);
+    ul_dcch_msg.msg.ul_info_transfer.dedicated_info.N_bytes = sdu->N_bytes;
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Reset and reuse sdu buffer
+    byte_buffer_t *pdu = sdu;
+    pdu->reset();
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+    pdu->N_bytes = bit_buf.N_bits / 8;
+    pdu->set_timestamp();
+    pdu->set_timestamp();
+
+    rrc_log->info("Sending RX Info Transfer\n");
+    pdcp->write_sdu(lcid, pdu);
   }
-  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
-  pdu->set_timestamp();
-  pdu->set_timestamp();
-
-  rrc_log->info("Sending RX Info Transfer\n");
-  pdcp->write_sdu(lcid, pdu);
-}
-
-void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu)
-{
-  rrc_log->debug("Preparing Security Mode Complete\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_SECURITY_MODE_COMPLETE;
-  ul_dcch_msg.msg.security_mode_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+  void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu) {
+    rrc_log->debug("Preparing Security Mode Complete\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_SECURITY_MODE_COMPLETE;
+    ul_dcch_msg.msg.security_mode_complete.rrc_transaction_id = transaction_id;
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+    pdu->N_bytes = bit_buf.N_bits / 8;
+    pdu->set_timestamp();
+
+    rrc_log->info("Sending Security Mode Complete\n");
+    pdcp->write_sdu(lcid, pdu);
   }
-  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
-  pdu->set_timestamp();
-
-  rrc_log->info("Sending Security Mode Complete\n");
-  pdcp->write_sdu(lcid, pdu);
-}
-
-void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu)
-{
-  rrc_log->debug("Preparing RRC Connection Reconfig Complete\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_RECONFIG_COMPLETE;
-  ul_dcch_msg.msg.rrc_con_reconfig_complete.rrc_transaction_id = transaction_id;
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+  void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu) {
+    rrc_log->debug("Preparing RRC Connection Reconfig Complete\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_RECONFIG_COMPLETE;
+    ul_dcch_msg.msg.rrc_con_reconfig_complete.rrc_transaction_id = transaction_id;
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+    pdu->N_bytes = bit_buf.N_bits / 8;
+    pdu->set_timestamp();
+
+    rrc_log->info("Sending RRC Connection Reconfig Complete\n");
+    pdcp->write_sdu(lcid, pdu);
   }
-  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
-  pdu->set_timestamp();
-
-  rrc_log->info("Sending RRC Connection Reconfig Complete\n");
-  pdcp->write_sdu(lcid, pdu);
-}
-
-void rrc::enable_capabilities()
-{
-  bool enable_ul_64 = ue_category>=5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
-  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64?"Enabling":"Disabling");
-  phy->set_config_64qam_en(enable_ul_64);  
-}
-
-void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu)
-{
-  rrc_log->debug("Preparing UE Capability Info\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
-  ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
-
-  LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
-  info->N_ue_caps = 1;
-  info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
-
-  LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
-  cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
-  cap->ue_category = ue_category;
-
-  cap->pdcp_params.max_rohc_ctxts_present = false;
-  cap->pdcp_params.supported_rohc_profiles[0] = false;
-  cap->pdcp_params.supported_rohc_profiles[1] = false;
-  cap->pdcp_params.supported_rohc_profiles[2] = false;
-  cap->pdcp_params.supported_rohc_profiles[3] = false;
-  cap->pdcp_params.supported_rohc_profiles[4] = false;
-  cap->pdcp_params.supported_rohc_profiles[5] = false;
-  cap->pdcp_params.supported_rohc_profiles[6] = false;
-  cap->pdcp_params.supported_rohc_profiles[7] = false;
-  cap->pdcp_params.supported_rohc_profiles[8] = false;
-
-  cap->phy_params.specific_ref_sigs_supported = false;
-  cap->phy_params.tx_antenna_selection_supported = false;
-
-  //TODO: Generate this from user input?
-  cap->rf_params.N_supported_band_eutras = 3;
-  cap->rf_params.supported_band_eutra[0].band_eutra = 3;
-  cap->rf_params.supported_band_eutra[0].half_duplex = false;
-  cap->rf_params.supported_band_eutra[1].band_eutra = 7;
-  cap->rf_params.supported_band_eutra[1].half_duplex = false;
-  cap->rf_params.supported_band_eutra[2].band_eutra = 20;
-  cap->rf_params.supported_band_eutra[2].half_duplex = false;
-
-  cap->meas_params.N_band_list_eutra = 3;
-  cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
-  cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
-  cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
-
-  cap->feature_group_indicator_present         = true;
-  cap->feature_group_indicator                 = 0x62001000;
-  cap->inter_rat_params.utra_fdd_present       = false;
-  cap->inter_rat_params.utra_tdd128_present    = false;
-  cap->inter_rat_params.utra_tdd384_present    = false;
-  cap->inter_rat_params.utra_tdd768_present    = false;
-  cap->inter_rat_params.geran_present          = false;
-  cap->inter_rat_params.cdma2000_hrpd_present  = false;
-  cap->inter_rat_params.cdma2000_1xrtt_present = false;
-
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT*)&bit_buf);
-
-  // Byte align and pack the message bits for PDCP
-  if((bit_buf.N_bits % 8) != 0)
-  {
-    for(uint32_t i=0; i<8-(bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+
+  void rrc::enable_capabilities() {
+    bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+    rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
+    phy->set_config_64qam_en(enable_ul_64);
   }
-  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits/8;
-  pdu->set_timestamp();
 
-  rrc_log->info("Sending UE Capability Info\n");
-  pdcp->write_sdu(lcid, pdu);
-}
+  void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
+    rrc_log->debug("Preparing UE Capability Info\n");
+    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
+    ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
+
+    LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
+    info->N_ue_caps = 1;
+    info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
+
+    LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
+    cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
+    cap->ue_category = ue_category;
+
+    cap->pdcp_params.max_rohc_ctxts_present = false;
+    cap->pdcp_params.supported_rohc_profiles[0] = false;
+    cap->pdcp_params.supported_rohc_profiles[1] = false;
+    cap->pdcp_params.supported_rohc_profiles[2] = false;
+    cap->pdcp_params.supported_rohc_profiles[3] = false;
+    cap->pdcp_params.supported_rohc_profiles[4] = false;
+    cap->pdcp_params.supported_rohc_profiles[5] = false;
+    cap->pdcp_params.supported_rohc_profiles[6] = false;
+    cap->pdcp_params.supported_rohc_profiles[7] = false;
+    cap->pdcp_params.supported_rohc_profiles[8] = false;
+
+    cap->phy_params.specific_ref_sigs_supported = false;
+    cap->phy_params.tx_antenna_selection_supported = false;
+
+    //TODO: Generate this from user input?
+    cap->rf_params.N_supported_band_eutras = 3;
+    cap->rf_params.supported_band_eutra[0].band_eutra = 3;
+    cap->rf_params.supported_band_eutra[0].half_duplex = false;
+    cap->rf_params.supported_band_eutra[1].band_eutra = 7;
+    cap->rf_params.supported_band_eutra[1].half_duplex = false;
+    cap->rf_params.supported_band_eutra[2].band_eutra = 20;
+    cap->rf_params.supported_band_eutra[2].half_duplex = false;
+
+    cap->meas_params.N_band_list_eutra = 3;
+    cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
+    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
+    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
+    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
+    cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
+    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
+    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
+    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
+    cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
+    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
+    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
+    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
+
+    cap->feature_group_indicator_present = true;
+    cap->feature_group_indicator = 0x62001000;
+    cap->inter_rat_params.utra_fdd_present = false;
+    cap->inter_rat_params.utra_tdd128_present = false;
+    cap->inter_rat_params.utra_tdd384_present = false;
+    cap->inter_rat_params.utra_tdd768_present = false;
+    cap->inter_rat_params.geran_present = false;
+    cap->inter_rat_params.cdma2000_hrpd_present = false;
+    cap->inter_rat_params.cdma2000_1xrtt_present = false;
+
+    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+    // Byte align and pack the message bits for PDCP
+    if ((bit_buf.N_bits % 8) != 0) {
+      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+        bit_buf.msg[bit_buf.N_bits + i] = 0;
+      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+    }
+    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+    pdu->N_bytes = bit_buf.N_bits / 8;
+    pdu->set_timestamp();
+
+    rrc_log->info("Sending UE Capability Info\n");
+    pdcp->write_sdu(lcid, pdu);
+  }
 
 /*******************************************************************************
   Parsers
 *******************************************************************************/
 
-void rrc::parse_dl_ccch(byte_buffer_t *pdu)
-{
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  pool->deallocate(pdu);
-  bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
-  liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_ccch_msg);
-
-  rrc_log->info("SRB0 - Received %s\n",
-                liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg.msg_type]);
-
-  switch(dl_ccch_msg.msg_type)
-  {
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
-    rrc_log->info("Connection Reject received. Wait time: %d\n",
-                  dl_ccch_msg.msg.rrc_con_rej.wait_time);
-    state = RRC_STATE_IDLE;
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
-    rrc_log->info("Connection Setup received\n");
-    transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
-    handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
-    rrc_log->info("Notifying NAS of connection setup\n");
-    nas->notify_connection_setup();
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
-    rrc_log->info("Connection Reestablishment received\n");
-    rrc_log->console("Reestablishment OK\n");
-    transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
-    handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
-    break;
-  case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
-    rrc_log->info("Connection Reestablishment Reject received\n");
-    rrc_log->console("Reestablishment Reject\n");
-    usleep(50000);
-    rrc_connection_release();
-    break;
-  default:
-    break;
+  void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
+    pool->deallocate(pdu);
+    bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
+    liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_ccch_msg);
+
+    rrc_log->info("SRB0 - Received %s\n",
+                  liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg.msg_type]);
+
+    switch (dl_ccch_msg.msg_type) {
+      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
+        rrc_log->info("Connection Reject received. Wait time: %d\n",
+                      dl_ccch_msg.msg.rrc_con_rej.wait_time);
+        state = RRC_STATE_IDLE;
+        break;
+      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
+        rrc_log->info("Connection Setup received\n");
+        transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
+        handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
+        rrc_log->info("Notifying NAS of connection setup\n");
+        nas->notify_connection_setup();
+        break;
+      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
+        rrc_log->info("Connection Reestablishment received\n");
+        rrc_log->console("Reestablishment OK\n");
+        transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
+        handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
+        break;
+      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
+        rrc_log->info("Connection Reestablishment Reject received\n");
+        rrc_log->console("Reestablishment Reject\n");
+        usleep(50000);
+        rrc_connection_release();
+        break;
+      default:
+        break;
+    }
   }
-}
-
-void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu)
-{
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes*8);
-  bit_buf.N_bits = pdu->N_bytes*8;
-  liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_dcch_msg);
-
-  rrc_log->info("%s - Received %s\n",
-                rb_id_text[lcid],
-                liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
-
-  // Reset and reuse pdu buffer if possible
-  pdu->reset();
-
-  switch(dl_dcch_msg.msg_type)
-  {
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
-    memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
-    pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
-    nas->write_pdu(lcid, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
-    transaction_id =  dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
-
-    cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM)dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
-    integ_algo  = (INTEGRITY_ALGORITHM_ID_ENUM)dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
-
-    // Configure PDCP for security
-    usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
-    pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
-    send_security_mode_complete(lcid, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
-    transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
-    handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
-    transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
-    for(uint32_t i=0; i<dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++)
-    {
-      if(LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i])
-      {
-        send_rrc_ue_cap_info(lcid, pdu);
+
+  void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
+    liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_dcch_msg);
+
+    rrc_log->info("%s - Received %s\n",
+                  rb_id_text[lcid],
+                  liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
+
+    // Reset and reuse pdu buffer if possible
+    pdu->reset();
+
+    switch (dl_dcch_msg.msg_type) {
+      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
+        memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg,
+               dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
+        pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
+        nas->write_pdu(lcid, pdu);
+        break;
+      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
+        transaction_id = dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
+
+        cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
+        integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
+
+        // Configure PDCP for security
+        usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
+        pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+        send_security_mode_complete(lcid, pdu);
+        break;
+      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
+        transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
+        handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
+        break;
+      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
+        transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
+        for (uint32_t i = 0; i < dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++) {
+          if (LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i]) {
+            send_rrc_ue_cap_info(lcid, pdu);
+            break;
+          }
+        }
+        break;
+      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
+        rrc_connection_release();
+        break;
+      default:
         break;
-      }
     }
-    break;
-  case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
-    rrc_connection_release();
-    break;
-  default:
-    break;
   }
-}
 
 
 /*******************************************************************************
   Timer expiration callback 
 *******************************************************************************/
-void rrc::timer_expired(uint32_t timeout_id)
-{
-  if (timeout_id == t310) {
-    rrc_log->info("Timer T310 expired: Radio Link Failure\n");
-    radio_link_failure();
-  } else if (timeout_id == t311) {
-    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
-    rrc_connection_release();
-  } else if (timeout_id == t301) {
-    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
-    rrc_connection_release();
-  } else if (timeout_id == safe_reset_timer) {
-    reset_ue();
-  } else {
-    rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
+  void rrc::timer_expired(uint32_t timeout_id) {
+    if (timeout_id == t310) {
+      rrc_log->info("Timer T310 expired: Radio Link Failure\n");
+      radio_link_failure();
+    } else if (timeout_id == t311) {
+      rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
+      rrc_connection_release();
+    } else if (timeout_id == t301) {
+      rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
+      rrc_connection_release();
+    } else if (timeout_id == safe_reset_timer) {
+      reset_ue();
+    } else {
+      rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
+    }
   }
-}
 
 /*******************************************************************************
   Helpers
 *******************************************************************************/
 
-void rrc::reset_ue() {
-  phy->reset();
-  mac->reset();
-  pdcp->reset();  
-  rlc->reset();
-  mac->pcch_start_rx();
-  mac_timers->get(safe_reset_timer)->stop();
-  mac_timers->get(safe_reset_timer)->reset();
-  rrc_log->console("RRC Connection released.\n");
-}
-
-void rrc::rrc_connection_release() {
-  pthread_mutex_lock(&mutex);
-  drb_up = false;
-  state  = RRC_STATE_IDLE;
-  set_phy_default();
-  set_mac_default();
-  mac_timers->get(t311)->run();
-  mac_timers->get(t310)->stop();
-  mac_timers->get(t311)->stop();
-  mac_timers->get(safe_reset_timer)->stop();
-  mac_timers->get(safe_reset_timer)->reset();
-  mac_timers->get(safe_reset_timer)->run();
-  pthread_mutex_unlock(&mutex);
-}
-
-void rrc::test_con_restablishment()
-{
-  printf("Testing connection Reestablishment\n"); 
-  send_con_restablish_request();
-}
+  void rrc::reset_ue() {
+    phy->reset();
+    mac->reset();
+    pdcp->reset();
+    rlc->reset();
+    mac->pcch_start_rx();
+    mac_timers->get(safe_reset_timer)->stop();
+    mac_timers->get(safe_reset_timer)->reset();
+    rrc_log->console("RRC Connection released.\n");
+  }
 
-/* Detection of radio link failure (5.3.11.3) */
-void rrc::radio_link_failure() {
-  // TODO: Generate and store failure report 
-  
-  rrc_log->warning("Detected Radio-Link Failure\n");
-  rrc_log->console("Warning: Detected Radio-Link Failure\n");
-  if (state != RRC_STATE_CONNECTED) {
-    rrc_connection_release();
-  } else {    
+  void rrc::rrc_connection_release() {
+    pthread_mutex_lock(&mutex);
+    drb_up = false;
+    state = RRC_STATE_IDLE;
+    set_phy_default();
+    set_mac_default();
+    mac_timers->get(t311)->run();
+    mac_timers->get(t310)->stop();
+    mac_timers->get(t311)->stop();
+    mac_timers->get(safe_reset_timer)->stop();
+    mac_timers->get(safe_reset_timer)->reset();
+    mac_timers->get(safe_reset_timer)->run();
+    pthread_mutex_unlock(&mutex);
+  }
+
+  void rrc::test_con_restablishment() {
+    printf("Testing connection Reestablishment\n");
     send_con_restablish_request();
   }
-}
 
+/* Detection of radio link failure (5.3.11.3) */
+  void rrc::radio_link_failure() {
+    // TODO: Generate and store failure report
 
-// Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
-uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
-  return (period*10*(1+tti/(period*10))+x)%10240; // the 1 means next opportunity
-}
-
-void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2)
-{
-  
-  // Apply RACH timeAlginmentTimer configuration 
-  mac_interface_rrc::mac_cfg_t cfg; 
-  mac->get_config(&cfg);
-  cfg.main.time_alignment_timer = sib2->time_alignment_timer; 
-  memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
-  cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index; 
-  mac->set_config(&cfg);
-  
-  rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
-         liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
-         liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
-         liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
-
-  // Apply PHY RR Config Common
-  phy_interface_rrc::phy_cfg_common_t common; 
-  memcpy(&common.pdsch_cnfg,  &sib2->rr_config_common_sib.pdsch_cnfg,  sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pusch_cnfg,  &sib2->rr_config_common_sib.pusch_cnfg,  sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.pucch_cnfg,  &sib2->rr_config_common_sib.pucch_cnfg,  sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
-  memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
-  memcpy(&common.prach_cnfg,  &sib2->rr_config_common_sib.prach_cnfg,  sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
-  if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
-    memcpy(&common.srs_ul_cnfg,  &sib2->rr_config_common_sib.srs_ul_cnfg, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
-  } else {
-    // default is release
-    common.srs_ul_cnfg.present = false; 
+    rrc_log->warning("Detected Radio-Link Failure\n");
+    rrc_log->console("Warning: Detected Radio-Link Failure\n");
+    if (state != RRC_STATE_CONNECTED) {
+      rrc_connection_release();
+    } else {
+      send_con_restablish_request();
+    }
   }
-  phy->set_config_common(&common);
-
-  phy->configure_ul_params();
-
-  rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
-                sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
-                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
-                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
-                sib2->rr_config_common_sib.pusch_cnfg.n_sb);
-
-  rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
-                liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
-                sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
-                sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
-                sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
-  
-  rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
-                 sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
-                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag?"yes":"no",
-                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
-                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
-                 sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
-
-  rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
-                 liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
-                 liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
-                 sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx?"yes":"no");
-
-  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
-  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
-  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
-  N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
-  N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
-  
-  rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n", 
-    N310, N311, mac_timers->get(t301)->get_timeout(), 
-    mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
-  
-}
 
-  // Go through all information elements and apply defaults (9.2.4) if not defined
-void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) 
-{
-  // Get current configuration 
-  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg; 
-  phy_interface_rrc::phy_cfg_t c; 
-  phy->get_config(&c);
-  current_cfg = &c.dedicated;
-    
-  if(phy_cnfg->pucch_cnfg_ded_present) {
-    memcpy(&current_cfg->pucch_cnfg_ded, &phy_cnfg->pucch_cnfg_ded, sizeof(LIBLTE_RRC_PUCCH_CONFIG_DEDICATED_STRUCT));
-  } else if (apply_defaults) {
-    current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING; 
-    current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false; 
-  }
-  if(phy_cnfg->pusch_cnfg_ded_present) {
-    memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT)); 
-  } else if (apply_defaults) {
-    current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10; 
-    current_cfg->pusch_cnfg_ded.beta_offset_ri_idx  = 12;
-    current_cfg->pusch_cnfg_ded.beta_offset_cqi_idx = 15;
-  }
-  if(phy_cnfg->ul_pwr_ctrl_ded_present) {
-    memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded, sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT)); 
-  } else if (apply_defaults) {
-    current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch     = 0; 
-    current_cfg->ul_pwr_ctrl_ded.delta_mcs_en    = LIBLTE_RRC_DELTA_MCS_ENABLED_EN0; 
-    current_cfg->ul_pwr_ctrl_ded.accumulation_en = true; 
-    current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch     = 0;
-    current_cfg->ul_pwr_ctrl_ded.p_srs_offset    = 7;
-    current_cfg->ul_pwr_ctrl_ded.filter_coeff    = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;      
-    current_cfg->ul_pwr_ctrl_ded.filter_coeff_present = true; 
-  }
-  if(phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
-    memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch, sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT)); 
-  } else if (apply_defaults) {
-    current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false; 
+
+// Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
+  uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
+    return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
   }
-  if(phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
-    memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch, sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT)); 
-  } else {
-    current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false; 
+
+  void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
+
+    // Apply RACH timeAlginmentTimer configuration
+    mac_interface_rrc::mac_cfg_t cfg;
+    mac->get_config(&cfg);
+    cfg.main.time_alignment_timer = sib2->time_alignment_timer;
+    memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT));
+    cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index;
+    mac->set_config(&cfg);
+
+    rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
+                  liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
+                  liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
+                  liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
+
+    // Apply PHY RR Config Common
+    phy_interface_rrc::phy_cfg_common_t common;
+    memcpy(&common.pdsch_cnfg, &sib2->rr_config_common_sib.pdsch_cnfg, sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
+    memcpy(&common.pusch_cnfg, &sib2->rr_config_common_sib.pusch_cnfg, sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
+    memcpy(&common.pucch_cnfg, &sib2->rr_config_common_sib.pucch_cnfg, sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
+    memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl,
+           sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
+    memcpy(&common.prach_cnfg, &sib2->rr_config_common_sib.prach_cnfg, sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
+    if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
+      memcpy(&common.srs_ul_cnfg, &sib2->rr_config_common_sib.srs_ul_cnfg,
+             sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
+    } else {
+      // default is release
+      common.srs_ul_cnfg.present = false;
+    }
+    phy->set_config_common(&common);
+
+    phy->configure_ul_params();
+
+    rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
+                  sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
+                  sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
+                  sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
+                  sib2->rr_config_common_sib.pusch_cnfg.n_sb);
+
+    rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
+                  liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
+                  sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
+                  sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
+                  sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
+
+    rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
+                  sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
+                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag ? "yes" : "no",
+                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
+                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
+                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
+
+    rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
+                  liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
+                  liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
+                  sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
+
+    mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
+    mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
+    mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+    N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
+    N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
+
+    rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n",
+                  N310, N311, mac_timers->get(t301)->get_timeout(),
+                  mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
+
   }
-  if(phy_cnfg->cqi_report_cnfg_present) {
-    if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-      memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic, sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
-      current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present; 
+
+  // Go through all information elements and apply defaults (9.2.4) if not defined
+  void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) {
+    // Get current configuration
+    LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg;
+    phy_interface_rrc::phy_cfg_t c;
+    phy->get_config(&c);
+    current_cfg = &c.dedicated;
+
+    if (phy_cnfg->pucch_cnfg_ded_present) {
+      memcpy(&current_cfg->pucch_cnfg_ded, &phy_cnfg->pucch_cnfg_ded, sizeof(LIBLTE_RRC_PUCCH_CONFIG_DEDICATED_STRUCT));
     } else if (apply_defaults) {
-      current_cfg->cqi_report_cnfg.report_periodic_setup_present = false; 
+      current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING;
+      current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false;
     }
-    if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-      current_cfg->cqi_report_cnfg.report_mode_aperiodic         = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
-      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present;
+    if (phy_cnfg->pusch_cnfg_ded_present) {
+      memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT));
     } else if (apply_defaults) {
-      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false; 
+      current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10;
+      current_cfg->pusch_cnfg_ded.beta_offset_ri_idx = 12;
+      current_cfg->pusch_cnfg_ded.beta_offset_cqi_idx = 15;
     }
-    current_cfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset = phy_cnfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset;
-  }
-  if(phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
-    memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded, sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT)); 
-  } else if (apply_defaults) {
-    current_cfg->srs_ul_cnfg_ded.setup_present = false; 
-  }
-  if(phy_cnfg->antenna_info_present) {
-    if (!phy_cnfg->antenna_info_default_value) {
-      if(phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
-         phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
-        rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n", liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
+    if (phy_cnfg->ul_pwr_ctrl_ded_present) {
+      memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded,
+             sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT));
+    } else if (apply_defaults) {
+      current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch = 0;
+      current_cfg->ul_pwr_ctrl_ded.delta_mcs_en = LIBLTE_RRC_DELTA_MCS_ENABLED_EN0;
+      current_cfg->ul_pwr_ctrl_ded.accumulation_en = true;
+      current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch = 0;
+      current_cfg->ul_pwr_ctrl_ded.p_srs_offset = 7;
+      current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
+      current_cfg->ul_pwr_ctrl_ded.filter_coeff_present = true;
+    }
+    if (phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
+      memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch,
+             sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
+    } else if (apply_defaults) {
+      current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false;
+    }
+    if (phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
+      memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch,
+             sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
+    } else {
+      current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false;
+    }
+    if (phy_cnfg->cqi_report_cnfg_present) {
+      if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
+        memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic,
+               sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
+        current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present;
+      } else if (apply_defaults) {
+        current_cfg->cqi_report_cnfg.report_periodic_setup_present = false;
+      }
+      if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
+        current_cfg->cqi_report_cnfg.report_mode_aperiodic = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
+        current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present;
+      } else if (apply_defaults) {
+        current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false;
+      }
+      current_cfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset = phy_cnfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset;
+    }
+    if (phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
+      memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded,
+             sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT));
+    } else if (apply_defaults) {
+      current_cfg->srs_ul_cnfg_ded.setup_present = false;
+    }
+    if (phy_cnfg->antenna_info_present) {
+      if (!phy_cnfg->antenna_info_default_value) {
+        if (phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
+            phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
+          rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n",
+                         liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
+        }
+        memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value,
+               sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT));
+      } else if (apply_defaults) {
+        current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
+        current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
+        current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
       }
-      memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value, sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT)); 
     } else if (apply_defaults) {
       current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
-      current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false; 
-      current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false; 
+      current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
+      current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
+    }
+    if (phy_cnfg->sched_request_cnfg_present) {
+      memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg,
+             sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+    } else if (apply_defaults) {
+      current_cfg->sched_request_cnfg.setup_present = false;
+    }
+    if (phy_cnfg->pdsch_cnfg_ded_present) {
+      current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded;
+    } else if (apply_defaults) {
+      current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0;
     }
-  } else if (apply_defaults) {
-    current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
-    current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false; 
-    current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false; 
-  }
-  if(phy_cnfg->sched_request_cnfg_present) {
-    memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-  } else if (apply_defaults) {
-    current_cfg->sched_request_cnfg.setup_present = false; 
-  }
-  if(phy_cnfg->pdsch_cnfg_ded_present) {
-    current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded; 
-  } else if (apply_defaults) {
-    current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0; 
-  }
 
-  if (phy_cnfg->cqi_report_cnfg_present) {
-    if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-      rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n", 
-        current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx, 
-        current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx, 
-        liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]); 
-    } 
-    if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-      rrc_log->info("Set cqi-ReportModeAperiodic=%s\n", 
-                    liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]); 
-    } 
-    
-  }
-  
-  if (phy_cnfg->sched_request_cnfg_present) {
-    rrc_log->info("Set PHY config ded: SR-n_pucch=%d, SR-ConfigIndex=%d, SR-TransMax=%d\n",
-                current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
-                current_cfg->sched_request_cnfg.sr_cnfg_idx,
-                liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
-  }
-  
-  if (current_cfg->srs_ul_cnfg_ded_present) {
-    rrc_log->info("Set PHY config ded: SRS-ConfigIndex=%d, SRS-bw=%s, SRS-Nrcc=%d, SRS-hop=%s, SRS-Ncs=%s\n",
-                current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
-                liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
-                current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
-                liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
-                liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
-  }
-  
-  phy->set_config_dedicated(current_cfg);
-
-  // Apply changes to PHY
-  phy->configure_ul_params();
-
-}
-
-void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) 
-{
-  // Set Default MAC main configuration (9.2.2)
-  LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
-  bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
-  default_cfg.ulsch_cnfg.max_harq_tx        = LIBLTE_RRC_MAX_HARQ_TX_N5;
-  default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
-  default_cfg.ulsch_cnfg.retx_bsr_timer     = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;  
-  default_cfg.ulsch_cnfg.tti_bundling       = false;
-  default_cfg.drx_cnfg.setup_present        = false;
-  default_cfg.phr_cnfg.setup_present        = false;
-  default_cfg.time_alignment_timer          = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY; 
-
-
-  if (!apply_defaults) {
-    if(mac_cnfg->ulsch_cnfg_present)
-    {
-      if(mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
-        default_cfg.ulsch_cnfg.max_harq_tx = mac_cnfg->ulsch_cnfg.max_harq_tx;
-        default_cfg.ulsch_cnfg.max_harq_tx_present = true; 
+    if (phy_cnfg->cqi_report_cnfg_present) {
+      if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
+        rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n",
+                      current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx,
+                      current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx,
+                      liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]);
       }
-      if(mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
-        default_cfg.ulsch_cnfg.periodic_bsr_timer = mac_cnfg->ulsch_cnfg.periodic_bsr_timer;
-        default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true; 
+      if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
+        rrc_log->info("Set cqi-ReportModeAperiodic=%s\n",
+                      liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]);
       }
-      default_cfg.ulsch_cnfg.retx_bsr_timer = mac_cnfg->ulsch_cnfg.retx_bsr_timer;
-      default_cfg.ulsch_cnfg.tti_bundling   = mac_cnfg->ulsch_cnfg.tti_bundling;
+
     }
-    if(mac_cnfg->drx_cnfg_present) {
-      memcpy(&default_cfg.drx_cnfg, &mac_cnfg->drx_cnfg, sizeof(LIBLTE_RRC_DRX_CONFIG_STRUCT));
-      default_cfg.drx_cnfg_present = true; 
+
+    if (phy_cnfg->sched_request_cnfg_present) {
+      rrc_log->info("Set PHY config ded: SR-n_pucch=%d, SR-ConfigIndex=%d, SR-TransMax=%d\n",
+                    current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
+                    current_cfg->sched_request_cnfg.sr_cnfg_idx,
+                    liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
     }
-    if(mac_cnfg->phr_cnfg_present) {
-      memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT)); 
-      default_cfg.phr_cnfg_present = true; 
+
+    if (current_cfg->srs_ul_cnfg_ded_present) {
+      rrc_log->info("Set PHY config ded: SRS-ConfigIndex=%d, SRS-bw=%s, SRS-Nrcc=%d, SRS-hop=%s, SRS-Ncs=%s\n",
+                    current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
+                    liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
+                    current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
+                    liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
+                    liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
     }
-    default_cfg.time_alignment_timer = mac_cnfg->time_alignment_timer;
-  }
-  
-  // Setup MAC configuration 
-  mac->set_config_main(&default_cfg);              
-
-  rrc_log->info("Set MAC main config: harq-MaxReTX=%d, bsr-TimerReTX=%d, bsr-TimerPeriodic=%d\n",
-                liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx],
-                liblte_rrc_retransmission_bsr_timer_num[default_cfg.ulsch_cnfg.retx_bsr_timer],
-                liblte_rrc_periodic_bsr_timer_num[default_cfg.ulsch_cnfg.periodic_bsr_timer]);
-  if (default_cfg.phr_cnfg_present) {
-    rrc_log->info("Set MAC PHR config: periodicPHR-Timer=%d, prohibitPHR-Timer=%d, dl-PathlossChange=%d\n",
-      liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
-      liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
-      liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
-  }
-}
-
-void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {   
-  if(cnfg->phy_cnfg_ded_present) {
-   apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
-   // Apply SR configuration to MAC 
-   if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
-    mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
-   }
-  }
-  
-  if(cnfg->mac_main_cnfg_present) {
-    apply_mac_config_dedicated(&cnfg->mac_main_cnfg.explicit_value, cnfg->mac_main_cnfg.default_value);
-  }
-  
-  if(cnfg->sps_cnfg_present) {
-    //TODO
-  }
-  if(cnfg->rlf_timers_and_constants_present) {
-    //TODO
-  }
-  for(uint32_t i=0; i<cnfg->srb_to_add_mod_list_size; i++) {
-    // TODO: handle SRB modification
-    add_srb(&cnfg->srb_to_add_mod_list[i]);
-  }
-  for(uint32_t i=0; i<cnfg->drb_to_release_list_size; i++) {
-    release_drb(cnfg->drb_to_release_list[i]);
-  }
-  for(uint32_t i=0; i<cnfg->drb_to_add_mod_list_size; i++) {
-    // TODO: handle DRB modification
-    add_drb(&cnfg->drb_to_add_mod_list[i]);
+
+    phy->set_config_dedicated(current_cfg);
+
+    // Apply changes to PHY
+    phy->configure_ul_params();
+
   }
-}
 
-void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup)
-{
-  // Apply the Radio Resource configuration
-  apply_rr_config_dedicated(&setup->rr_cnfg);  
-}
+  void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) {
+    // Set Default MAC main configuration (9.2.2)
+    LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
+    bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
+    default_cfg.ulsch_cnfg.max_harq_tx = LIBLTE_RRC_MAX_HARQ_TX_N5;
+    default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
+    default_cfg.ulsch_cnfg.retx_bsr_timer = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;
+    default_cfg.ulsch_cnfg.tti_bundling = false;
+    default_cfg.drx_cnfg.setup_present = false;
+    default_cfg.phr_cnfg.setup_present = false;
+    default_cfg.time_alignment_timer = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY;
+
+
+    if (!apply_defaults) {
+      if (mac_cnfg->ulsch_cnfg_present) {
+        if (mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
+          default_cfg.ulsch_cnfg.max_harq_tx = mac_cnfg->ulsch_cnfg.max_harq_tx;
+          default_cfg.ulsch_cnfg.max_harq_tx_present = true;
+        }
+        if (mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
+          default_cfg.ulsch_cnfg.periodic_bsr_timer = mac_cnfg->ulsch_cnfg.periodic_bsr_timer;
+          default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true;
+        }
+        default_cfg.ulsch_cnfg.retx_bsr_timer = mac_cnfg->ulsch_cnfg.retx_bsr_timer;
+        default_cfg.ulsch_cnfg.tti_bundling = mac_cnfg->ulsch_cnfg.tti_bundling;
+      }
+      if (mac_cnfg->drx_cnfg_present) {
+        memcpy(&default_cfg.drx_cnfg, &mac_cnfg->drx_cnfg, sizeof(LIBLTE_RRC_DRX_CONFIG_STRUCT));
+        default_cfg.drx_cnfg_present = true;
+      }
+      if (mac_cnfg->phr_cnfg_present) {
+        memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT));
+        default_cfg.phr_cnfg_present = true;
+      }
+      default_cfg.time_alignment_timer = mac_cnfg->time_alignment_timer;
+    }
 
-/* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
-void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) 
-{
-  mac_timers->get(t301)->stop();
-  
-  // TODO: Restablish DRB1. Not done because never was suspended
-
-  // Apply the Radio Resource configuration
-  apply_rr_config_dedicated(&setup->rr_cnfg);
-
-  // TODO: Some security stuff here... is it necessary?
-  
-  send_con_restablish_complete();
-}
-
-
-void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu)
-{
-  uint32_t i;
-  
-  if (reconfig->rr_cnfg_ded_present) {
-    apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
-  } else {
-    printf("received con reconfig no rr confg present\n");
+    // Setup MAC configuration
+    mac->set_config_main(&default_cfg);
+
+    rrc_log->info("Set MAC main config: harq-MaxReTX=%d, bsr-TimerReTX=%d, bsr-TimerPeriodic=%d\n",
+                  liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx],
+                  liblte_rrc_retransmission_bsr_timer_num[default_cfg.ulsch_cnfg.retx_bsr_timer],
+                  liblte_rrc_periodic_bsr_timer_num[default_cfg.ulsch_cnfg.periodic_bsr_timer]);
+    if (default_cfg.phr_cnfg_present) {
+      rrc_log->info("Set MAC PHR config: periodicPHR-Timer=%d, prohibitPHR-Timer=%d, dl-PathlossChange=%d\n",
+                    liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
+                    liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
+                    liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
+    }
   }
-  if(reconfig->meas_cnfg_present)
-  {
-    //TODO: handle meas_cnfg
+
+  void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {
+    if (cnfg->phy_cnfg_ded_present) {
+      apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
+      // Apply SR configuration to MAC
+      if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
+        mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
+      }
+    }
+
+    if (cnfg->mac_main_cnfg_present) {
+      apply_mac_config_dedicated(&cnfg->mac_main_cnfg.explicit_value, cnfg->mac_main_cnfg.default_value);
+    }
+
+    if (cnfg->sps_cnfg_present) {
+      //TODO
+    }
+    if (cnfg->rlf_timers_and_constants_present) {
+      //TODO
+    }
+    for (uint32_t i = 0; i < cnfg->srb_to_add_mod_list_size; i++) {
+      // TODO: handle SRB modification
+      add_srb(&cnfg->srb_to_add_mod_list[i]);
+    }
+    for (uint32_t i = 0; i < cnfg->drb_to_release_list_size; i++) {
+      release_drb(cnfg->drb_to_release_list[i]);
+    }
+    for (uint32_t i = 0; i < cnfg->drb_to_add_mod_list_size; i++) {
+      // TODO: handle DRB modification
+      add_drb(&cnfg->drb_to_add_mod_list[i]);
+    }
   }
-  if(reconfig->mob_ctrl_info_present)
-  {
-    //TODO: handle mob_ctrl_info
+
+  void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
+    // Apply the Radio Resource configuration
+    apply_rr_config_dedicated(&setup->rr_cnfg);
   }
 
-  send_rrc_con_reconfig_complete(lcid, pdu);
+/* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
+  void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
+    mac_timers->get(t301)->stop();
+
+    // TODO: Restablish DRB1. Not done because never was suspended
 
-  byte_buffer_t *nas_sdu;
-  for(i=0;i<reconfig->N_ded_info_nas;i++)
-  {
-    nas_sdu = pool_allocate;;
-    memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
-    nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
-    nas->write_pdu(lcid, nas_sdu);
+    // Apply the Radio Resource configuration
+    apply_rr_config_dedicated(&setup->rr_cnfg);
+
+    // TODO: Some security stuff here... is it necessary?
+
+    send_con_restablish_complete();
   }
-}
-
-void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
-{
-  // Setup PDCP
-  pdcp->add_bearer(srb_cnfg->srb_id);
-  if(RB_ID_SRB2 == srb_cnfg->srb_id)
-    pdcp->config_security(srb_cnfg->srb_id, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
-
-  // Setup RLC
-  if(srb_cnfg->rlc_cnfg_present)
-  {
-    if(srb_cnfg->rlc_default_cnfg_present)
-    {
-      rlc->add_bearer(srb_cnfg->srb_id);
-    }else{
-      rlc->add_bearer(srb_cnfg->srb_id, &srb_cnfg->rlc_explicit_cnfg);
+
+
+  void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
+                                    byte_buffer_t *pdu) {
+    uint32_t i;
+
+    if (reconfig->rr_cnfg_ded_present) {
+      apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
+    } else {
+      printf("received con reconfig no rr confg present\n");
+    }
+    if (reconfig->meas_cnfg_present) {
+      //TODO: handle meas_cnfg
+    }
+    if (reconfig->mob_ctrl_info_present) {
+      //TODO: handle mob_ctrl_info
+    }
+
+    send_rrc_con_reconfig_complete(lcid, pdu);
+
+    byte_buffer_t *nas_sdu;
+    for (i = 0; i < reconfig->N_ded_info_nas; i++) {
+      nas_sdu = pool_allocate;;
+      memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
+      nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
+      nas->write_pdu(lcid, nas_sdu);
     }
   }
 
-  // Setup MAC
-  uint8_t  log_chan_group       =  0;
-  uint8_t  priority             =  1;
-  int      prioritized_bit_rate = -1;
-  int      bucket_size_duration = -1;
-
-  if(srb_cnfg->lc_cnfg_present)
-  {
-    if(srb_cnfg->lc_default_cnfg_present)
-    {
-      if(RB_ID_SRB2 == srb_cnfg->srb_id)
-        priority = 3;
-    }else{
-      if(srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present)
-      {
-        //TODO
-      }
-      if(srb_cnfg->lc_explicit_cnfg.ul_specific_params_present)
-      {
-        if(srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
-          log_chan_group      = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
-
-        priority              = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
-        prioritized_bit_rate  = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
-        bucket_size_duration  = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
+  void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg) {
+    // Setup PDCP
+    pdcp->add_bearer(srb_cnfg->srb_id);
+    if (RB_ID_SRB2 == srb_cnfg->srb_id)
+      pdcp->config_security(srb_cnfg->srb_id, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+
+    // Setup RLC
+    if (srb_cnfg->rlc_cnfg_present) {
+      if (srb_cnfg->rlc_default_cnfg_present) {
+        rlc->add_bearer(srb_cnfg->srb_id);
+      } else {
+        rlc->add_bearer(srb_cnfg->srb_id, &srb_cnfg->rlc_explicit_cnfg);
       }
     }
-    mac->setup_lcid(srb_cnfg->srb_id, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
-  }
 
-  srbs[srb_cnfg->srb_id] = *srb_cnfg;
-  rrc_log->info("Added radio bearer %s\n", rb_id_text[srb_cnfg->srb_id]);
-}
+    // Setup MAC
+    uint8_t log_chan_group = 0;
+    uint8_t priority = 1;
+    int prioritized_bit_rate = -1;
+    int bucket_size_duration = -1;
 
-void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
-{
+    if (srb_cnfg->lc_cnfg_present) {
+      if (srb_cnfg->lc_default_cnfg_present) {
+        if (RB_ID_SRB2 == srb_cnfg->srb_id)
+          priority = 3;
+      } else {
+        if (srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present) {
+          //TODO
+        }
+        if (srb_cnfg->lc_explicit_cnfg.ul_specific_params_present) {
+          if (srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
+            log_chan_group = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
 
-  if(!drb_cnfg->pdcp_cnfg_present ||
-     !drb_cnfg->rlc_cnfg_present  ||
-     !drb_cnfg->lc_cnfg_present)
-  {
-    rrc_log->error("Cannot add DRB - incomplete configuration\n");
-    return;
-  }
-  uint32_t lcid = 0; 
-  if (drb_cnfg->lc_id_present) {
-    lcid = drb_cnfg->lc_id;
-  } else {
-    lcid = RB_ID_SRB2 + drb_cnfg->drb_id;
-    rrc_log->warning("LCID not present, using %d\n", lcid);
+          priority = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
+          prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
+          bucket_size_duration = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
+        }
+      }
+      mac->setup_lcid(srb_cnfg->srb_id, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
+    }
+
+    srbs[srb_cnfg->srb_id] = *srb_cnfg;
+    rrc_log->info("Added radio bearer %s\n", rb_id_text[srb_cnfg->srb_id]);
   }
-  
-  // Setup PDCP
-  pdcp->add_bearer(lcid, &drb_cnfg->pdcp_cnfg);
-  // TODO: setup PDCP security (using k_up_enc)
-
-  // Setup RLC
-  rlc->add_bearer(lcid, &drb_cnfg->rlc_cnfg);
-
-  // Setup MAC
-  uint8_t  log_chan_group       =  0;
-  uint8_t  priority             =  1;
-  int      prioritized_bit_rate = -1;
-  int      bucket_size_duration = -1;
-  if(drb_cnfg->lc_cnfg.ul_specific_params_present)
-  {
-    if(drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
-      log_chan_group      = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
+
+  void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg) {
+
+    if (!drb_cnfg->pdcp_cnfg_present ||
+        !drb_cnfg->rlc_cnfg_present ||
+        !drb_cnfg->lc_cnfg_present) {
+      rrc_log->error("Cannot add DRB - incomplete configuration\n");
+      return;
+    }
+    uint32_t lcid = 0;
+    if (drb_cnfg->lc_id_present) {
+      lcid = drb_cnfg->lc_id;
     } else {
-      rrc_log->warning("LCG not present, setting to 0\n");
+      lcid = RB_ID_SRB2 + drb_cnfg->drb_id;
+      rrc_log->warning("LCID not present, using %d\n", lcid);
     }
-    priority              = drb_cnfg->lc_cnfg.ul_specific_params.priority;
-    prioritized_bit_rate  = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
-    
-    if (prioritized_bit_rate > 0) {
-      rrc_log->warning("PBR>0 currently not supported. Setting it to Inifinty\n");
-      prioritized_bit_rate = -1; 
+
+    // Setup PDCP
+    pdcp->add_bearer(lcid, &drb_cnfg->pdcp_cnfg);
+    // TODO: setup PDCP security (using k_up_enc)
+
+    // Setup RLC
+    rlc->add_bearer(lcid, &drb_cnfg->rlc_cnfg);
+
+    // Setup MAC
+    uint8_t log_chan_group = 0;
+    uint8_t priority = 1;
+    int prioritized_bit_rate = -1;
+    int bucket_size_duration = -1;
+    if (drb_cnfg->lc_cnfg.ul_specific_params_present) {
+      if (drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
+        log_chan_group = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
+      } else {
+        rrc_log->warning("LCG not present, setting to 0\n");
+      }
+      priority = drb_cnfg->lc_cnfg.ul_specific_params.priority;
+      prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
+
+      if (prioritized_bit_rate > 0) {
+        rrc_log->warning("PBR>0 currently not supported. Setting it to Inifinty\n");
+        prioritized_bit_rate = -1;
+      }
+
+      bucket_size_duration = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
     }
-    
-    bucket_size_duration  = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
-  } 
-  mac->setup_lcid(lcid, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
-  
-  drbs[lcid] = *drb_cnfg;
-  drb_up     = true;
-  rrc_log->info("Added radio bearer %s\n", rb_id_text[lcid]);
-}
-
-void rrc::release_drb(uint8_t lcid)
-{
-  // TODO
-}
+    mac->setup_lcid(lcid, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
+
+    drbs[lcid] = *drb_cnfg;
+    drb_up = true;
+    rrc_log->info("Added radio bearer %s\n", rb_id_text[lcid]);
+  }
+
+  void rrc::release_drb(uint8_t lcid) {
+    // TODO
+  }
 
 /************************** 
  *  DEFAULT VALUES Section 9
 ****************************/
 
 // PHY CONFIG DEDICATED Defaults (3GPP 36.331 v10 9.2.4)
-void rrc::set_phy_default_pucch_srs()
-{
-  
-  phy_interface_rrc::phy_cfg_t current_cfg;
-  phy->get_config(&current_cfg);
-  
-  // Set defaults to CQI, SRS and SR
-  current_cfg.dedicated.cqi_report_cnfg_present    = false; 
-  current_cfg.dedicated.srs_ul_cnfg_ded_present    = false; 
-  current_cfg.dedicated.sched_request_cnfg_present = false; 
-
-  apply_phy_config_dedicated(&current_cfg.dedicated, true);
-  
-  // Release SR configuration from MAC 
-  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg; 
-  bzero(&cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-  mac->set_config_sr(&cfg);
-}
-
-void rrc::set_phy_default()
-{
-  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults; 
-  bzero(&defaults, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
-  apply_phy_config_dedicated(&defaults, true);
-}
-
-void rrc::set_mac_default()
-{
-  apply_mac_config_dedicated(NULL, true);
-  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg; 
-  bzero(&sr_cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-  sr_cfg.setup_present = false; 
-  mac->set_config_sr(&sr_cfg);
-}
-
-void rrc::set_rrc_default() {
-  N310 = 1;
-  N311 = 1; 
-  t301 = mac_timers->get_unique_id();
-  t310 = mac_timers->get_unique_id();
-  t311 = mac_timers->get_unique_id();
-  safe_reset_timer = mac_timers->get_unique_id();
-  mac_timers->get(t310)->set(this, 1000);
-  mac_timers->get(t311)->set(this, 1000);
-  mac_timers->get(safe_reset_timer)->set(this, 10);
-}
+  void rrc::set_phy_default_pucch_srs() {
+
+    phy_interface_rrc::phy_cfg_t current_cfg;
+    phy->get_config(&current_cfg);
+
+    // Set defaults to CQI, SRS and SR
+    current_cfg.dedicated.cqi_report_cnfg_present = false;
+    current_cfg.dedicated.srs_ul_cnfg_ded_present = false;
+    current_cfg.dedicated.sched_request_cnfg_present = false;
+
+    apply_phy_config_dedicated(&current_cfg.dedicated, true);
+
+    // Release SR configuration from MAC
+    LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg;
+    bzero(&cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+    mac->set_config_sr(&cfg);
+  }
+
+  void rrc::set_phy_default() {
+    LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults;
+    bzero(&defaults, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
+    apply_phy_config_dedicated(&defaults, true);
+  }
+
+  void rrc::set_mac_default() {
+    apply_mac_config_dedicated(NULL, true);
+    LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg;
+    bzero(&sr_cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+    sr_cfg.setup_present = false;
+    mac->set_config_sr(&sr_cfg);
+  }
+
+  void rrc::set_rrc_default() {
+    N310 = 1;
+    N311 = 1;
+    t301 = mac_timers->get_unique_id();
+    t310 = mac_timers->get_unique_id();
+    t311 = mac_timers->get_unique_id();
+    safe_reset_timer = mac_timers->get_unique_id();
+    mac_timers->get(t310)->set(this, 1000);
+    mac_timers->get(t311)->set(this, 1000);
+    mac_timers->get(safe_reset_timer)->set(this, 10);
+  }
 
 } // namespace srsue

From ce88544d073529b30a542baf3d8928da696814dc Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Fri, 23 Jun 2017 19:36:30 +0100
Subject: [PATCH 017/269] Minor fft library tidy-up

---
 CMakeLists.txt             | 19 ++++++++++++++++---
 lib/src/phy/CMakeLists.txt | 19 +------------------
 2 files changed, 17 insertions(+), 21 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e40737900..269b5a6a3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -74,20 +74,30 @@ set(GCC_ARCH native CACHE STRING "GCC compile for specific architecture.")
 ########################################################################
 # Find dependencies
 ########################################################################
+
+# Threads
 find_package(Threads REQUIRED)
 
+# FFT
 find_package(MKL)
 if(MKL_FOUND)
   include_directories(${MKL_INCLUDE_DIRS})
   link_directories(${MKL_LIBRARY_DIRS})
+  set(FFT_LIBRARIES "${MKL_STATIC_LIBRARIES}") # Static by default
 else(MKL_FOUND)
   find_package(FFTW3F REQUIRED)
   if(FFTW3F_FOUND)
     include_directories(${FFTW3F_INCLUDE_DIRS})
     link_directories(${FFTW3F_LIBRARY_DIRS})
+    if(BUILD_STATIC)
+      set(FFT_LIBRARIES "${FFTW3F_STATIC_LIBRARIES}")
+    else(BUILD_STATIC)
+      set(FFT_LIBRARIES "${FFTW3F_LIBRARIES}")
+    endif(BUILD_STATIC)
   endif(FFTW3F_FOUND)
 endif(MKL_FOUND)
 
+# Crypto
 find_package(Polarssl)
 if (POLARSSL_FOUND)
   set(SEC_INCLUDE_DIRS "${POLARSSL_INCLUDE_DIRS}")
@@ -112,12 +122,14 @@ else(POLARSSL_FOUND)
   endif (MBEDTLS_FOUND)
 endif(POLARSSL_FOUND)
 
+# UHD
 find_package(UHD)
 if(UHD_FOUND)
   include_directories(${UHD_INCLUDE_DIRS})
   link_directories(${UHD_LIBRARY_DIRS})
 endif(UHD_FOUND)
 
+# BladeRF
 if(ENABLE_BLADERF)
   find_package(bladeRF)
   if(BLADERF_FOUND)
@@ -126,6 +138,7 @@ if(ENABLE_BLADERF)
   endif(BLADERF_FOUND)
 endif(ENABLE_BLADERF)
 
+# Soapy
 find_package(SoapySDR)
 if(SOAPYSDR_FOUND)
   include_directories(${SOAPYSDR_INCLUDE_DIRS})
@@ -139,8 +152,8 @@ else(BLADERF_FOUND OR UHD_FOUND OR SOAPYSDR_FOUND)
   add_definitions(-DDISABLE_RF)
 endif(BLADERF_FOUND OR UHD_FOUND OR SOAPYSDR_FOUND)
 
+# Boost
 if(ENABLE_SRSUE OR ENABLE_SRSENB)
-  # Find Boost
   if(BUILD_STATIC)
     set(Boost_USE_STATIC_LIBS ON)
   endif(BUILD_STATIC)
@@ -164,7 +177,7 @@ if(ENABLE_SRSUE OR ENABLE_SRSENB)
   find_package(Boost "1.35" COMPONENTS ${BOOST_REQUIRED_COMPONENTS})
 endif(ENABLE_SRSUE OR ENABLE_SRSENB)
 
-
+# srsGUI
 if(ENABLE_GUI)
   find_package(SRSGUI)
   if(SRSGUI_FOUND)
@@ -174,6 +187,7 @@ if(ENABLE_GUI)
   endif(SRSGUI_FOUND)
 endif(ENABLE_GUI)
 
+# VOLK
 include(CheckFunctionExistsMath)
 if(ENABLE_VOLK)
   find_package(Volk)
@@ -188,7 +202,6 @@ else(ENABLE_VOLK)
   message(STATUS "VOLK library disabled")
 endif(ENABLE_VOLK)
 
-
 ########################################################################
 # Install Dirs
 ########################################################################
diff --git a/lib/src/phy/CMakeLists.txt b/lib/src/phy/CMakeLists.txt
index bd5ae332d..572ff19c3 100644
--- a/lib/src/phy/CMakeLists.txt
+++ b/lib/src/phy/CMakeLists.txt
@@ -55,24 +55,7 @@ set(srslte_srcs     $<TARGET_OBJECTS:srslte_agc>
 )
 
 add_library(srslte_phy STATIC ${srslte_srcs})
-set_target_properties(srslte_phy PROPERTIES
-                                 VERSION ${SRSLTE_VERSION_MAJOR}.${SRSLTE_VERSION_MINOR})
-
-if(MKL_FOUND)
-  if(BUILD_STATIC)
-    target_link_libraries(srslte_phy ${MKL_STATIC_LIBRARIES})
-  else(BUILD_STATIC)
-    target_link_libraries(srslte_phy ${MKL_LIBRARIES})
-  endif(BUILD_STATIC)
-else(MKL_FOUND)
-  if(BUILD_STATIC)
-    target_link_libraries(srslte_phy ${FFTW3F_STATIC_LIBRARIES})
-  else(BUILD_STATIC)
-    target_link_libraries(srslte_phy ${FFTW3F_LIBRARIES})
-  endif(BUILD_STATIC)
-
-endif(MKL_FOUND)
-
+target_link_libraries(srslte_phy ${FFT_LIBRARIES})
 
 if(VOLK_FOUND)
   target_link_libraries(srslte_phy ${VOLK_LIBRARIES})

From e92418d2da43556f136a8149dbf6d9e4af85ab3d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Sat, 24 Jun 2017 16:10:38 +0200
Subject: [PATCH 018/269] minor edit in cmake

---
 lib/examples/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/examples/CMakeLists.txt b/lib/examples/CMakeLists.txt
index 9c6ee193e..652a2c195 100644
--- a/lib/examples/CMakeLists.txt
+++ b/lib/examples/CMakeLists.txt
@@ -65,7 +65,7 @@ if(RF_FOUND)
   add_executable(cell_search cell_search.c)
   target_link_libraries(cell_search srslte_phy srslte_rf)
 
-  add_executable(cell_measurement cell_measurement.c ../include/srslte/common/logger_stdout.h)
+  add_executable(cell_measurement cell_measurement.c)
   target_link_libraries(cell_measurement srslte_phy srslte_rf)
 
   add_executable(usrp_capture usrp_capture.c)

From cec7b95d4517164293e854afbb701449fb259aaa Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Sat, 24 Jun 2017 19:39:33 +0100
Subject: [PATCH 019/269] Fix for race condition, fix for window size checks

---
 lib/src/upper/rlc_am.cc | 47 +++++++++++++++++++++++++++--------------
 1 file changed, 31 insertions(+), 16 deletions(-)

diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index b9545375a..bef8bb024 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -208,12 +208,14 @@ uint32_t rlc_am::get_total_buffer_state()
   }
 
   // Bytes needed for tx SDUs
-  n_sdus  = tx_sdu_queue.size();
-  n_bytes += tx_sdu_queue.size_bytes();
-  if(tx_sdu)
-  {
-    n_sdus++;
-    n_bytes += tx_sdu->N_bytes;
+  if(tx_window.size() < 1024) {
+    n_sdus  = tx_sdu_queue.size();
+    n_bytes += tx_sdu_queue.size_bytes();
+    if(tx_sdu)
+    {
+      n_sdus++;
+      n_bytes += tx_sdu->N_bytes;
+    }
   }
 
   // Room needed for header extensions? (integer rounding)
@@ -256,7 +258,7 @@ uint32_t rlc_am::get_buffer_state()
   }
 
   // Bytes needed for tx SDUs
-  if(tx_window.size() < RLC_AM_WINDOW_SIZE) {
+  if(tx_window.size() < 1024) {
     n_sdus  = tx_sdu_queue.size();
     n_bytes = tx_sdu_queue.size_bytes();
     if(tx_sdu)
@@ -286,6 +288,7 @@ int rlc_am::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   pthread_mutex_lock(&mutex);
 
   log->debug("MAC opportunity - %d bytes\n", nof_bytes);
+  log->debug("tx_window size - %d PDUs\n", tx_window.size());
 
   // Tx STATUS if requested
   if(do_status && !status_prohibited()) {
@@ -298,10 +301,11 @@ int rlc_am::read_pdu(uint8_t *payload, uint32_t nof_bytes)
     return build_retx_pdu(payload, nof_bytes);
   }
 
-  pthread_mutex_unlock(&mutex);
-
   // Build a PDU from SDUs
-  return build_data_pdu(payload, nof_bytes);
+  int ret = build_data_pdu(payload, nof_bytes);
+
+  pthread_mutex_unlock(&mutex);
+  return ret;
 }
 
 void rlc_am::write_pdu(uint8_t *payload, uint32_t nof_bytes)
@@ -582,6 +586,16 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   byte_buffer_t *pdu = pool_allocate;
   if (!pdu) {
     log->console("Fatal Error: Could not allocate PDU in build_data_pdu()\n");
+    log->console("tx_window size: %d PDUs\n", tx_window.size());
+    log->console("vt_a = %d, vt_ms = %d, vt_s = %d, poll_sn = %d "
+                 "vr_r = %d, vr_mr = %d, vr_x = %d, vr_ms = %d, vr_h = %d\n",
+                 vt_a, vt_ms, vt_s, poll_sn,
+                 vr_r, vr_mr, vr_x, vr_ms, vr_h);
+    log->console("retx_queue size: %d PDUs\n", retx_queue.size());
+    std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator txit;
+    for(txit = tx_window.begin(); txit != tx_window.end(); txit++) {
+      log->console("tx_window - SN: %d\n", txit->first);
+    }
     exit(-1);
   }
   rlc_amd_pdu_header_t header;
@@ -943,14 +957,15 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
 
     if(!nack) {
       //ACKed SNs get marked and removed from tx_window if possible
-      it = tx_window.find(i);
-      if(tx_window.end() != it)
-      {
-        tx_window[i].is_acked = true;
+      if(tx_window.count(i) > 0) {
+        it = tx_window.find(i);
+        it->second.is_acked = true;
         if(update_vt_a)
         {
-          pool->deallocate(tx_window[i].buf);
-          tx_window.erase(i);
+          if(it->second.buf) {
+            pool->deallocate(it->second.buf);
+          }
+          tx_window.erase(it);
           vt_a = (vt_a + 1)%MOD;
           vt_ms = (vt_ms + 1)%MOD;
         }

From 89cc50c7c4a03a89372e359a8f40122673dc94b6 Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Sun, 25 Jun 2017 14:35:41 +0100
Subject: [PATCH 020/269] Deallocating buffers for ACKed RLC tx PDUs

---
 lib/include/srslte/common/buffer_pool.h |  3 +++
 lib/src/upper/rlc_am.cc                 | 13 +++++++------
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/lib/include/srslte/common/buffer_pool.h b/lib/include/srslte/common/buffer_pool.h
index eec35b79a..94439387a 100644
--- a/lib/include/srslte/common/buffer_pool.h
+++ b/lib/include/srslte/common/buffer_pool.h
@@ -156,6 +156,9 @@ public:
     return pool->allocate(debug_name);
   }
   void deallocate(byte_buffer_t *b) {
+    if(!b) {
+      return;
+    }
     b->reset();
     pool->deallocate(b);
   }
diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index bef8bb024..416a141c8 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -911,13 +911,13 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
   poll_retx_timeout.reset();
 
   // Handle ACKs and NACKs
+  std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator it;
   bool update_vt_a = true;
-  uint32_t i = vt_a;
+  uint32_t i       = vt_a;
+
   while(TX_MOD_BASE(i) < TX_MOD_BASE(status.ack_sn) &&
         TX_MOD_BASE(i) < TX_MOD_BASE(vt_s))
   {
-    std::map<uint32_t, rlc_amd_tx_pdu_t>::iterator it;
-
     bool nack = false;
     for(uint32_t j=0;j<status.N_nack;j++) {
       if(status.nacks[j].nack_sn == i) {
@@ -960,11 +960,12 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
       if(tx_window.count(i) > 0) {
         it = tx_window.find(i);
         it->second.is_acked = true;
+        if(it->second.buf) {
+          pool->deallocate(it->second.buf);
+          it->second.buf = 0;
+        }
         if(update_vt_a)
         {
-          if(it->second.buf) {
-            pool->deallocate(it->second.buf);
-          }
           tx_window.erase(it);
           vt_a = (vt_a + 1)%MOD;
           vt_ms = (vt_ms + 1)%MOD;

From 2c85d3f8905090c6a1f19156fa99886d5e71c5ef Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 27 Jun 2017 11:42:20 +0200
Subject: [PATCH 021/269] added ctest options for valgrind

---
 CMakeLists.txt       | 2 ++
 CTestCustom.cmake.in | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 269b5a6a3..c4904e5f6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,6 +42,8 @@ include(SRSLTEPackage) #setup cpack
 
 include(CTest)
 set(CTEST_MEMORYCHECK_COMMAND valgrind)
+set(CTEST_MEMORYCHECK_COMMAND_OPTIONS "--trace-children=yes --leak-check=full" )
+
 configure_file(
     "${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in"
     "${CMAKE_CURRENT_BINARY_DIR}/CTestCustom.cmake"
diff --git a/CTestCustom.cmake.in b/CTestCustom.cmake.in
index a66fe53e2..180c08de4 100644
--- a/CTestCustom.cmake.in
+++ b/CTestCustom.cmake.in
@@ -7,7 +7,7 @@ SET(CTEST_CUSTOM_MEMCHECK_IGNORE
   realplot_test
   complexplot_test
 
-# Ignore these to, they take too lonk
+# Ignore these to, they take too long
   fft_normal
   fft_extened
   chest_test_all_cellids

From 6c0587e5a497c97e43b1225a46610e3d3f337214 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 28 Jun 2017 14:59:54 +0200
Subject: [PATCH 022/269] fixed missing initialization of PUSCH scrambling
 sequence

---
 lib/src/phy/phch/pusch.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 65ab08411..5e1cbd922 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -393,7 +393,7 @@ int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
   uint32_t i;
   
   if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_pusch_user_t));
+    q->users[rnti] = calloc(1, sizeof(srslte_pusch_user_t));
     if (q->users[rnti]) {
       for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
         if (srslte_sequence_pusch(&q->users[rnti]->seq[i], rnti, 2 * i, q->cell.id,

From bfa10148f5830341125e8a6ee73916a0e82e5e60 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 30 Jun 2017 16:14:05 +0200
Subject: [PATCH 023/269] fix bug in c-rnti MAC subheader

---
 lib/src/common/pdu.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/common/pdu.cc b/lib/src/common/pdu.cc
index 960f7d85e..c54636fcd 100644
--- a/lib/src/common/pdu.cc
+++ b/lib/src/common/pdu.cc
@@ -515,8 +515,8 @@ bool sch_subh::set_bsr(uint32_t buff_size[4], sch_subh::cetype format)
 bool sch_subh::set_c_rnti(uint16_t crnti)
 {
   if (((sch_pdu*)parent)->has_space_ce(2)) {
-    w_payload_ce[0] = (uint8_t) (crnti&0xff00)>>8;
-    w_payload_ce[1] = (uint8_t) (crnti&0x00ff);
+    w_payload_ce[0] = (uint8_t) ((crnti&0xff00)>>8);
+    w_payload_ce[1] = (uint8_t) ((crnti&0x00ff));
     lcid = CRNTI;
     ((sch_pdu*)parent)->update_space_ce(2);
     nof_bytes = 2; 

From faf120501a14fa96f571924ebfa11d2098aa95c9 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 12:27:42 +0200
Subject: [PATCH 024/269] make DL and UL harq header-only

- move src of DL and UL harq in to header in prep for templating both
- add commong MAC header
---
 srsue/hdr/mac/dl_harq.h    | 308 +++++++++++++++++++++++++----
 srsue/hdr/mac/mac_common.h |  45 +++++
 srsue/hdr/mac/ul_harq.h    | 351 +++++++++++++++++++++++++++++----
 srsue/src/mac/dl_harq.cc   | 337 -------------------------------
 srsue/src/mac/ul_harq.cc   | 394 -------------------------------------
 5 files changed, 631 insertions(+), 804 deletions(-)
 create mode 100644 srsue/hdr/mac/mac_common.h
 delete mode 100644 srsue/src/mac/dl_harq.cc
 delete mode 100644 srsue/src/mac/ul_harq.cc

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index e64e204c6..3b9c25e28 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -27,9 +27,15 @@
 #ifndef DL_HARQ_H
 #define DL_HARQ_H
 
+#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+
 #include "srslte/common/log.h"
 #include "srslte/common/timers.h"
 #include "mac/demux.h"
+#include "mac/mac_common.h"
 #include "mac/dl_sps.h"
 #include "srslte/common/mac_pcap.h"
 
@@ -47,58 +53,294 @@ public:
   const static uint32_t NOF_HARQ_PROC = 8; 
   const static uint32_t HARQ_BCCH_PID = NOF_HARQ_PROC; 
   
-  dl_harq_entity();
-  bool init(srslte::log *log_h_, mac_interface_rrc::mac_cfg_t *mac_cfg, srslte::timers *timers_, demux *demux_unit);
-  
-  
+  dl_harq_entity()
+  {
+    pcap = NULL;
+  }
+    
+  bool init(srslte::log *log_h_, mac_interface_rrc::mac_cfg_t *mac_cfg_, srslte::timers *timers_, demux *demux_unit_)
+  {
+    timers_db  = timers_; 
+    demux_unit = demux_unit_; 
+    mac_cfg    = mac_cfg_; 
+    si_window_start = 0; 
+    log_h = log_h_; 
+    for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
+      if (!proc[i].init(i, this)) {
+        return false; 
+      }
+    }
+    return true;
+  }
+
   /***************** PHY->MAC interface for DL processes **************************/
-  void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action);
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
- 
-  
-  void reset();
-  void start_pcap(srslte::mac_pcap* pcap);
-  int  get_current_tbs(uint32_t harq_pid);
+  void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action)
+  {
+    if (grant.rnti_type != SRSLTE_RNTI_SPS) {
+      uint32_t harq_pid; 
+      // Set BCCH PID for SI RNTI 
+      if (grant.rnti_type == SRSLTE_RNTI_SI) {
+        harq_pid = HARQ_BCCH_PID; 
+      } else {
+        harq_pid = grant.pid%NOF_HARQ_PROC; 
+      }
+      if (grant.rnti_type == SRSLTE_RNTI_TEMP && last_temporal_crnti != grant.rnti) {
+        grant.ndi = true;
+        Info("Set NDI=1 for Temp-RNTI DL grant\n");
+        last_temporal_crnti = grant.rnti;
+      }
+      if (grant.rnti_type == SRSLTE_RNTI_USER && proc[harq_pid].is_sps()) {
+        grant.ndi = true;
+        Info("Set NDI=1 for C-RNTI DL grant\n");
+      }
+      proc[harq_pid].new_grant_dl(grant, action);
+    } else {
+      /* This is for SPS scheduling */
+      uint32_t harq_pid = get_harq_sps_pid(grant.tti)%NOF_HARQ_PROC; 
+      if (grant.ndi) {
+        grant.ndi = false; 
+        proc[harq_pid].new_grant_dl(grant, action);
+      } else {
+        if (grant.is_sps_release) {
+          dl_sps_assig.clear();
+          if (timers_db->get(TIME_ALIGNMENT)->is_running()) {
+            //phy_h->send_sps_ack();
+            Warning("PHY Send SPS ACK not implemented\n");
+          }
+        } else {
+          Error("SPS not implemented\n");
+          //dl_sps_assig.reset(grant.tti, grant);
+          //grant.ndi = true;
+          //procs[harq_pid].save_grant();
+        }
+      }
+    }
+  }
+
+
+  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
+  {
+    if (rnti_type == SRSLTE_RNTI_SI) {
+      proc[NOF_HARQ_PROC].tb_decoded(ack);    
+    } else {
+      proc[harq_pid%NOF_HARQ_PROC].tb_decoded(ack);
+    }
+  }
+
+
+  void reset()
+  {
+    for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
+      proc[i].reset();
+    }
+    dl_sps_assig.clear();
+  }
+
+  void start_pcap(srslte::mac_pcap* pcap_) { pcap = pcap_; }
+
+  int  get_current_tbs(uint32_t harq_pid) { return proc[harq_pid%NOF_HARQ_PROC].get_current_tbs(); }
+
+  void set_si_window_start(int si_window_start_) { si_window_start = si_window_start_; }
+
+  float get_average_retx() { return average_retx; }
 
-  void set_si_window_start(int si_window_start);
-  
-  float get_average_retx(); 
-  
 private:  
-  
-  
   class dl_harq_process {
   public:
-    dl_harq_process();
-    bool init(uint32_t pid, dl_harq_entity *parent);
-    void reset();
-    bool is_sps(); 
-    void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action);
-    void tb_decoded(bool ack);   
-    int get_current_tbs();
+    dl_harq_process()
+    {
+      is_initiated = false; 
+      ack = false; 
+      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+    }
+
+    bool init(uint32_t pid_, dl_harq_entity *parent)
+    {
+      if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
+        Error("Error initiating soft buffer\n");
+        return false; 
+      } else {
+        pid = pid_;
+        is_initiated = true; 
+        harq_entity = parent; 
+        log_h = harq_entity->log_h; 
+        return true;
+      }
+    }
+
+    void reset()
+    {
+      ack = false; 
+      payload_buffer_ptr = NULL; 
+      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+      if (is_initiated) {
+        srslte_softbuffer_rx_reset(&softbuffer);
+      }
+    }
     
-  private: 
-    bool calc_is_new_transmission(mac_interface_phy::mac_grant_t grant); 
+    void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action)
+    {
+      // Compute RV for BCCH when not specified in PDCCH format
+      if (pid == HARQ_BCCH_PID && grant.rv == -1) {
+        uint32_t k; 
+        if ((grant.tti/10)%2 == 0 && grant.tti%10 == 5) { // This is SIB1, k is different
+          k = (grant.tti/20)%4; 
+          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
+        } else if (grant.rv == -1) {      
+          k = (grant.tti-harq_entity->si_window_start)%4; 
+          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
+        }
+      }
+      calc_is_new_transmission(grant);
+      if (is_new_transmission) {
+        ack = false; 
+        srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
+        n_retx = 0; 
+      }
+      
+      // Save grant 
+      grant.last_ndi = cur_grant.ndi; 
+      grant.last_tti = cur_grant.tti; 
+      memcpy(&cur_grant, &grant, sizeof(mac_interface_phy::mac_grant_t)); 
+      
+      // Fill action structure 
+      bzero(action, sizeof(mac_interface_phy::tb_action_dl_t));
+      action->default_ack = ack; 
+      action->generate_ack = true; 
+      action->decode_enabled = false; 
+      
+      // If data has not yet been successfully decoded
+      if (ack == false) {
+        
+        // Instruct the PHY To combine the received data and attempt to decode it
+        payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid, cur_grant.n_bytes);
+        action->payload_ptr = payload_buffer_ptr;
+        if (!action->payload_ptr) {
+          action->decode_enabled = false; 
+          Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes);
+          return;       
+        }    
+        action->decode_enabled = true;     
+        action->rv = cur_grant.rv; 
+        action->rnti = cur_grant.rnti; 
+        action->softbuffer = &softbuffer;     
+        memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
+        n_retx++; 
+        
+      } else {
+        Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+      }
+        
+      if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
+        // Do not generate ACK
+        Debug("Not generating ACK\n");
+        action->generate_ack = false;    
+      } else {
+        if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && ack == false) {
+          // Postpone ACK after contention resolution is resolved
+          action->generate_ack_callback = harq_entity->generate_ack_callback; 
+          action->generate_ack_callback_arg = harq_entity->demux_unit;
+          Debug("ACK pending contention resolution\n");
+        } else {
+          Debug("Generating ACK\n");
+        }
+      }
+    }
     
+    void tb_decoded(bool ack_)
+    {
+      ack = ack_;
+      if (ack == true) {
+        if (pid == HARQ_BCCH_PID) {
+          if (harq_entity->pcap) {
+            harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes, ack, cur_grant.tti);
+          }
+          Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes);
+          harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
+        } else {      
+          if (harq_entity->pcap) {
+            harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes, cur_grant.rnti, ack, cur_grant.tti);            
+          }
+          if (ack) {
+            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n", cur_grant.n_bytes);
+              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes);
+            } else {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes);
+              harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
+
+              // Compute average number of retransmissions per packet 
+              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx, harq_entity->nof_pkts++); 
+            }
+          }
+        }
+      } else {
+        harq_entity->demux_unit->deallocate(payload_buffer_ptr);
+      }
+      
+      Info("DL %d:  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n", 
+           pid, is_new_transmission?"newTX":"reTX ", 
+           cur_grant.n_bytes, cur_grant.rv, ack?"OK":"KO", 
+           cur_grant.ndi, cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
+      
+      if (ack && pid == HARQ_BCCH_PID) {
+        reset();
+      }
+    }
+
+    bool is_sps() { return false; }
+
+    int get_current_tbs() { return cur_grant.n_bytes*8; }
+    
+  private: 
+    bool calc_is_new_transmission(mac_interface_phy::mac_grant_t grant)
+    {
+      bool is_new_tb = true; 
+      if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes == cur_grant.n_bytes)) ||
+          pid == HARQ_BCCH_PID) 
+      {
+        is_new_tb = false; 
+      }
+      
+      if ((grant.ndi != cur_grant.ndi && !is_new_tb) || // NDI toggled for same TB
+          is_new_tb                                  || // is new TB
+          (pid == HARQ_BCCH_PID && grant.rv == 0))      // Broadcast PID and 1st TX (RV=0)
+      {
+        is_new_transmission = true; 
+        Debug("Set HARQ for new transmission\n");
+      } else {
+        is_new_transmission = false; 
+        Debug("Set HARQ for retransmission\n");
+      }
+
+      return is_new_transmission;
+    }
+
     bool            is_initiated; 
     dl_harq_entity *harq_entity; 
     srslte::log    *log_h;
     
-    bool	    is_new_transmission; 
+    bool            is_new_transmission; 
     
     uint32_t        pid;    
     uint8_t        *payload_buffer_ptr; 
     bool            ack;
     
-    uint32_t 	    n_retx; 
+    uint32_t        n_retx; 
     
     mac_interface_phy::mac_grant_t cur_grant;    
     srslte_softbuffer_rx_t         softbuffer; 
-    
   };
-  static bool      generate_ack_callback(void *arg);
+  
+  // Private members of dl_harq_entity
+  
+  static bool      generate_ack_callback(void *arg)
+  {
+    demux *demux_unit = (demux*) arg;
+    return demux_unit->get_uecrid_successful();
+  }
 
-  uint32_t         get_harq_sps_pid(uint32_t tti);
+  uint32_t         get_harq_sps_pid(uint32_t tti) { return 0; }
   
   dl_sps           dl_sps_assig;
   
@@ -109,9 +351,9 @@ private:
   srslte::log     *log_h;
   srslte::mac_pcap *pcap; 
   uint16_t         last_temporal_crnti;
-  int 	           si_window_start;
+  int              si_window_start;
 
-  float 	   average_retx;   
+  float            average_retx;   
   uint64_t         nof_pkts; 
 };
 
diff --git a/srsue/hdr/mac/mac_common.h b/srsue/hdr/mac/mac_common.h
new file mode 100644
index 000000000..d40d179eb
--- /dev/null
+++ b/srsue/hdr/mac/mac_common.h
@@ -0,0 +1,45 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef MAC_COMMON_H
+#define MAC_COMMON_H
+
+namespace srsue {
+
+typedef enum {
+  HARQ_RTT, 
+  TIME_ALIGNMENT,
+  CONTENTION_TIMER,
+  BSR_TIMER_PERIODIC,
+  BSR_TIMER_RETX,
+  PHR_TIMER_PERIODIC,
+  PHR_TIMER_PROHIBIT,
+  NOF_MAC_TIMERS
+} mac_timers_t; 
+  
+} // namespace srsue
+
+#endif // MAC_COMMON_H
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index cea469e33..38046b099 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -27,24 +27,31 @@
 #ifndef ULHARQ_H
 #define ULHARQ_H
 
+#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/log.h"
 #include "mac/mux.h"
+#include "mac/mac_common.h"
 #include "mac/ul_sps.h"
 #include "srslte/common/mac_pcap.h"
 #include "srslte/common/timers.h"
+#include "srslte/common/interfaces_common.h"
 
 /* Uplink HARQ entity as defined in 5.4.2 of 36.321 */
-
-
 namespace srsue {
   
 class ul_harq_entity
 {
 public:
-
   const static uint32_t NOF_HARQ_PROC = 8; 
-  static uint32_t pidof(uint32_t tti);
+  static uint32_t pidof(uint32_t tti)
+  {
+    return (uint32_t) tti%NOF_HARQ_PROC;
+  }
   
   ul_harq_entity() {  
     pcap        = NULL; 
@@ -56,46 +63,228 @@ public:
     average_retx = 0; 
     nof_pkts     = 0; 
   }
-  bool init(srslte::log *log_h, 
-            mac_interface_rrc::ue_rnti_t *rntis, 
-            mac_interface_rrc::mac_cfg_t *mac_cfg, 
-            srslte::timers* timers_, 
-            mux *mux_unit);
-  void reset();
-  void reset_ndi();
 
-  void start_pcap(srslte::mac_pcap* pcap);
+  bool init(srslte::log *log_h_, 
+            mac_interface_rrc::ue_rnti_t *rntis_, 
+            mac_interface_rrc::mac_cfg_t *mac_cfg_, 
+            srslte::timers* timers_db_, 
+            mux *mux_unit_)
+  {
+    log_h     = log_h_;
+    mux_unit  = mux_unit_;
+    mac_cfg   = mac_cfg_;
+    rntis     = rntis_;
+    timers_db = timers_db_;
+    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+      if (!proc[i].init(i, this)) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  void reset()
+  {
+    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+      proc[i].reset();
+    }
+    ul_sps_assig.clear();
+  }
+
+  void reset_ndi()
+  {
+    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+      proc[i].reset_ndi();
+    }
+  }
+
+  void start_pcap(srslte::mac_pcap* pcap_)
+  {
+    pcap = pcap_;
+  }
 
   
   /***************** PHY->MAC interface for UL processes **************************/
-  void new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_ul_t *action);
-  void new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t *action);
-  void harq_recv(uint32_t tti, bool ack, mac_interface_phy::tb_action_ul_t *action);
+  void new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_ul_t *action)
+  {
+    if (grant.rnti_type == SRSLTE_RNTI_USER ||
+      grant.rnti_type == SRSLTE_RNTI_TEMP ||
+      grant.rnti_type == SRSLTE_RNTI_RAR)
+    {
+      if (grant.rnti_type == SRSLTE_RNTI_USER && proc[pidof(grant.tti)].is_sps()) {
+        grant.ndi = true;
+      }
+      run_tti(grant.tti, &grant, action);
+    } else if (grant.rnti_type == SRSLTE_RNTI_SPS) {
+      if (grant.ndi) {
+        grant.ndi = proc[pidof(grant.tti)].get_ndi();
+        run_tti(grant.tti, &grant, action);
+      } else {
+        Info("Not implemented\n");
+      }
+    }
+  }
+
+  void new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t *action)
+  {
+    set_ack(grant.tti, ack);
+    new_grant_ul(grant, action);
+  }
 
-  int get_current_tbs(uint32_t tti);
+  void harq_recv(uint32_t tti, bool ack, mac_interface_phy::tb_action_ul_t *action)
+  {
+    set_ack(tti, ack);
+    run_tti(tti, NULL, action);
+  }
+
+  int get_current_tbs(uint32_t tti)
+  {
+    int tti_harq = (int) tti-4;
+    if (tti_harq < 0) {
+      tti_harq += 10240;
+    }
+    uint32_t pid_harq = pidof(tti_harq);
+    return proc[pid_harq].get_current_tbs();
+  }
   
-  float get_average_retx(); 
+  float get_average_retx()
+  {
+    return average_retx;
+  }
     
-private:  
-
+private:
   class ul_harq_process {
   public:
-    ul_harq_process();
-    bool init(uint32_t pid, ul_harq_entity *parent);
-    void reset();
-    void reset_ndi();
+    ul_harq_process()
+    {
+      current_tx_nb = 0;
+      current_irv = 0;
+      is_initiated = false;
+      is_grant_configured = false;
+      tti_last_tx = 0;
+      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+    }
+
+    bool init(uint32_t pid_, ul_harq_entity *parent)
+    {
+      if (srslte_softbuffer_tx_init(&softbuffer, 110)) {
+        fprintf(stderr, "Error initiating soft buffer\n");
+        return false;
+      } else {
+        is_initiated = true;
+        harq_entity = parent;
+        log_h = harq_entity->log_h;
+        pid = pid_;
+        payload_buffer = (uint8_t*) srslte_vec_malloc(payload_buffer_len*sizeof(uint8_t));
+        if (!payload_buffer) {
+          Error("Allocating memory\n");
+          return false;
+        }
+        pdu_ptr = payload_buffer;
+        return true;
+      }
+    }
+
+    void reset()
+    {
+      current_tx_nb = 0;
+      current_irv = 0;
+      tti_last_tx = 0;
+      is_grant_configured = false;
+      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+    }
+
+    void reset_ndi() { ndi = false; }
     
-    void run_tti(uint32_t tti, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action);
+    void run_tti(uint32_t tti_tx, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action)
+    {
+      uint32_t max_retx;
+      if (is_msg3) {
+        max_retx = harq_entity->mac_cfg->rach.max_harq_msg3_tx;
+      } else {
+        max_retx = liblte_rrc_max_harq_tx_num[harq_entity->mac_cfg->main.ulsch_cnfg.max_harq_tx];
+      }
+
+      // Receive and route HARQ feedbacks
+      if (grant) {
+        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi != get_ndi()) ||
+          (grant->rnti_type == SRSLTE_RNTI_USER && !has_grant())             ||
+          grant->is_from_rar)
+        {
+          // New transmission
 
-    uint32_t get_rv();
-    bool has_grant();
+          // Uplink grant in a RAR
+          if (grant->is_from_rar) {
+            Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes);
+            pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes);
+            if (pdu_ptr) {
+              generate_new_tx(tti_tx, true, grant, action);
+            } else {
+              Warning("UL RAR grant available but no Msg3 on buffer\n");
+            }
 
-    void set_harq_feedback(bool ack);
-    bool get_ndi();
-    bool is_sps();
-    uint32_t last_tx_tti();
-    uint32_t get_nof_retx();
-    int get_current_tbs();
+            // Normal UL grant
+          } else {
+            // Request a MAC PDU from the Multiplexing & Assemble Unit
+            pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes, tti_tx, pid);
+            if (pdu_ptr) {
+              generate_new_tx(tti_tx, false, grant, action);
+            } else {
+              Warning("Uplink grant but no MAC PDU in Multiplex Unit buffer\n");
+            }
+          }
+        } else {
+          // Adaptive Re-TX
+          if (current_tx_nb >= max_retx) {
+            Info("UL %d:  Maximum number of ReTX reached (%d). Discarting TB.\n", pid, max_retx);
+            reset();
+            action->expect_ack = false;
+          } else {
+            generate_retx(tti_tx, grant, action);
+          }
+        }
+      } else if (has_grant()) {
+        // Non-Adaptive Re-Tx
+        if (current_tx_nb >= max_retx) {
+          Info("UL %d:  Maximum number of ReTX reached (%d). Discarting TB.\n", pid, max_retx);
+          reset();
+          action->expect_ack = false;
+        } else {
+          generate_retx(tti_tx, action);
+        }
+      }
+      if (harq_entity->pcap && grant) {
+        if (grant->is_from_rar) {
+          grant->rnti = harq_entity->rntis->temp_rnti;
+        }
+        harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes, grant->rnti, get_nof_retx(), tti_tx);
+      }
+    }
+
+    void set_harq_feedback(bool ack)
+    {
+      harq_feedback = ack;
+      // UL packet successfully delivered
+      if (ack) {
+        Info("UL %d:  HARQ = ACK for UL transmission. Discarting TB.\n", pid);
+        reset();
+      } else {
+        Info("UL %d:  HARQ = NACK for UL transmission\n", pid);
+      }
+    }
+
+    uint32_t get_rv()
+    {
+      int rv_of_irv[4] = {0, 2, 3, 1};
+      return rv_of_irv[current_irv%4];
+    }
+
+    bool has_grant() { return is_grant_configured; }
+    bool get_ndi() { return ndi; }
+    bool is_sps() { return false; }
+    uint32_t last_tx_tti() { return tti_last_tx; }
+    uint32_t get_nof_retx() { return current_tx_nb; }
+    int get_current_tbs() { return cur_grant.n_bytes*8; }
    
   private: 
     mac_interface_phy::mac_grant_t cur_grant;
@@ -118,17 +307,99 @@ private:
     uint8_t *payload_buffer;
     uint8_t *pdu_ptr; 
     
-    void generate_retx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action); 
+    void generate_retx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
+    {
+      generate_retx(tti_tx, NULL, action);
+    }
+
+    // Retransmission with or w/o grant (Section 5.4.2.2)
     void generate_retx(uint32_t tti_tx, mac_interface_phy::mac_grant_t *grant, 
-                                        mac_interface_phy::tb_action_ul_t *action); 
-    void generate_new_tx(uint32_t tti_tx, bool is_msg3, mac_interface_phy::mac_grant_t *grant, 
-                         mac_interface_phy::tb_action_ul_t *action);
-    void generate_tx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action);
+                                        mac_interface_phy::tb_action_ul_t *action)
+    {
+      int irv_of_rv[4] = {0, 3, 1, 2};
+      if (grant) {
+        // HARQ entity requests an adaptive transmission
+        if (grant->rv) {
+          current_irv = irv_of_rv[grant->rv%4];
+        }
+        memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
+        harq_feedback = false;
+        Info("UL %d:  Adaptive retx=%d, RV=%d, TBS=%d\n",
+             pid, current_tx_nb, get_rv(), grant->n_bytes);
+        generate_tx(tti_tx, action);
+      } else {
+        Info("UL %d:  Non-Adaptive retx=%d, RV=%d, TBS=%d\n",
+             pid, current_tx_nb, get_rv(), cur_grant.n_bytes);
+        // HARQ entity requests a non-adaptive transmission
+        if (!harq_feedback) {
+          generate_tx(tti_tx, action);
+        }
+      }
+
+      // On every Msg3 retransmission, restart mac-ContentionResolutionTimer as defined in Section 5.1.5
+      if (is_msg3) {
+        harq_entity->timers_db->get(CONTENTION_TIMER)->reset();
+      }
+
+      harq_entity->mux_unit->pusch_retx(tti_tx, pid);
+    }
+
+    // New transmission (Section 5.4.2.2)
+    void generate_new_tx(uint32_t tti_tx, bool is_msg3_, mac_interface_phy::mac_grant_t *grant,
+                         mac_interface_phy::tb_action_ul_t *action)
+    {
+      if (grant) {
+        // Compute average number of retransmissions per packet considering previous packet
+        harq_entity->average_retx = SRSLTE_VEC_CMA((float) current_tx_nb, harq_entity->average_retx, harq_entity->nof_pkts++);
+        memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
+        harq_feedback = false;
+        is_grant_configured = true;
+        current_tx_nb = 0;
+        current_irv = 0;
+        is_msg3 = is_msg3_;
+        Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
+             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes, cur_grant.rnti);
+        generate_tx(tti_tx, action);
+      }
+    }
+
+    // Transmission of pending frame (Section 5.4.2.2)
+    void generate_tx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
+    {
+      action->current_tx_nb = current_tx_nb;
+      current_tx_nb++;
+      action->expect_ack = true;
+      action->rnti = is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti;
+      action->rv = cur_grant.rv>0?cur_grant.rv:get_rv();
+      action->softbuffer = &softbuffer;
+      action->tx_enabled = true;
+      action->payload_ptr = pdu_ptr;
+      memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
+
+      current_irv = (current_irv+1)%4;
+      tti_last_tx = tti_tx;
+    }
   };
 
-  
-  void run_tti(uint32_t tti, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action);
-  void set_ack(uint32_t tti, bool ack);
+  // Implements Section 5.4.2.1
+  // Called with UL grant
+  void run_tti(uint32_t tti, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action)
+  {
+    uint32_t tti_tx = (tti+4)%10240;
+    proc[pidof(tti_tx)].run_tti(tti_tx, grant, action);
+  }
+
+  void set_ack(uint32_t tti, bool ack)
+  {
+    int tti_harq = (int) tti - 4;
+    if (tti_harq < 0) {
+      tti_harq += 10240;
+    }
+    uint32_t pid_harq = pidof(tti_harq);
+    if (proc[pid_harq].has_grant() && (proc[pid_harq].last_tx_tti() <= (uint32_t)tti_harq)) {
+      proc[pid_harq].set_harq_feedback(ack);
+    }
+  }
   
   ul_sps           ul_sps_assig;
 
diff --git a/srsue/src/mac/dl_harq.cc b/srsue/src/mac/dl_harq.cc
deleted file mode 100644
index 685224786..000000000
--- a/srsue/src/mac/dl_harq.cc
+++ /dev/null
@@ -1,337 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-
-#include "mac/mac.h"
-#include "mac/dl_harq.h"
-
-
-namespace srsue {
-    
-
-  /***********************************************************
-  * 
-  * HARQ ENTITY
-  * 
-  *********************************************************/
-        
-dl_harq_entity::dl_harq_entity()
-{
-  pcap = NULL; 
-}
-
-bool dl_harq_entity::init(srslte::log* log_h_, mac_interface_rrc::mac_cfg_t *mac_cfg_, srslte::timers* timers_, demux *demux_unit_)
-{
-  timers_db  = timers_; 
-  demux_unit = demux_unit_; 
-  mac_cfg    = mac_cfg_; 
-  si_window_start = 0; 
-  log_h = log_h_; 
-  for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
-    if (!proc[i].init(i, this)) {
-      return false; 
-    }
-  }
-  return true; 
-
-}
-
-void dl_harq_entity::start_pcap(srslte::mac_pcap* pcap_)
-{
-  pcap = pcap_; 
-}
-
-void dl_harq_entity::reset()
-{
-  for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
-    proc[i].reset();
-  }
-  dl_sps_assig.clear();
-}
-
-uint32_t dl_harq_entity::get_harq_sps_pid(uint32_t tti) {
-  return 0;
-}
-
-void dl_harq_entity::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t* action)
-{
-  
-  if (grant.rnti_type != SRSLTE_RNTI_SPS) {
-    uint32_t harq_pid; 
-    // Set BCCH PID for SI RNTI 
-    if (grant.rnti_type == SRSLTE_RNTI_SI) {
-      harq_pid = HARQ_BCCH_PID; 
-    } else {
-      harq_pid = grant.pid%NOF_HARQ_PROC; 
-    }
-    if (grant.rnti_type == SRSLTE_RNTI_TEMP && last_temporal_crnti != grant.rnti) {
-      grant.ndi = true;
-      Info("Set NDI=1 for Temp-RNTI DL grant\n");
-      last_temporal_crnti = grant.rnti;
-    }
-    if (grant.rnti_type == SRSLTE_RNTI_USER && proc[harq_pid].is_sps()) {
-      grant.ndi = true;
-      Info("Set NDI=1 for C-RNTI DL grant\n");
-    }
-    proc[harq_pid].new_grant_dl(grant, action);
-  } else {
-    /* This is for SPS scheduling */
-    uint32_t harq_pid = get_harq_sps_pid(grant.tti)%NOF_HARQ_PROC; 
-    if (grant.ndi) {
-      grant.ndi = false; 
-      proc[harq_pid].new_grant_dl(grant, action);
-    } else {
-      if (grant.is_sps_release) {
-        dl_sps_assig.clear();
-        if (timers_db->get(mac::TIME_ALIGNMENT)->is_running()) {
-          //phy_h->send_sps_ack();
-          Warning("PHY Send SPS ACK not implemented\n");
-        }
-      } else {
-        Error("SPS not implemented\n");
-        //dl_sps_assig.reset(grant.tti, grant);
-        //grant.ndi = true;
-        //procs[harq_pid].save_grant();
-      }
-    }
-  }
-}
-
-void dl_harq_entity::tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
-{
-  if (rnti_type == SRSLTE_RNTI_SI) {
-    proc[NOF_HARQ_PROC].tb_decoded(ack);    
-  } else {
-    proc[harq_pid%NOF_HARQ_PROC].tb_decoded(ack);
-  }
-}
-
-int dl_harq_entity::get_current_tbs(uint32_t harq_pid)
-{
-  return proc[harq_pid%NOF_HARQ_PROC].get_current_tbs();
-}
-
-
-bool dl_harq_entity::generate_ack_callback(void *arg)
-{
-  demux *demux_unit = (demux*) arg;
-  return demux_unit->get_uecrid_successful();
-}
-
-void dl_harq_entity::set_si_window_start(int si_window_start_)
-{
-  si_window_start = si_window_start_;
-}
-
-float dl_harq_entity::get_average_retx()
-{
-  return average_retx; 
-}
-
-  /***********************************************************
-  * 
-  * HARQ PROCESS
-  * 
-  *********************************************************/
-          
-dl_harq_entity::dl_harq_process::dl_harq_process() {
-  is_initiated = false; 
-  ack = false; 
-  bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
-}  
-  
-void dl_harq_entity::dl_harq_process::reset() {
-  ack = false; 
-  payload_buffer_ptr = NULL; 
-  bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
-  if (is_initiated) {
-    srslte_softbuffer_rx_reset(&softbuffer);
-  }
-}
-
-bool dl_harq_entity::dl_harq_process::init(uint32_t pid_, dl_harq_entity *parent) {
-  if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
-    Error("Error initiating soft buffer\n");
-    return false; 
-  } else {
-    pid = pid_;
-    is_initiated = true; 
-    harq_entity = parent; 
-    log_h = harq_entity->log_h; 
-    return true;
-  }     
-}
-
-bool dl_harq_entity::dl_harq_process::is_sps()
-{
-  return false; 
-}                                                            
-
-bool dl_harq_entity::dl_harq_process::calc_is_new_transmission(mac_interface_phy::mac_grant_t grant) {
-  
-  bool is_new_tb = true; 
-  if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes == cur_grant.n_bytes)) ||
-      pid == HARQ_BCCH_PID) 
-  {
-    is_new_tb = false; 
-  }
-  
-  if ((grant.ndi != cur_grant.ndi && !is_new_tb) || // NDI toggled for same TB
-      is_new_tb                                  || // is new TB
-      (pid == HARQ_BCCH_PID && grant.rv == 0))      // Broadcast PID and 1st TX (RV=0)
-  {
-    is_new_transmission = true; 
-    Debug("Set HARQ for new transmission\n");
-  } else {
-    is_new_transmission = false; 
-    Debug("Set HARQ for retransmission\n");
-  }
-
-  return is_new_transmission;
-}
-
-void dl_harq_entity::dl_harq_process::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t* action)
-{
-  // Compute RV for BCCH when not specified in PDCCH format
-  if (pid == HARQ_BCCH_PID && grant.rv == -1) {
-    uint32_t k; 
-    if ((grant.tti/10)%2 == 0 && grant.tti%10 == 5) { // This is SIB1, k is different
-      k = (grant.tti/20)%4; 
-      grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-    } else if (grant.rv == -1) {      
-      k = (grant.tti-harq_entity->si_window_start)%4; 
-      grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-    }
-  }
-  calc_is_new_transmission(grant);
-  if (is_new_transmission) {
-    ack = false; 
-    srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
-    n_retx = 0; 
-  }
-  
-  // Save grant 
-  grant.last_ndi = cur_grant.ndi; 
-  grant.last_tti = cur_grant.tti; 
-  memcpy(&cur_grant, &grant, sizeof(mac_interface_phy::mac_grant_t)); 
-  
-  // Fill action structure 
-  bzero(action, sizeof(mac_interface_phy::tb_action_dl_t));
-  action->default_ack = ack; 
-  action->generate_ack = true; 
-  action->decode_enabled = false; 
-  
-  // If data has not yet been successfully decoded
-  if (ack == false) {
-    
-    // Instruct the PHY To combine the received data and attempt to decode it
-    payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid, cur_grant.n_bytes);
-    action->payload_ptr = payload_buffer_ptr;
-    if (!action->payload_ptr) {
-      action->decode_enabled = false; 
-      Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes);
-      return;       
-    }    
-    action->decode_enabled = true;     
-    action->rv = cur_grant.rv; 
-    action->rnti = cur_grant.rnti; 
-    action->softbuffer = &softbuffer;     
-    memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
-    n_retx++; 
-    
-  } else {
-    Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
-  }
-    
-  if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(mac::TIME_ALIGNMENT)->is_expired()) {
-    // Do not generate ACK
-    Debug("Not generating ACK\n");
-    action->generate_ack = false;    
-  } else {
-    if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && ack == false) {
-      // Postpone ACK after contention resolution is resolved
-      action->generate_ack_callback = harq_entity->generate_ack_callback; 
-      action->generate_ack_callback_arg = harq_entity->demux_unit;
-      Debug("ACK pending contention resolution\n");
-    } else {
-      Debug("Generating ACK\n");
-    }
-  }
-}
-
-int dl_harq_entity::dl_harq_process::get_current_tbs()
-{
-  return cur_grant.n_bytes*8;
-}
-
-void dl_harq_entity::dl_harq_process::tb_decoded(bool ack_)
-{
-  ack = ack_;
-  if (ack == true) {
-    if (pid == HARQ_BCCH_PID) {
-      if (harq_entity->pcap) {
-        harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes, ack, cur_grant.tti);
-      }
-      Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes);
-      harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-    } else {      
-      if (harq_entity->pcap) {
-        harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes, cur_grant.rnti, ack, cur_grant.tti);            
-      }
-      if (ack) {
-        if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-          Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n", cur_grant.n_bytes);
-          harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes);
-        } else {
-          Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes);
-          harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-
-          // Compute average number of retransmissions per packet 
-          harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx, harq_entity->nof_pkts++); 
-        }
-      }
-    }
-  } else {
-    harq_entity->demux_unit->deallocate(payload_buffer_ptr);
-  }
-  
-  Info("DL %d:  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n", 
-       pid, is_new_transmission?"newTX":"reTX ", 
-       cur_grant.n_bytes, cur_grant.rv, ack?"OK":"KO", 
-       cur_grant.ndi, cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
-  
-  if (ack && pid == HARQ_BCCH_PID) {
-    reset();
-  }
-}
-
-
-    
-}
diff --git a/srsue/src/mac/ul_harq.cc b/srsue/src/mac/ul_harq.cc
deleted file mode 100644
index da28d62b3..000000000
--- a/srsue/src/mac/ul_harq.cc
+++ /dev/null
@@ -1,394 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-#define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-
-#include "srslte/common/log.h"
-#include "mac/mac.h"
-#include "mac/ul_harq.h"
-
-
-  namespace srsue {
-
-  /***********************************************************
-  * 
-  * HARQ ENTITY
-  * 
-  *********************************************************/
-    
-bool ul_harq_entity::init(srslte::log *log_h_, 
-                          mac_interface_rrc::ue_rnti_t *rntis_, 
-                          mac_interface_rrc::mac_cfg_t *mac_cfg_, 
-                          srslte::timers *timers_db_, 
-                          mux *mux_unit_) {
-  log_h     = log_h_; 
-  mux_unit  = mux_unit_; 
-  mac_cfg   = mac_cfg_; 
-  rntis     = rntis_; 
-  timers_db = timers_db_;
-  for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
-    if (!proc[i].init(i, this)) {
-      return false; 
-    }
-  }
-  return true; 
-}
-uint32_t ul_harq_entity::pidof(uint32_t tti) {
-  return (uint32_t) tti%NOF_HARQ_PROC;  
-}
-
-void ul_harq_entity::start_pcap(srslte::mac_pcap* pcap_)
-{
-  pcap = pcap_; 
-}
-
-void ul_harq_entity::reset() {
-  for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
-    proc[i].reset();
-  }
-  ul_sps_assig.clear();
-}
-void ul_harq_entity::reset_ndi() {
-  for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
-    proc[i].reset_ndi();
-  }
-}
-
-int ul_harq_entity::get_current_tbs(uint32_t tti) {
-  int tti_harq = (int) tti-4;
-  if (tti_harq < 0) {
-    tti_harq += 10240; 
-  }
-  uint32_t pid_harq = pidof(tti_harq); 
-  return proc[pid_harq].get_current_tbs();
-}
-
-void ul_harq_entity::set_ack(uint32_t tti, bool ack) {
-  int tti_harq = (int) tti - 4; 
-  if (tti_harq < 0) {
-    tti_harq += 10240; 
-  }
-  uint32_t pid_harq = pidof(tti_harq); 
-  if (proc[pid_harq].has_grant() && (proc[pid_harq].last_tx_tti() <= (uint32_t)tti_harq)) {
-    proc[pid_harq].set_harq_feedback(ack);
-  }
-}
-
-void ul_harq_entity::harq_recv(uint32_t tti, bool ack, mac_interface_phy::tb_action_ul_t* action)
-{
-  set_ack(tti, ack);
-  run_tti(tti, NULL, action);
-}
-
-// Implements Section 5.4.1 
-void ul_harq_entity::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_ul_t* action)
-{
-  if (grant.rnti_type == SRSLTE_RNTI_USER || 
-      grant.rnti_type == SRSLTE_RNTI_TEMP ||
-      grant.rnti_type == SRSLTE_RNTI_RAR)
-  {
-    if (grant.rnti_type == SRSLTE_RNTI_USER && proc[pidof(grant.tti)].is_sps()) {
-      grant.ndi = true; 
-    }
-    run_tti(grant.tti, &grant, action);
-  } else if (grant.rnti_type == SRSLTE_RNTI_SPS) {
-    if (grant.ndi) {
-      grant.ndi = proc[pidof(grant.tti)].get_ndi();
-      run_tti(grant.tti, &grant, action);
-    } else {
-      Info("Not implemented\n");
-    }
-  }
-}
-
-void ul_harq_entity::new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t* action)
-{
-  set_ack(grant.tti, ack);
-  new_grant_ul(grant, action);
-}
-
-
-
-// Implements Section 5.4.2.1
-// Called with UL grant
-void ul_harq_entity::run_tti(uint32_t tti, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action)
-{
-  uint32_t tti_tx = (tti+4)%10240;
-  proc[pidof(tti_tx)].run_tti(tti_tx, grant, action);
-}
-
-
-float ul_harq_entity::get_average_retx()
-{
-  return average_retx; 
-}
-
-
-/***********************************************************
-  * 
-  * HARQ PROCESS
-  * 
-  *********************************************************/
-
-static int rv_of_irv[4] = {0, 2, 3, 1}; 
-static int irv_of_rv[4] = {0, 3, 1, 2}; 
-    
-ul_harq_entity::ul_harq_process::ul_harq_process() {
-  current_tx_nb = 0; 
-  current_irv = 0; 
-  is_initiated = false; 
-  is_grant_configured = false; 
-  tti_last_tx = 0; 
-  bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
-}
-
-void ul_harq_entity::ul_harq_process::reset() {
-  current_tx_nb = 0; 
-  current_irv = 0; 
-  tti_last_tx = 0; 
-  is_grant_configured = false; 
-  bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
-}
-
-bool ul_harq_entity::ul_harq_process::has_grant() {
-  return is_grant_configured; 
-}
-
-void ul_harq_entity::ul_harq_process::reset_ndi() {
-  ndi = false; 
-}
-
-bool ul_harq_entity::ul_harq_process::get_ndi()
-{
-  return ndi; 
-}
-
-uint32_t ul_harq_entity::ul_harq_process::get_rv()
-{
-  return rv_of_irv[current_irv%4];
-}
-
-void ul_harq_entity::ul_harq_process::set_harq_feedback(bool ack) {
-  harq_feedback = ack; 
-  // UL packet successfully delivered
-  if (ack) {
-    Info("UL %d:  HARQ = ACK for UL transmission. Discarting TB.\n", pid);
-    reset();
-  } else {
-    Info("UL %d:  HARQ = NACK for UL transmission\n", pid);
-  }
-}
-
-bool ul_harq_entity::ul_harq_process::init(uint32_t pid_, ul_harq_entity* parent) {
-  if (srslte_softbuffer_tx_init(&softbuffer, 110)) {
-    fprintf(stderr, "Error initiating soft buffer\n");
-    return false; 
-  } else {
-    is_initiated = true; 
-    harq_entity = parent; 
-    log_h = harq_entity->log_h;
-    pid = pid_;
-    payload_buffer = (uint8_t*) srslte_vec_malloc(payload_buffer_len*sizeof(uint8_t));
-    if (!payload_buffer) {
-      Error("Allocating memory\n");
-      return false; 
-    }
-    pdu_ptr = payload_buffer;
-    return true; 
-  }     
-}
-
-void ul_harq_entity::ul_harq_process::run_tti(uint32_t tti_tx, mac_interface_phy::mac_grant_t* grant, mac_interface_phy::tb_action_ul_t* action)
-{   
-  
-  
-  uint32_t max_retx;
-  if (is_msg3) {
-    max_retx = harq_entity->mac_cfg->rach.max_harq_msg3_tx;
-  } else {
-    max_retx = liblte_rrc_max_harq_tx_num[harq_entity->mac_cfg->main.ulsch_cnfg.max_harq_tx];
-  }
-
-  
-  // Receive and route HARQ feedbacks
-  if (grant) {
-    if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi != get_ndi()) ||
-        (grant->rnti_type == SRSLTE_RNTI_USER && !has_grant())             ||
-         grant->is_from_rar) 
-    {          
-      // New transmission
-
-      // Uplink grant in a RAR
-      if (grant->is_from_rar) {
-        Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes);
-        pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes);
-        if (pdu_ptr) {
-          generate_new_tx(tti_tx, true, grant, action);
-        } else {
-          Warning("UL RAR grant available but no Msg3 on buffer\n");
-        }
-              
-      // Normal UL grant
-      } else {
-        // Request a MAC PDU from the Multiplexing & Assemble Unit
-        pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes, tti_tx, pid);
-        if (pdu_ptr) {            
-          generate_new_tx(tti_tx, false, grant, action);          
-        } else {
-          Warning("Uplink grant but no MAC PDU in Multiplex Unit buffer\n");
-        }
-      }
-    } else {
-      // Adaptive Re-TX 
-      if (current_tx_nb >= max_retx) {
-        Info("UL %d:  Maximum number of ReTX reached (%d). Discarting TB.\n", pid, max_retx);
-        reset();          
-        action->expect_ack = false;
-      } else {
-        generate_retx(tti_tx, grant, action);
-      }
-    }        
-  } else if (has_grant()) {
-    // Non-Adaptive Re-Tx
-    if (current_tx_nb >= max_retx) {
-      Info("UL %d:  Maximum number of ReTX reached (%d). Discarting TB.\n", pid, max_retx);
-      reset();          
-      action->expect_ack = false;
-    } else {
-      generate_retx(tti_tx, action);
-    }
-  }
-  if (harq_entity->pcap && grant) {
-    if (grant->is_from_rar) {
-      grant->rnti = harq_entity->rntis->temp_rnti;
-    }
-    harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes, grant->rnti, get_nof_retx(), tti_tx);
-  }
-
-  
-
-}
-
-int ul_harq_entity::ul_harq_process::get_current_tbs()
-{
-  return cur_grant.n_bytes*8;
-}
-
-void ul_harq_entity::ul_harq_process::generate_retx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
-{
-  generate_retx(tti_tx, NULL, action);
-}
-
-// Retransmission with or w/o grant (Section 5.4.2.2)
-void ul_harq_entity::ul_harq_process::generate_retx(uint32_t tti_tx, mac_interface_phy::mac_grant_t *grant, 
-                                                    mac_interface_phy::tb_action_ul_t *action)
-{
-  if (grant) {
-    // HARQ entity requests an adaptive transmission
-    if (grant->rv) {
-      current_irv = irv_of_rv[grant->rv%4];
-    }
-    memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
-    harq_feedback = false; 
-    Info("UL %d:  Adaptive retx=%d, RV=%d, TBS=%d\n", 
-         pid, current_tx_nb, get_rv(), grant->n_bytes);
-    generate_tx(tti_tx, action);
-  } else {
-    Info("UL %d:  Non-Adaptive retx=%d, RV=%d, TBS=%d\n", 
-         pid, current_tx_nb, get_rv(), cur_grant.n_bytes);
-    // HARQ entity requests a non-adaptive transmission
-    if (!harq_feedback) {
-      generate_tx(tti_tx, action);
-    }
-  }
-  
-  // On every Msg3 retransmission, restart mac-ContentionResolutionTimer as defined in Section 5.1.5
-  if (is_msg3) {
-    harq_entity->timers_db->get(mac::CONTENTION_TIMER)->reset();
-  }
-  
-  harq_entity->mux_unit->pusch_retx(tti_tx, pid);
-}
-
-// New transmission (Section 5.4.2.2)
-void ul_harq_entity::ul_harq_process::generate_new_tx(uint32_t tti_tx, bool is_msg3_, 
-                                                      mac_interface_phy::mac_grant_t *grant, 
-                                                      mac_interface_phy::tb_action_ul_t *action)
-{
-  if (grant) {
-    
-  // Compute average number of retransmissions per packet considering previous packet
-  harq_entity->average_retx = SRSLTE_VEC_CMA((float) current_tx_nb, harq_entity->average_retx, harq_entity->nof_pkts++); 
-
-    
-    memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
-    harq_feedback = false; 
-    is_grant_configured = true; 
-    current_tx_nb = 0; 
-    current_irv = 0;         
-    is_msg3 = is_msg3_;
-    Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n", 
-         pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes, cur_grant.rnti);
-    generate_tx(tti_tx, action);
-  }
-}
-
-// Transmission of pending frame (Section 5.4.2.2)
-void ul_harq_entity::ul_harq_process::generate_tx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
-{
-  action->current_tx_nb = current_tx_nb;
-  current_tx_nb++;    
-  action->expect_ack = true;
-  action->rnti = is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti; 
-  action->rv = cur_grant.rv>0?cur_grant.rv:get_rv();
-  action->softbuffer = &softbuffer; 
-  action->tx_enabled = true; 
-  action->payload_ptr = pdu_ptr; 
-  memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
-  
-  current_irv = (current_irv+1)%4;  
-  tti_last_tx = tti_tx; 
-}
-
-bool ul_harq_entity::ul_harq_process::is_sps()
-{
-  return false; 
-}
-
-uint32_t ul_harq_entity::ul_harq_process::last_tx_tti()
-{
-  return tti_last_tx;
-}
-
-uint32_t ul_harq_entity::ul_harq_process::get_nof_retx()
-{
-  return current_tx_nb;
-}
-
-}

From 869786a27c8a532d2f18974076d00f2f7471b70b Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 12:55:54 +0200
Subject: [PATCH 025/269] adapt MAC procedures to also use MAC timers defined
 in mac_common.h

---
 srsue/hdr/mac/mac.h       | 11 ----------
 srsue/src/mac/demux.cc    |  4 ++--
 srsue/src/mac/mac.cc      |  2 +-
 srsue/src/mac/proc_bsr.cc | 42 +++++++++++++++++++--------------------
 srsue/src/mac/proc_phr.cc | 26 ++++++++++++------------
 srsue/src/mac/proc_ra.cc  | 20 +++++++++----------
 6 files changed, 47 insertions(+), 58 deletions(-)

diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index f6f22f334..82a7b1e76 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -98,17 +98,6 @@ public:
   u_int32_t                get_unique_id();
   
   uint32_t get_current_tti();
-      
-  enum {
-    HARQ_RTT, 
-    TIME_ALIGNMENT,
-    CONTENTION_TIMER,
-    BSR_TIMER_PERIODIC,
-    BSR_TIMER_RETX,
-    PHR_TIMER_PERIODIC,
-    PHR_TIMER_PROHIBIT,
-    NOF_MAC_TIMERS
-  } mac_timers_t; 
   
   static const int MAC_NOF_UPPER_TIMERS = 20; 
   
diff --git a/srsue/src/mac/demux.cc b/srsue/src/mac/demux.cc
index c7c73a14b..71ad0cc58 100644
--- a/srsue/src/mac/demux.cc
+++ b/srsue/src/mac/demux.cc
@@ -190,8 +190,8 @@ bool demux::process_ce(srslte::sch_subh *subh) {
       Info("Received TA=%d\n", subh->get_ta_cmd());
       
       // Start or restart timeAlignmentTimer
-      timers_db->get(mac::TIME_ALIGNMENT)->reset();
-      timers_db->get(mac::TIME_ALIGNMENT)->run();      
+      timers_db->get(TIME_ALIGNMENT)->reset();
+      timers_db->get(TIME_ALIGNMENT)->run();
       break;
     case srslte::sch_subh::PADDING:
       break;
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index f583dd2df..9a461ca05 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -308,7 +308,7 @@ void mac::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
   /* Start PHR Periodic timer on first UL grant */
   if (is_first_ul_grant) {
     is_first_ul_grant = false; 
-    timers_db.get(mac::PHR_TIMER_PERIODIC)->run();
+    timers_db.get(PHR_TIMER_PERIODIC)->run();
   }
   if (grant.rnti_type == SRSLTE_RNTI_USER && ra_procedure.is_contention_resolution()) {
     ra_procedure.pdcch_to_crnti(true);    
diff --git a/srsue/src/mac/proc_bsr.cc b/srsue/src/mac/proc_bsr.cc
index d93b7f0f4..3117eeffc 100644
--- a/srsue/src/mac/proc_bsr.cc
+++ b/srsue/src/mac/proc_bsr.cc
@@ -57,10 +57,10 @@ void bsr_proc::init(rlc_interface_mac *rlc_, srslte::log* log_h_, mac_interface_
 
 void bsr_proc::reset()
 {
-  timers_db->get(mac::BSR_TIMER_PERIODIC)->stop();
-  timers_db->get(mac::BSR_TIMER_PERIODIC)->reset();
-  timers_db->get(mac::BSR_TIMER_RETX)->stop();
-  timers_db->get(mac::BSR_TIMER_RETX)->reset();
+  timers_db->get(BSR_TIMER_PERIODIC)->stop();
+  timers_db->get(BSR_TIMER_PERIODIC)->reset();
+  timers_db->get(BSR_TIMER_RETX)->stop();
+  timers_db->get(BSR_TIMER_RETX)->reset();
   
   reset_sr = false; 
   sr_is_sent = false; 
@@ -78,14 +78,14 @@ void bsr_proc::reset()
 /* Process Periodic BSR */
 void bsr_proc::timer_expired(uint32_t timer_id) {
   switch(timer_id) {
-    case mac::BSR_TIMER_PERIODIC:
+    case BSR_TIMER_PERIODIC:
       if (triggered_bsr_type == NONE) {
         // Check condition 4 in Sec 5.4.5 
         triggered_bsr_type = PERIODIC; 
         Debug("BSR:   Triggering Periodic BSR\n");
       }
       break;
-    case mac::BSR_TIMER_RETX:
+    case BSR_TIMER_RETX:
       // Enable reTx of SR only if periodic timer is not infinity
       int periodic = liblte_rrc_periodic_bsr_timer_num[mac_cfg->main.ulsch_cnfg.periodic_bsr_timer];
       if (periodic >= 0) {
@@ -222,17 +222,17 @@ void bsr_proc::step(uint32_t tti)
   }  
   
   int periodic = liblte_rrc_periodic_bsr_timer_num[mac_cfg->main.ulsch_cnfg.periodic_bsr_timer];
-  if (periodic > 0 && (uint32_t)periodic != timers_db->get(mac::BSR_TIMER_PERIODIC)->get_timeout())
+  if (periodic > 0 && (uint32_t)periodic != timers_db->get(BSR_TIMER_PERIODIC)->get_timeout())
   {
-    timers_db->get(mac::BSR_TIMER_PERIODIC)->set(this, periodic);
-    timers_db->get(mac::BSR_TIMER_PERIODIC)->run();
+    timers_db->get(BSR_TIMER_PERIODIC)->set(this, periodic);
+    timers_db->get(BSR_TIMER_PERIODIC)->run();
     Info("BSR:   Configured timer periodic %d ms\n", periodic);    
   }      
   int retx = liblte_rrc_retransmission_bsr_timer_num[mac_cfg->main.ulsch_cnfg.retx_bsr_timer];
-  if (retx > 0 && (uint32_t)retx != timers_db->get(mac::BSR_TIMER_RETX)->get_timeout())
+  if (retx > 0 && (uint32_t)retx != timers_db->get(BSR_TIMER_RETX)->get_timeout())
   {
-    timers_db->get(mac::BSR_TIMER_RETX)->set(this, retx);
-    timers_db->get(mac::BSR_TIMER_RETX)->run();
+    timers_db->get(BSR_TIMER_RETX)->set(this, retx);
+    timers_db->get(BSR_TIMER_RETX)->run();
     Info("BSR:   Configured timer reTX %d ms\n", retx);
   }
 
@@ -309,18 +309,18 @@ bool bsr_proc::need_to_send_bsr_on_ul_grant(uint32_t grant_size, bsr_t *bsr)
           grant_size, total_data, bsr_sz);
       ret = true; 
     }    
-    if (timers_db->get(mac::BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
-      timers_db->get(mac::BSR_TIMER_PERIODIC)->reset();
-      timers_db->get(mac::BSR_TIMER_PERIODIC)->run();
+    if (timers_db->get(BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
+      timers_db->get(BSR_TIMER_PERIODIC)->reset();
+      timers_db->get(BSR_TIMER_PERIODIC)->run();
     }
   }
   // Cancel all triggered BSR and SR     
   triggered_bsr_type = NONE; 
   reset_sr = true;     
   // Restart or Start ReTX timer
-  if (timers_db->get(mac::BSR_TIMER_RETX)->get_timeout()) {
-    timers_db->get(mac::BSR_TIMER_RETX)->reset();
-    timers_db->get(mac::BSR_TIMER_RETX)->run();
+  if (timers_db->get(BSR_TIMER_RETX)->get_timeout()) {
+    timers_db->get(BSR_TIMER_RETX)->reset();
+    timers_db->get(BSR_TIMER_RETX)->run();
   }
   return ret;   
 }
@@ -340,9 +340,9 @@ bool bsr_proc::generate_padding_bsr(uint32_t nof_padding_bytes, bsr_t *bsr)
          bsr_type_tostring(triggered_bsr_type), bsr_format_tostring(bsr->format), 
          bsr->buff_size[0], bsr->buff_size[1], bsr->buff_size[2], bsr->buff_size[3]);
     
-    if (timers_db->get(mac::BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
-      timers_db->get(mac::BSR_TIMER_PERIODIC)->reset();
-      timers_db->get(mac::BSR_TIMER_PERIODIC)->run();
+    if (timers_db->get(BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
+      timers_db->get(BSR_TIMER_PERIODIC)->reset();
+      timers_db->get(BSR_TIMER_PERIODIC)->run();
     }    
     
   }
diff --git a/srsue/src/mac/proc_phr.cc b/srsue/src/mac/proc_phr.cc
index b432f1c06..11828f306 100644
--- a/srsue/src/mac/proc_phr.cc
+++ b/srsue/src/mac/proc_phr.cc
@@ -76,13 +76,13 @@ bool phr_proc::pathloss_changed() {
 /* Trigger PHR when timers exire */
 void phr_proc::timer_expired(uint32_t timer_id) {
   switch(timer_id) {
-    case mac::PHR_TIMER_PERIODIC:
-      timers_db->get(mac::PHR_TIMER_PERIODIC)->reset();    
-      timers_db->get(mac::PHR_TIMER_PERIODIC)->run();    
+    case PHR_TIMER_PERIODIC:
+      timers_db->get(PHR_TIMER_PERIODIC)->reset();
+      timers_db->get(PHR_TIMER_PERIODIC)->run();
       Debug("PHR:   Triggered by timer periodic (timer expired).\n");
       phr_is_triggered = true; 
       break;
-    case mac::PHR_TIMER_PROHIBIT:
+    case PHR_TIMER_PROHIBIT:
       int pathloss_db = liblte_rrc_dl_pathloss_change_num[mac_cfg->main.phr_cnfg.dl_pathloss_change];
       if (pathloss_changed()) {
         Info("PHR:   Triggered by pathloss difference. cur_pathloss_db=%f (timer expired)\n", last_pathloss_db);
@@ -105,8 +105,8 @@ void phr_proc::step(uint32_t tti)
     if (timer_periodic != cfg_timer_periodic && cfg_timer_periodic > 0) 
     {
       timer_periodic = cfg_timer_periodic; 
-      timers_db->get(mac::PHR_TIMER_PERIODIC)->set(this, timer_periodic);
-      timers_db->get(mac::PHR_TIMER_PERIODIC)->run();
+      timers_db->get(PHR_TIMER_PERIODIC)->set(this, timer_periodic);
+      timers_db->get(PHR_TIMER_PERIODIC)->run();
       phr_is_triggered = true; 
       Info("PHR:   Configured timer periodic %d ms\n", timer_periodic);
     }
@@ -118,12 +118,12 @@ void phr_proc::step(uint32_t tti)
   if (timer_prohibit != cfg_timer_prohibit && cfg_timer_prohibit > 0) 
   {
     timer_prohibit = cfg_timer_prohibit; 
-    timers_db->get(mac::PHR_TIMER_PROHIBIT)->set(this, timer_prohibit);
-    timers_db->get(mac::PHR_TIMER_PROHIBIT)->run();
+    timers_db->get(PHR_TIMER_PROHIBIT)->set(this, timer_prohibit);
+    timers_db->get(PHR_TIMER_PROHIBIT)->run();
     Info("PHR:   Configured timer prohibit %d ms\n", timer_prohibit);
     phr_is_triggered = true; 
   }  
-  if (pathloss_changed() && timers_db->get(mac::PHR_TIMER_PROHIBIT)->is_expired()) 
+  if (pathloss_changed() && timers_db->get(PHR_TIMER_PROHIBIT)->is_expired())
   {
     Info("PHR:   Triggered by pathloss difference. cur_pathloss_db=%f\n", last_pathloss_db);
     phr_is_triggered = true;        
@@ -140,10 +140,10 @@ bool phr_proc::generate_phr_on_ul_grant(float *phr)
     
     Debug("PHR:   Generating PHR=%f\n", phr?*phr:0.0);
     
-    timers_db->get(mac::PHR_TIMER_PERIODIC)->reset();
-    timers_db->get(mac::PHR_TIMER_PROHIBIT)->reset();
-    timers_db->get(mac::PHR_TIMER_PERIODIC)->run();
-    timers_db->get(mac::PHR_TIMER_PROHIBIT)->run();
+    timers_db->get(PHR_TIMER_PERIODIC)->reset();
+    timers_db->get(PHR_TIMER_PROHIBIT)->reset();
+    timers_db->get(PHR_TIMER_PERIODIC)->run();
+    timers_db->get(PHR_TIMER_PROHIBIT)->run();
     
     phr_is_triggered = false; 
     
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 495c9945f..73b51d61c 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -115,7 +115,7 @@ void ra_proc::read_params() {
   delta_preamble_db         = delta_preamble_db_table[configIndex%5]; 
   
   if (contentionResolutionTimer > 0) {
-    timers_db->get(mac::CONTENTION_TIMER)->set(this, contentionResolutionTimer);
+    timers_db->get(CONTENTION_TIMER)->set(this, contentionResolutionTimer);
   }
 
 }
@@ -165,14 +165,14 @@ void ra_proc::process_timeadv_cmd(uint32_t ta) {
   if (preambleIndex == 0) {
     // Preamble not selected by UE MAC 
     phy_h->set_timeadv_rar(ta);
-    timers_db->get(mac::TIME_ALIGNMENT)->reset();
-    timers_db->get(mac::TIME_ALIGNMENT)->run();
+    timers_db->get(TIME_ALIGNMENT)->reset();
+    timers_db->get(TIME_ALIGNMENT)->run();
     Debug("Applying RAR TA CMD %d\n", ta);
   } else {
     // Preamble selected by UE MAC 
-    if (!timers_db->get(mac::TIME_ALIGNMENT)->is_running()) {
+    if (!timers_db->get(TIME_ALIGNMENT)->is_running()) {
       phy_h->set_timeadv_rar(ta);
-      timers_db->get(mac::TIME_ALIGNMENT)->run();
+      timers_db->get(TIME_ALIGNMENT)->run();
       Debug("Applying RAR TA CMD %d\n", ta);
     } else {
       // Ignore TA CMD
@@ -355,8 +355,8 @@ void ra_proc::tb_decoded_ok() {
         state = CONTENTION_RESOLUTION;
         
         // Start contention resolution timer 
-        timers_db->get(mac::CONTENTION_TIMER)->reset();
-        timers_db->get(mac::CONTENTION_TIMER)->run();                      
+        timers_db->get(CONTENTION_TIMER)->reset();
+        timers_db->get(CONTENTION_TIMER)->run();
       }  
     } else {
       rDebug("Found RAR for preamble %d\n", rar_pdu_msg.get()->get_rapid());
@@ -417,7 +417,7 @@ bool ra_proc::contention_resolution_id_received(uint64_t rx_contention_id) {
   rDebug("MAC PDU Contains Contention Resolution ID CE\n");
   
   // MAC PDU successfully decoded and contains MAC CE contention Id
-  timers_db->get(mac::CONTENTION_TIMER)->stop();
+  timers_db->get(CONTENTION_TIMER)->stop();
   
   if (transmitted_contention_id == rx_contention_id) 
   {    
@@ -453,7 +453,7 @@ void ra_proc::step_contention_resolution() {
             (started_by_pdcch && pdcch_to_crnti_received != PDCCH_CRNTI_NOT_RECEIVED))
       {
         rDebug("PDCCH for C-RNTI received\n");
-        timers_db->get(mac::CONTENTION_TIMER)->stop();
+        timers_db->get(CONTENTION_TIMER)->stop();
         rntis->temp_rnti = 0; 
         state = COMPLETION;           
       }            
@@ -559,7 +559,7 @@ void ra_proc::pdcch_to_crnti(bool contains_uplink_grant) {
 
 void ra_proc::harq_retx()
 {
-  timers_db->get(mac::CONTENTION_TIMER)->reset();
+  timers_db->get(CONTENTION_TIMER)->reset();
 }
 
 }

From 704b8c3dc46d8661e9af4bb51c66d268ec26a6d0 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 13:19:18 +0200
Subject: [PATCH 026/269] make DL and UL harq template classes

---
 srsue/hdr/mac/dl_harq.h | 43 +++++++++++++++++-----------------
 srsue/hdr/mac/mac.h     |  7 +++---
 srsue/hdr/mac/ul_harq.h | 52 ++++++++++++++++++++---------------------
 3 files changed, 52 insertions(+), 50 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 3b9c25e28..25677dd84 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -45,15 +45,15 @@
 
 
 namespace srsue {
-  
+
+template <std::size_t N, typename Tgrant, typename Taction, typename Tphygrant>
 class dl_harq_entity
 {
 public:
 
-  const static uint32_t NOF_HARQ_PROC = 8; 
-  const static uint32_t HARQ_BCCH_PID = NOF_HARQ_PROC; 
+  const static uint32_t HARQ_BCCH_PID = N;
   
-  dl_harq_entity()
+  dl_harq_entity() : proc(N+1)
   {
     pcap = NULL;
   }
@@ -65,7 +65,7 @@ public:
     mac_cfg    = mac_cfg_; 
     si_window_start = 0; 
     log_h = log_h_; 
-    for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
+    for (uint32_t i=0;i<N+1;i++) {
       if (!proc[i].init(i, this)) {
         return false; 
       }
@@ -74,7 +74,7 @@ public:
   }
 
   /***************** PHY->MAC interface for DL processes **************************/
-  void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action)
+  void new_grant_dl(Tgrant grant, Taction *action)
   {
     if (grant.rnti_type != SRSLTE_RNTI_SPS) {
       uint32_t harq_pid; 
@@ -82,7 +82,7 @@ public:
       if (grant.rnti_type == SRSLTE_RNTI_SI) {
         harq_pid = HARQ_BCCH_PID; 
       } else {
-        harq_pid = grant.pid%NOF_HARQ_PROC; 
+        harq_pid = grant.pid%N;
       }
       if (grant.rnti_type == SRSLTE_RNTI_TEMP && last_temporal_crnti != grant.rnti) {
         grant.ndi = true;
@@ -96,7 +96,7 @@ public:
       proc[harq_pid].new_grant_dl(grant, action);
     } else {
       /* This is for SPS scheduling */
-      uint32_t harq_pid = get_harq_sps_pid(grant.tti)%NOF_HARQ_PROC; 
+      uint32_t harq_pid = get_harq_sps_pid(grant.tti)%N;
       if (grant.ndi) {
         grant.ndi = false; 
         proc[harq_pid].new_grant_dl(grant, action);
@@ -121,16 +121,16 @@ public:
   void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
   {
     if (rnti_type == SRSLTE_RNTI_SI) {
-      proc[NOF_HARQ_PROC].tb_decoded(ack);    
+      proc[N].tb_decoded(ack);
     } else {
-      proc[harq_pid%NOF_HARQ_PROC].tb_decoded(ack);
+      proc[harq_pid%N].tb_decoded(ack);
     }
   }
 
 
   void reset()
   {
-    for (uint32_t i=0;i<NOF_HARQ_PROC+1;i++) {
+    for (uint32_t i=0;i<N+1;i++) {
       proc[i].reset();
     }
     dl_sps_assig.clear();
@@ -138,7 +138,7 @@ public:
 
   void start_pcap(srslte::mac_pcap* pcap_) { pcap = pcap_; }
 
-  int  get_current_tbs(uint32_t harq_pid) { return proc[harq_pid%NOF_HARQ_PROC].get_current_tbs(); }
+  int  get_current_tbs(uint32_t harq_pid) { return proc[harq_pid%N].get_current_tbs(); }
 
   void set_si_window_start(int si_window_start_) { si_window_start = si_window_start_; }
 
@@ -151,7 +151,7 @@ private:
     {
       is_initiated = false; 
       ack = false; 
-      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+      bzero(&cur_grant, sizeof(Tgrant));
     }
 
     bool init(uint32_t pid_, dl_harq_entity *parent)
@@ -172,13 +172,13 @@ private:
     {
       ack = false; 
       payload_buffer_ptr = NULL; 
-      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+      bzero(&cur_grant, sizeof(Tgrant));
       if (is_initiated) {
         srslte_softbuffer_rx_reset(&softbuffer);
       }
     }
     
-    void new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t *action)
+    void new_grant_dl(Tgrant grant, Taction *action)
     {
       // Compute RV for BCCH when not specified in PDCCH format
       if (pid == HARQ_BCCH_PID && grant.rv == -1) {
@@ -201,10 +201,10 @@ private:
       // Save grant 
       grant.last_ndi = cur_grant.ndi; 
       grant.last_tti = cur_grant.tti; 
-      memcpy(&cur_grant, &grant, sizeof(mac_interface_phy::mac_grant_t)); 
+      memcpy(&cur_grant, &grant, sizeof(Tgrant));
       
       // Fill action structure 
-      bzero(action, sizeof(mac_interface_phy::tb_action_dl_t));
+      bzero(action, sizeof(Taction));
       action->default_ack = ack; 
       action->generate_ack = true; 
       action->decode_enabled = false; 
@@ -224,7 +224,7 @@ private:
         action->rv = cur_grant.rv; 
         action->rnti = cur_grant.rnti; 
         action->softbuffer = &softbuffer;     
-        memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
+        memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
         n_retx++; 
         
       } else {
@@ -293,7 +293,7 @@ private:
     int get_current_tbs() { return cur_grant.n_bytes*8; }
     
   private: 
-    bool calc_is_new_transmission(mac_interface_phy::mac_grant_t grant)
+    bool calc_is_new_transmission(Tgrant grant)
     {
       bool is_new_tb = true; 
       if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes == cur_grant.n_bytes)) ||
@@ -328,7 +328,7 @@ private:
     
     uint32_t        n_retx; 
     
-    mac_interface_phy::mac_grant_t cur_grant;    
+    Tgrant                         cur_grant;
     srslte_softbuffer_rx_t         softbuffer; 
   };
   
@@ -344,7 +344,8 @@ private:
   
   dl_sps           dl_sps_assig;
   
-  dl_harq_process  proc[NOF_HARQ_PROC+1];
+
+  std::vector<dl_harq_process> proc;
   srslte::timers   *timers_db;
   mac_interface_rrc::mac_cfg_t *mac_cfg; 
   demux           *demux_unit; 
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index 82a7b1e76..660f7fbba 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -106,6 +106,7 @@ private:
   
   static const int MAC_MAIN_THREAD_PRIO = 5; 
   static const int MAC_PDU_THREAD_PRIO  = 6;
+  static const int MAC_NOF_HARQ_PROC    = 8;
 
   // Interaction with PHY 
   srslte::tti_sync_cv   ttisync; 
@@ -130,9 +131,9 @@ private:
   mux           mux_unit; 
   demux         demux_unit; 
   
-  /* DL/UL HARQ */  
-  dl_harq_entity dl_harq; 
-  ul_harq_entity ul_harq; 
+  /* DL/UL HARQ */
+  dl_harq_entity<MAC_NOF_HARQ_PROC, mac_grant_t, tb_action_dl_t, srslte_phy_grant_t> dl_harq;
+  ul_harq_entity<MAC_NOF_HARQ_PROC, mac_grant_t, tb_action_ul_t, srslte_phy_grant_t> ul_harq;
   
   /* MAC Uplink-related Procedures */
   ra_proc       ra_procedure;
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 38046b099..21c4a9602 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -43,17 +43,18 @@
 
 /* Uplink HARQ entity as defined in 5.4.2 of 36.321 */
 namespace srsue {
-  
+
+template <std::size_t N, typename Tgrant, typename Taction, typename Tphygrant>
 class ul_harq_entity
 {
 public:
-  const static uint32_t NOF_HARQ_PROC = 8; 
   static uint32_t pidof(uint32_t tti)
   {
-    return (uint32_t) tti%NOF_HARQ_PROC;
+    return (uint32_t) tti%N;
   }
   
-  ul_harq_entity() {  
+  ul_harq_entity() : proc(N)
+  {
     pcap        = NULL; 
     timers_db   = NULL; 
     mux_unit    = NULL; 
@@ -61,7 +62,7 @@ public:
     mac_cfg     = NULL; 
     rntis       = NULL; 
     average_retx = 0; 
-    nof_pkts     = 0; 
+    nof_pkts     = 0;
   }
 
   bool init(srslte::log *log_h_, 
@@ -75,7 +76,7 @@ public:
     mac_cfg   = mac_cfg_;
     rntis     = rntis_;
     timers_db = timers_db_;
-    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+    for (uint32_t i=0;i<N;i++) {
       if (!proc[i].init(i, this)) {
         return false;
       }
@@ -85,7 +86,7 @@ public:
 
   void reset()
   {
-    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+    for (uint32_t i=0;i<N;i++) {
       proc[i].reset();
     }
     ul_sps_assig.clear();
@@ -93,7 +94,7 @@ public:
 
   void reset_ndi()
   {
-    for (uint32_t i=0;i<NOF_HARQ_PROC;i++) {
+    for (uint32_t i=0;i<N;i++) {
       proc[i].reset_ndi();
     }
   }
@@ -105,7 +106,7 @@ public:
 
   
   /***************** PHY->MAC interface for UL processes **************************/
-  void new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_ul_t *action)
+  void new_grant_ul(Tgrant grant, Taction *action)
   {
     if (grant.rnti_type == SRSLTE_RNTI_USER ||
       grant.rnti_type == SRSLTE_RNTI_TEMP ||
@@ -125,13 +126,13 @@ public:
     }
   }
 
-  void new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t *action)
+  void new_grant_ul_ack(Tgrant grant, bool ack, Taction *action)
   {
     set_ack(grant.tti, ack);
     new_grant_ul(grant, action);
   }
 
-  void harq_recv(uint32_t tti, bool ack, mac_interface_phy::tb_action_ul_t *action)
+  void harq_recv(uint32_t tti, bool ack, Taction *action)
   {
     set_ack(tti, ack);
     run_tti(tti, NULL, action);
@@ -162,7 +163,7 @@ private:
       is_initiated = false;
       is_grant_configured = false;
       tti_last_tx = 0;
-      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+      bzero(&cur_grant, sizeof(Tgrant));
     }
 
     bool init(uint32_t pid_, ul_harq_entity *parent)
@@ -191,12 +192,12 @@ private:
       current_irv = 0;
       tti_last_tx = 0;
       is_grant_configured = false;
-      bzero(&cur_grant, sizeof(mac_interface_phy::mac_grant_t));
+      bzero(&cur_grant, sizeof(Tgrant));
     }
 
     void reset_ndi() { ndi = false; }
     
-    void run_tti(uint32_t tti_tx, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action)
+    void run_tti(uint32_t tti_tx, Tgrant *grant, Taction* action)
     {
       uint32_t max_retx;
       if (is_msg3) {
@@ -287,7 +288,7 @@ private:
     int get_current_tbs() { return cur_grant.n_bytes*8; }
    
   private: 
-    mac_interface_phy::mac_grant_t cur_grant;
+    Tgrant                      cur_grant;
     
     uint32_t                    pid;
     uint32_t                    current_tx_nb;
@@ -307,14 +308,14 @@ private:
     uint8_t *payload_buffer;
     uint8_t *pdu_ptr; 
     
-    void generate_retx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
+    void generate_retx(uint32_t tti_tx, Taction *action)
     {
       generate_retx(tti_tx, NULL, action);
     }
 
     // Retransmission with or w/o grant (Section 5.4.2.2)
-    void generate_retx(uint32_t tti_tx, mac_interface_phy::mac_grant_t *grant, 
-                                        mac_interface_phy::tb_action_ul_t *action)
+    void generate_retx(uint32_t tti_tx, Tgrant *grant,
+                                        Taction *action)
     {
       int irv_of_rv[4] = {0, 3, 1, 2};
       if (grant) {
@@ -322,7 +323,7 @@ private:
         if (grant->rv) {
           current_irv = irv_of_rv[grant->rv%4];
         }
-        memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
+        memcpy(&cur_grant, grant, sizeof(Tgrant));
         harq_feedback = false;
         Info("UL %d:  Adaptive retx=%d, RV=%d, TBS=%d\n",
              pid, current_tx_nb, get_rv(), grant->n_bytes);
@@ -345,13 +346,12 @@ private:
     }
 
     // New transmission (Section 5.4.2.2)
-    void generate_new_tx(uint32_t tti_tx, bool is_msg3_, mac_interface_phy::mac_grant_t *grant,
-                         mac_interface_phy::tb_action_ul_t *action)
+    void generate_new_tx(uint32_t tti_tx, bool is_msg3_, Tgrant *grant, Taction *action)
     {
       if (grant) {
         // Compute average number of retransmissions per packet considering previous packet
         harq_entity->average_retx = SRSLTE_VEC_CMA((float) current_tx_nb, harq_entity->average_retx, harq_entity->nof_pkts++);
-        memcpy(&cur_grant, grant, sizeof(mac_interface_phy::mac_grant_t));
+        memcpy(&cur_grant, grant, sizeof(Tgrant));
         harq_feedback = false;
         is_grant_configured = true;
         current_tx_nb = 0;
@@ -364,7 +364,7 @@ private:
     }
 
     // Transmission of pending frame (Section 5.4.2.2)
-    void generate_tx(uint32_t tti_tx, mac_interface_phy::tb_action_ul_t *action)
+    void generate_tx(uint32_t tti_tx, Taction *action)
     {
       action->current_tx_nb = current_tx_nb;
       current_tx_nb++;
@@ -374,7 +374,7 @@ private:
       action->softbuffer = &softbuffer;
       action->tx_enabled = true;
       action->payload_ptr = pdu_ptr;
-      memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(srslte_phy_grant_t));
+      memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
 
       current_irv = (current_irv+1)%4;
       tti_last_tx = tti_tx;
@@ -383,7 +383,7 @@ private:
 
   // Implements Section 5.4.2.1
   // Called with UL grant
-  void run_tti(uint32_t tti, mac_interface_phy::mac_grant_t *grant, mac_interface_phy::tb_action_ul_t* action)
+  void run_tti(uint32_t tti, Tgrant *grant, Taction* action)
   {
     uint32_t tti_tx = (tti+4)%10240;
     proc[pidof(tti_tx)].run_tti(tti_tx, grant, action);
@@ -405,7 +405,7 @@ private:
 
   srslte::timers  *timers_db;
   mux             *mux_unit;
-  ul_harq_process proc[NOF_HARQ_PROC];
+  std::vector<ul_harq_process> proc;
   srslte::log     *log_h;
   srslte::mac_pcap *pcap; 
 

From 583aa2d180ded3ca5369774a75631b01efbb045e Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 14:19:53 +0200
Subject: [PATCH 027/269] move MAC structs shared between multiple RATs out RAT
 specific interface

---
 lib/include/srslte/interfaces/ue_interfaces.h | 31 ++++++++++++-------
 1 file changed, 20 insertions(+), 11 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 62b2a7ef7..a05f117a4 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -309,9 +309,27 @@ public:
   
 };
 
+/* Interface RRC -> MAC shared between different RATs */
+class mac_interface_rrc_common
+{
+public:
+  // Class to handle UE specific RNTIs between RRC and MAC
+  typedef struct {
+    uint16_t crnti;
+    uint16_t temp_rnti;
+    uint16_t tpc_rnti;
+    uint16_t sps_rnti;
+    uint64_t contention_id;
+  } ue_rnti_t;
+
+  typedef struct {
+    uint32_t max_harq_msg3_tx;
+    uint32_t max_harq_tx;
+  } ul_harq_params_t;
+};
 
 /* Interface RRC -> MAC */
-class mac_interface_rrc
+class mac_interface_rrc : public mac_interface_rrc_common
 {
 public:
   
@@ -319,19 +337,10 @@ public:
     LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT           main; 
     LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT        rach;     
     LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr; 
+    ul_harq_params_t                            ul_harq_params;
     uint32_t prach_config_index; 
   } mac_cfg_t; 
 
-  
-  // Class to handle UE specific RNTIs between RRC and MAC
-  typedef struct {
-    uint16_t crnti; 
-    uint16_t temp_rnti; 
-    uint16_t tpc_rnti; 
-    uint16_t sps_rnti; 
-    uint64_t contention_id; 
-  } ue_rnti_t;  
-  
   /* Instructs the MAC to start receiving BCCH */
   virtual void    bcch_start_rx() = 0; 
   virtual void    bcch_stop_rx() = 0; 

From 9f3b4bf9a516f43bfdd65d2caeec6dd8adf98810 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 14:30:51 +0200
Subject: [PATCH 028/269] use dedicated UL harq config

- this patch changes the UL harq config to a dedicated one
  instead of passing the whole MAC config which may be specific to
  a specific RAT
- it also extends the RRC to fill the harq specific config field in the
  MAC config
- clean up dl_harq and remove unused mac config
---
 srsue/hdr/mac/dl_harq.h |  4 +---
 srsue/hdr/mac/ul_harq.h | 16 ++++++++--------
 srsue/src/mac/mac.cc    | 12 ++++++------
 srsue/src/upper/rrc.cc  | 11 +++++++++--
 4 files changed, 24 insertions(+), 19 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 25677dd84..809ecd151 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -58,11 +58,10 @@ public:
     pcap = NULL;
   }
     
-  bool init(srslte::log *log_h_, mac_interface_rrc::mac_cfg_t *mac_cfg_, srslte::timers *timers_, demux *demux_unit_)
+  bool init(srslte::log *log_h_, srslte::timers *timers_, demux *demux_unit_)
   {
     timers_db  = timers_; 
     demux_unit = demux_unit_; 
-    mac_cfg    = mac_cfg_; 
     si_window_start = 0; 
     log_h = log_h_; 
     for (uint32_t i=0;i<N+1;i++) {
@@ -347,7 +346,6 @@ private:
 
   std::vector<dl_harq_process> proc;
   srslte::timers   *timers_db;
-  mac_interface_rrc::mac_cfg_t *mac_cfg; 
   demux           *demux_unit; 
   srslte::log     *log_h;
   srslte::mac_pcap *pcap; 
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 21c4a9602..107683ebb 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -59,21 +59,21 @@ public:
     timers_db   = NULL; 
     mux_unit    = NULL; 
     log_h       = NULL; 
-    mac_cfg     = NULL; 
+    params      = NULL;
     rntis       = NULL; 
     average_retx = 0; 
     nof_pkts     = 0;
   }
 
   bool init(srslte::log *log_h_, 
-            mac_interface_rrc::ue_rnti_t *rntis_, 
-            mac_interface_rrc::mac_cfg_t *mac_cfg_, 
+            mac_interface_rrc_common::ue_rnti_t *rntis_,
+            mac_interface_rrc_common::ul_harq_params_t *params_,
             srslte::timers* timers_db_, 
             mux *mux_unit_)
   {
     log_h     = log_h_;
     mux_unit  = mux_unit_;
-    mac_cfg   = mac_cfg_;
+    params    = params_;
     rntis     = rntis_;
     timers_db = timers_db_;
     for (uint32_t i=0;i<N;i++) {
@@ -201,9 +201,9 @@ private:
     {
       uint32_t max_retx;
       if (is_msg3) {
-        max_retx = harq_entity->mac_cfg->rach.max_harq_msg3_tx;
+        max_retx = harq_entity->params->max_harq_msg3_tx;
       } else {
-        max_retx = liblte_rrc_max_harq_tx_num[harq_entity->mac_cfg->main.ulsch_cnfg.max_harq_tx];
+        max_retx = harq_entity->params->max_harq_tx;
       }
 
       // Receive and route HARQ feedbacks
@@ -409,8 +409,8 @@ private:
   srslte::log     *log_h;
   srslte::mac_pcap *pcap; 
 
-  mac_interface_rrc::ue_rnti_t *rntis;
-  mac_interface_rrc::mac_cfg_t *mac_cfg; 
+  mac_interface_rrc_common::ue_rnti_t *rntis;
+  mac_interface_rrc_common::ul_harq_params_t *params;
   
   float            average_retx;   
   uint64_t         nof_pkts; 
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 9a461ca05..9fe08166f 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -66,13 +66,13 @@ bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac
   srslte_softbuffer_rx_init(&pch_softbuffer, 100);
   
   bsr_procedure.init(       rlc_h, log_h,          &config, &timers_db);
-  phr_procedure.init(phy_h,        log_h,          &config, &timers_db);
-  mux_unit.init     (       rlc_h, log_h,                               &bsr_procedure, &phr_procedure);
-  demux_unit.init   (phy_h, rlc_h, log_h,                   &timers_db);
-  ra_procedure.init (phy_h, rrc,   log_h, &uernti, &config, &timers_db, &mux_unit, &demux_unit);
+  phr_procedure.init(phy_h,        log_h,          &config,                &timers_db);
+  mux_unit.init     (       rlc_h, log_h,                                              &bsr_procedure, &phr_procedure);
+  demux_unit.init   (phy_h, rlc_h, log_h,                                  &timers_db);
+  ra_procedure.init (phy_h, rrc,   log_h, &uernti, &config,                &timers_db, &mux_unit, &demux_unit);
   sr_procedure.init (phy_h, rrc,   log_h,          &config);
-  ul_harq.init      (              log_h, &uernti, &config, &timers_db, &mux_unit);
-  dl_harq.init      (              log_h,          &config, &timers_db, &demux_unit);
+  ul_harq.init      (              log_h, &uernti, &config.ul_harq_params, &timers_db, &mux_unit);
+  dl_harq.init      (              log_h,                                  &timers_db, &demux_unit);
 
   reset();
   
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index a1805a1e9..144d98711 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -989,7 +989,8 @@ void rrc::apply_sib2_configs()
   mac->get_config(&cfg);
   cfg.main.time_alignment_timer = sib2.time_alignment_timer; 
   memcpy(&cfg.rach, &sib2.rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT)); 
-  cfg.prach_config_index = sib2.rr_config_common_sib.prach_cnfg.root_sequence_index; 
+  cfg.prach_config_index = sib2.rr_config_common_sib.prach_cnfg.root_sequence_index;
+  cfg.ul_harq_params.max_harq_msg3_tx = cfg.rach.max_harq_msg3_tx;
   mac->set_config(&cfg);
   
   rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
@@ -1218,7 +1219,13 @@ void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg
   }
   
   // Setup MAC configuration 
-  mac->set_config_main(&default_cfg);              
+  mac->set_config_main(&default_cfg);
+
+  // Update UL HARQ config
+  mac_interface_rrc::mac_cfg_t cfg;
+  mac->get_config(&cfg);
+  cfg.ul_harq_params.max_harq_tx = liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx];
+  mac->set_config(&cfg);
 
   rrc_log->info("Set MAC main config: harq-MaxReTX=%d, bsr-TimerReTX=%d, bsr-TimerPeriodic=%d\n",
                 liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx],

From f26969db8b0aa468500aaba3c674d8605919c6b4 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 15:00:41 +0200
Subject: [PATCH 029/269] adding UE base class

- this patch moves common code shared between multiple UE types, i.e.,
  UEs for different RATs into a commong base class
- it also introduces a switch during UE startup to instantiate the
  selected UE type
---
 srsue/hdr/ue.h           |  83 +-------------------
 srsue/hdr/ue_base.h      | 164 +++++++++++++++++++++++++++++++++++++++
 srsue/src/CMakeLists.txt |   2 +-
 srsue/src/main.cc        |  20 +++--
 srsue/src/ue.cc          |  70 +----------------
 srsue/src/ue_base.cc     | 111 ++++++++++++++++++++++++++
 6 files changed, 296 insertions(+), 154 deletions(-)
 create mode 100644 srsue/hdr/ue_base.h
 create mode 100644 srsue/src/ue_base.cc

diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 6fb593d55..c69dbddeb 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -37,6 +37,7 @@
 #include <string>
 #include <pthread.h>
 
+#include "ue_base.h"
 #include "srslte/radio/radio_multi.h"
 #include "phy/phy.h"
 #include "mac/mac.h"
@@ -56,89 +57,15 @@
 
 namespace srsue {
 
-/*******************************************************************************
-  UE Parameters
-*******************************************************************************/
-
-typedef struct {
-  float         dl_freq;
-  float         ul_freq;
-  float         rx_gain;
-  float         tx_gain;
-  uint32_t      nof_rx_ant; 
-  std::string   device_name; 
-  std::string   device_args; 
-  std::string   time_adv_nsamples; 
-  std::string   burst_preamble; 
-}rf_args_t;
-
-typedef struct {
-  bool          enable;
-  std::string   filename;
-}pcap_args_t;
-
-typedef struct {
-  bool          enable;
-  std::string   phy_filename;
-  std::string   radio_filename;
-}trace_args_t;
-
-typedef struct {
-  std::string   phy_level;
-  std::string   mac_level;
-  std::string   rlc_level;
-  std::string   pdcp_level;
-  std::string   rrc_level;
-  std::string   gw_level;
-  std::string   nas_level;
-  std::string   usim_level;
-  std::string   all_level;
-  int           phy_hex_limit;
-  int           mac_hex_limit;
-  int           rlc_hex_limit;
-  int           pdcp_hex_limit;
-  int           rrc_hex_limit;
-  int           gw_hex_limit;
-  int           nas_hex_limit;
-  int           usim_hex_limit;
-  int           all_hex_limit;
-  std::string   filename;
-}log_args_t;
-
-typedef struct {
-  bool          enable;
-}gui_args_t;
-
-typedef struct {
-  phy_args_t phy; 
-  float      metrics_period_secs;
-  bool pregenerate_signals;
-  int ue_cateogry;
-  
-}expert_args_t;
-
-typedef struct {
-  rf_args_t     rf;
-  rf_cal_t      rf_cal; 
-  pcap_args_t   pcap;
-  trace_args_t  trace;
-  log_args_t    log;
-  gui_args_t    gui;
-  usim_args_t   usim;
-  expert_args_t expert;
-}all_args_t;
-
 /*******************************************************************************
   Main UE class
 *******************************************************************************/
 
 class ue
-    :public ue_interface
-    ,public ue_metrics_interface
+    :public ue_base
 {
 public:
-  static ue* get_instance(void);
-  static void cleanup(void);
+  ue();
 
   bool init(all_args_t *args_);
   void stop();
@@ -158,8 +85,6 @@ public:
   
 
 private:
-  static ue *instance;
-  ue();
   virtual ~ue();
 
   srslte::radio_multi radio;
@@ -189,8 +114,6 @@ private:
   all_args_t       *args;
   bool              started;
   rf_metrics_t     rf_metrics;
-
-  srslte::LOG_LEVEL_ENUM level(std::string l);
   
   bool check_srslte_version();
 };
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
new file mode 100644
index 000000000..06c3f3e24
--- /dev/null
+++ b/srsue/hdr/ue_base.h
@@ -0,0 +1,164 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ * File:        ue_base.h
+ * Description: Base class for UEs.
+ *****************************************************************************/
+
+#ifndef UE_BASE_H
+#define UE_BASE_H
+
+#include <stdarg.h>
+#include <string>
+#include <pthread.h>
+#include "srslte/radio/radio_multi.h"
+#include "phy/phy.h"
+#include "upper/usim.h"
+#include "srslte/interfaces/ue_interfaces.h"
+
+#include "srslte/common/logger.h"
+#include "srslte/common/log_filter.h"
+
+#include "ue_metrics_interface.h"
+
+namespace srsue {
+
+/*******************************************************************************
+  UE Parameters
+*******************************************************************************/
+
+typedef struct {
+  float         dl_freq;
+  float         ul_freq;
+  float         rx_gain;
+  float         tx_gain;
+  uint32_t      nof_rx_ant; 
+  std::string   device_name; 
+  std::string   device_args; 
+  std::string   time_adv_nsamples; 
+  std::string   burst_preamble; 
+}rf_args_t;
+
+typedef struct {
+  bool          enable;
+  std::string   filename;
+}pcap_args_t;
+
+typedef struct {
+  bool          enable;
+  std::string   phy_filename;
+  std::string   radio_filename;
+}trace_args_t;
+
+typedef struct {
+  std::string   phy_level;
+  std::string   mac_level;
+  std::string   rlc_level;
+  std::string   pdcp_level;
+  std::string   rrc_level;
+  std::string   gw_level;
+  std::string   nas_level;
+  std::string   usim_level;
+  std::string   all_level;
+  int           phy_hex_limit;
+  int           mac_hex_limit;
+  int           rlc_hex_limit;
+  int           pdcp_hex_limit;
+  int           rrc_hex_limit;
+  int           gw_hex_limit;
+  int           nas_hex_limit;
+  int           usim_hex_limit;
+  int           all_hex_limit;
+  std::string   filename;
+}log_args_t;
+
+typedef struct {
+  bool          enable;
+}gui_args_t;
+
+typedef struct {
+  phy_args_t phy;
+  float      metrics_period_secs;
+  bool pregenerate_signals;
+  std::string ue_cateogry;
+}expert_args_t;
+
+typedef struct {
+  rf_args_t     rf;
+  rf_cal_t      rf_cal; 
+  pcap_args_t   pcap;
+  trace_args_t  trace;
+  log_args_t    log;
+  gui_args_t    gui;
+  usim_args_t   usim;
+  expert_args_t expert;
+}all_args_t;
+
+typedef enum {
+  LTE = 0,
+  SRSUE_INSTANCE_TYPE_NITEMS
+} srsue_instance_type_t;
+static const char srsue_instance_type_text[SRSUE_INSTANCE_TYPE_NITEMS][10] = { "LTE" };
+
+
+/*******************************************************************************
+  Main UE class
+*******************************************************************************/
+
+class ue_base
+    :public ue_interface
+    ,public ue_metrics_interface
+{
+public:
+  ue_base() {}
+  virtual ~ue_base() {}
+
+  static ue_base* get_instance(srsue_instance_type_t type);
+
+  void cleanup(void);
+
+  virtual bool init(all_args_t *args_) = 0;
+  virtual void stop() = 0;
+  virtual bool is_attached() = 0;
+  virtual void start_plot() = 0;
+  
+  void handle_rf_msg(srslte_rf_error_t error);
+
+  // UE metrics interface
+  virtual bool get_metrics(ue_metrics_t &m) = 0;
+
+  virtual void pregenerate_signals(bool enable) = 0;
+
+  srslte::log_filter rf_log;
+  rf_metrics_t     rf_metrics;
+  srslte::LOG_LEVEL_ENUM level(std::string l);
+};
+
+} // namespace srsue
+
+#endif // UE_BASE_H
+  
diff --git a/srsue/src/CMakeLists.txt b/srsue/src/CMakeLists.txt
index 4b2a7e282..3a3b9ebfe 100644
--- a/srsue/src/CMakeLists.txt
+++ b/srsue/src/CMakeLists.txt
@@ -31,7 +31,7 @@ if (RPATH)
   set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
 endif (RPATH)
 
-add_executable(srsue main.cc ue.cc metrics_stdout.cc)
+add_executable(srsue main.cc ue_base.cc ue.cc metrics_stdout.cc)
 target_link_libraries(srsue   srsue_mac
                               srsue_phy
                               srsue_upper
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index f114c0606..16e0cb59f 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -126,7 +126,7 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      "index of the core used by the sync thread")
 
     ("expert.ue_category",
-     bpo::value<int>(&args->expert.ue_cateogry)->default_value(4),
+     bpo::value<string>(&args->expert.ue_cateogry)->default_value("4"),
      "UE Category (1 to 5)")
 
     ("expert.metrics_period_secs",
@@ -342,18 +342,26 @@ void *input_loop(void *m) {
   return NULL;
 }
 
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[])
+{
   signal(SIGINT, sig_int_handler);
   all_args_t args;
-  metrics_stdout metrics;
-  ue *ue = ue::get_instance();
+  parse_args(&args, argc, argv);
 
-  cout << "---  Software Radio Systems LTE UE  ---" << endl << endl;
 
-  parse_args(&args, argc, argv);
+  srsue_instance_type_t type = LTE;
+  ue_base *ue = ue_base::get_instance(type);
+  if (!ue) {
+    cout << "Error creating UE instance." << endl << endl;
+    exit(1);
+  }
+
+  cout << "---  Software Radio Systems " << srsue_instance_type_text[type] << " UE  ---" << endl << endl;
   if (!ue->init(&args)) {
     exit(1);
   }
+
+  metrics_stdout metrics;
   metrics.init(ue, args.expert.metrics_period_secs);
 
   pthread_t input;
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 97f59c15e..dea1c3751 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -26,7 +26,6 @@
 
 
 #include "ue.h"
-//#include "srslte_version_check.h"
 #include "srslte/srslte.h"
 #include <pthread.h>
 #include <iostream>
@@ -38,28 +37,6 @@ using namespace srslte;
 
 namespace srsue{
 
-ue*           ue::instance = NULL;
-pthread_mutex_t ue_instance_mutex = PTHREAD_MUTEX_INITIALIZER;
-
-ue* ue::get_instance(void)
-{
-  pthread_mutex_lock(&ue_instance_mutex);
-  if(NULL == instance) {
-      instance = new ue();
-  }
-  pthread_mutex_unlock(&ue_instance_mutex);
-  return(instance);
-}
-void ue::cleanup(void)
-{
-  pthread_mutex_lock(&ue_instance_mutex);
-  if(NULL != instance) {
-      delete instance;
-      instance = NULL;
-  }
-  pthread_mutex_unlock(&ue_instance_mutex);
-}
-
 ue::ue()
     :started(false)
 {
@@ -188,7 +165,7 @@ bool ue::init(all_args_t *args_)
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, SECURITY_DIRECTION_UPLINK);
   rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
   
-  rrc.set_ue_category(args->expert.ue_cateogry);
+  rrc.set_ue_category(atoi(args->expert.ue_cateogry.c_str()));
   
   nas.init(&usim, &rrc, &gw, &nas_log);
   gw.init(&pdcp, &rrc, this, &gw_log);
@@ -271,49 +248,8 @@ bool ue::get_metrics(ue_metrics_t &m)
 
 void ue::rf_msg(srslte_rf_error_t error)
 {
-  ue *u = ue::get_instance();
-  u->handle_rf_msg(error);
-}
-
-void ue::handle_rf_msg(srslte_rf_error_t error)
-{
-  if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OVERFLOW) {
-    rf_metrics.rf_o++;
-    rf_metrics.rf_error = true;
-    rf_log.warning("Overflow\n");
-  }else if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_UNDERFLOW) {
-    rf_metrics.rf_u++;
-    rf_metrics.rf_error = true;
-    rf_log.warning("Underflow\n");
-  } else if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_LATE) {
-    rf_metrics.rf_l++;
-    rf_metrics.rf_error = true;
-    rf_log.warning("Late\n");
-  } else if (error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OTHER) {
-    std::string str(error.msg);
-    str.erase(std::remove(str.begin(), str.end(), '\n'), str.end());
-    str.erase(std::remove(str.begin(), str.end(), '\r'), str.end());
-    str.push_back('\n');
-    rf_log.info(str);
-  }
-}
-
-srslte::LOG_LEVEL_ENUM ue::level(std::string l)
-{
-  std::transform(l.begin(), l.end(), l.begin(), ::toupper);
-  if("NONE" == l){
-    return srslte::LOG_LEVEL_NONE;
-  }else if("ERROR" == l){
-    return srslte::LOG_LEVEL_ERROR;
-  }else if("WARNING" == l){
-    return srslte::LOG_LEVEL_WARNING;
-  }else if("INFO" == l){
-    return srslte::LOG_LEVEL_INFO;
-  }else if("DEBUG" == l){
-    return srslte::LOG_LEVEL_DEBUG;
-  }else{
-    return srslte::LOG_LEVEL_NONE;
-  }
+  ue_base *ue = ue_base::get_instance(LTE);
+  ue->handle_rf_msg(error);
 }
 
 } // namespace srsue
diff --git a/srsue/src/ue_base.cc b/srsue/src/ue_base.cc
new file mode 100644
index 000000000..61cbafb2c
--- /dev/null
+++ b/srsue/src/ue_base.cc
@@ -0,0 +1,111 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+
+#include "ue_base.h"
+#include "ue.h"
+#include "srslte/srslte.h"
+#include <pthread.h>
+#include <iostream>
+#include <string>
+#include <algorithm>
+#include <iterator>
+
+using namespace srslte;
+
+namespace srsue{
+
+static ue_base* instance = NULL;
+pthread_mutex_t ue_instance_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+ue_base* ue_base::get_instance(srsue_instance_type_t type)
+{
+  pthread_mutex_lock(&ue_instance_mutex);
+  if(NULL == instance) {
+    switch (type) {
+      case LTE:
+        instance = new ue();
+        break;
+      default:
+        perror("Unknown UE type.\n");
+    }
+  }
+  pthread_mutex_unlock(&ue_instance_mutex);
+  return(instance);
+}
+
+void ue_base::cleanup(void)
+{
+  pthread_mutex_lock(&ue_instance_mutex);
+  if(NULL != instance) {
+    delete instance;
+    instance = NULL;
+  }
+  pthread_mutex_unlock(&ue_instance_mutex);
+}
+
+void ue_base::handle_rf_msg(srslte_rf_error_t error)
+{
+  if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OVERFLOW) {
+    rf_metrics.rf_o++;
+    rf_metrics.rf_error = true;
+    rf_log.warning("Overflow\n");
+  }else if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_UNDERFLOW) {
+    rf_metrics.rf_u++;
+    rf_metrics.rf_error = true;
+    rf_log.warning("Underflow\n");
+  } else if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_LATE) {
+    rf_metrics.rf_l++;
+    rf_metrics.rf_error = true;
+    rf_log.warning("Late\n");
+  } else if (error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OTHER) {
+    std::string str(error.msg);
+    str.erase(std::remove(str.begin(), str.end(), '\n'), str.end());
+    str.erase(std::remove(str.begin(), str.end(), '\r'), str.end());
+    str.push_back('\n');
+    rf_log.info(str);
+  }
+}
+
+srslte::LOG_LEVEL_ENUM ue_base::level(std::string l)
+{
+  std::transform(l.begin(), l.end(), l.begin(), ::toupper);
+  if("NONE" == l){
+    return srslte::LOG_LEVEL_NONE;
+  }else if("ERROR" == l){
+    return srslte::LOG_LEVEL_ERROR;
+  }else if("WARNING" == l){
+    return srslte::LOG_LEVEL_WARNING;
+  }else if("INFO" == l){
+    return srslte::LOG_LEVEL_INFO;
+  }else if("DEBUG" == l){
+    return srslte::LOG_LEVEL_DEBUG;
+  }else{
+    return srslte::LOG_LEVEL_NONE;
+  }
+}
+
+} // namespace srsue

From 20a004a7681848da6db743c81fdad10ab6e6c3f1 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 23 Jun 2017 15:24:51 +0200
Subject: [PATCH 030/269] add seperate interface for MUX to query BSR procedure

- this is also intented to make MUX RAT-agnostic
---
 lib/include/srslte/interfaces/ue_interfaces.h | 25 +++++++++++++++++++
 srsue/hdr/mac/mux.h                           |  4 +--
 srsue/hdr/mac/proc_bsr.h                      | 14 +----------
 srsue/src/mac/mux.cc                          |  2 +-
 4 files changed, 29 insertions(+), 16 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index a05f117a4..41b90bf9f 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -230,6 +230,31 @@ public:
 };
 
 
+//BSR interface for MUX
+class bsr_interface_mux
+{
+public:
+  typedef enum {
+    LONG_BSR,
+    SHORT_BSR,
+    TRUNC_BSR
+  } bsr_format_t;
+
+  typedef struct {
+    bsr_format_t format;
+    uint32_t buff_size[4];
+  } bsr_t;
+
+  /* MUX calls BSR to check if it can fit a BSR into PDU */
+  virtual bool need_to_send_bsr_on_ul_grant(uint32_t grant_size, bsr_t *bsr) = 0;
+
+  /* MUX calls BSR to let it generate a padding BSR if there is space in PDU */
+  virtual bool generate_padding_bsr(uint32_t nof_padding_bytes, bsr_t *bsr) = 0;
+
+  /* MAX calls BSR to set the Tx TTI */
+  virtual void set_tx_tti(uint32_t tti) = 0;
+};
+
 
 /** MAC interface 
  *
diff --git a/srsue/hdr/mac/mux.h b/srsue/hdr/mac/mux.h
index db98ebe78..b33cf0d19 100644
--- a/srsue/hdr/mac/mux.h
+++ b/srsue/hdr/mac/mux.h
@@ -57,7 +57,7 @@ class mux
 public:
   mux();
   void     reset();
-  void     init(rlc_interface_mac *rlc, srslte::log *log_h, bsr_proc *bsr_procedure, phr_proc *phr_procedure_);
+  void     init(rlc_interface_mac *rlc, srslte::log *log_h, bsr_interface_mux *bsr_procedure, phr_proc *phr_procedure_);
 
   bool     is_pending_any_sdu();
   bool     is_pending_sdu(uint32_t lcid); 
@@ -94,7 +94,7 @@ private:
 
   srslte::log       *log_h;
   rlc_interface_mac *rlc; 
-  bsr_proc          *bsr_procedure;
+  bsr_interface_mux *bsr_procedure;
   phr_proc          *phr_procedure;
   uint16_t           pending_crnti_ce;
   
diff --git a/srsue/hdr/mac/proc_bsr.h b/srsue/hdr/mac/proc_bsr.h
index bbfaa1c90..21f278e29 100644
--- a/srsue/hdr/mac/proc_bsr.h
+++ b/srsue/hdr/mac/proc_bsr.h
@@ -37,7 +37,7 @@
 
 namespace srsue {
 
-class bsr_proc : public srslte::timer_callback
+class bsr_proc : public srslte::timer_callback, public bsr_interface_mux
 {
 public:
   bsr_proc();
@@ -48,18 +48,6 @@ public:
   void set_priority(uint32_t lcid, uint32_t priority); 
   void timer_expired(uint32_t timer_id);
   uint32_t get_buffer_state();
-  
-  typedef enum {
-    LONG_BSR, 
-    SHORT_BSR, 
-    TRUNC_BSR    
-  } bsr_format_t;
-  
-  typedef struct {
-    bsr_format_t format; 
-    uint32_t buff_size[4];
-  } bsr_t; 
-
   bool need_to_send_bsr_on_ul_grant(uint32_t grant_size, bsr_t *bsr); 
   bool generate_padding_bsr(uint32_t nof_padding_bytes, bsr_t *bsr);
   bool need_to_send_sr(uint32_t tti); 
diff --git a/srsue/src/mac/mux.cc b/srsue/src/mac/mux.cc
index 58ba3d57f..38a71649a 100644
--- a/srsue/src/mac/mux.cc
+++ b/srsue/src/mac/mux.cc
@@ -51,7 +51,7 @@ mux::mux() : pdu_msg(MAX_NOF_SUBHEADERS)
   msg3_flush();
 }
 
-void mux::init(rlc_interface_mac *rlc_, srslte::log *log_h_, bsr_proc *bsr_procedure_, phr_proc *phr_procedure_)
+void mux::init(rlc_interface_mac *rlc_, srslte::log *log_h_, bsr_interface_mux *bsr_procedure_, phr_proc *phr_procedure_)
 {
   log_h      = log_h_;
   rlc        = rlc_;

From c247f1103d1dd99a04fb70de712ce379141b5ed1 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 26 Jun 2017 10:08:55 +0200
Subject: [PATCH 031/269] parameterize TTI offset between UL grant and UL tx
 and UL tx and HARQ rx

---
 lib/include/srslte/interfaces/ue_interfaces.h |  1 +
 srsue/hdr/mac/ul_harq.h                       | 10 +++++-----
 srsue/src/phy/phch_worker.cc                  |  3 +++
 3 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 41b90bf9f..3cffaae1e 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -300,6 +300,7 @@ public:
     uint32_t                rv;
     uint16_t                rnti; 
     uint32_t                current_tx_nb;
+    int32_t                 tti_offset;     // relative offset between grant and UL tx/HARQ rx
     srslte_softbuffer_tx_t *softbuffer;
     srslte_phy_grant_t      phy_grant;
     uint8_t                *payload_ptr; 
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 107683ebb..2ef1ed2b6 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -128,13 +128,13 @@ public:
 
   void new_grant_ul_ack(Tgrant grant, bool ack, Taction *action)
   {
-    set_ack(grant.tti, ack);
+    set_ack(grant.tti, ack, action);
     new_grant_ul(grant, action);
   }
 
   void harq_recv(uint32_t tti, bool ack, Taction *action)
   {
-    set_ack(tti, ack);
+    set_ack(tti, ack, action);
     run_tti(tti, NULL, action);
   }
 
@@ -385,13 +385,13 @@ private:
   // Called with UL grant
   void run_tti(uint32_t tti, Tgrant *grant, Taction* action)
   {
-    uint32_t tti_tx = (tti+4)%10240;
+    uint32_t tti_tx = (tti+action->tti_offset)%10240;
     proc[pidof(tti_tx)].run_tti(tti_tx, grant, action);
   }
 
-  void set_ack(uint32_t tti, bool ack)
+  void set_ack(uint32_t tti, bool ack, Taction *action)
   {
-    int tti_harq = (int) tti - 4;
+    int tti_harq = (int) tti - action->tti_offset;
     if (tti_harq < 0) {
       tti_harq += 10240;
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 14de9b51a..c2796e2ad 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -240,6 +240,9 @@ void phch_worker::work_imp()
     set_uci_periodic_cqi();
   }
 
+  /* TTI offset for UL is always 4 for LTE */
+  ul_action.tti_offset = 4;
+
   /* Send UL grant or HARQ information (from PHICH) to MAC */
   if (ul_grant_available         && ul_ack_available)  {    
     phy->mac->new_grant_ul_ack(ul_mac_grant, ul_ack, &ul_action);      

From ce1e565c793b3095cc5eed0a39b439a1656b6606 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 26 Jun 2017 10:21:20 +0200
Subject: [PATCH 032/269] move RRC states definition into common header

---
 srsue/hdr/upper/rrc.h        | 19 +------------
 srsue/hdr/upper/rrc_common.h | 52 ++++++++++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+), 18 deletions(-)
 create mode 100644 srsue/hdr/upper/rrc_common.h

diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index cc1a22fd1..a36fdacc0 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -29,6 +29,7 @@
 
 #include "pthread.h"
 
+#include "rrc_common.h"
 #include "srslte/common/buffer_pool.h"
 #include "srslte/common/log.h"
 #include "srslte/common/common.h"
@@ -41,24 +42,6 @@ using srslte::byte_buffer_t;
 
 namespace srsue {
 
-// RRC states (3GPP 36.331 v10.0.0)
-typedef enum{
-    RRC_STATE_IDLE = 0,
-    RRC_STATE_SIB1_SEARCH,
-    RRC_STATE_SIB2_SEARCH,
-    RRC_STATE_WAIT_FOR_CON_SETUP,
-    RRC_STATE_COMPLETING_SETUP,
-    RRC_STATE_RRC_CONNECTED,
-    RRC_STATE_N_ITEMS,
-}rrc_state_t;
-static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
-                                                            "SIB1_SEARCH",
-                                                            "SIB2_SEARCH",
-                                                            "WAIT FOR CON SETUP",
-                                                            "COMPLETING SETUP",
-                                                            "RRC CONNECTED"};
-
-
 class rrc
     :public rrc_interface_nas
     ,public rrc_interface_phy
diff --git a/srsue/hdr/upper/rrc_common.h b/srsue/hdr/upper/rrc_common.h
new file mode 100644
index 000000000..692a20cb3
--- /dev/null
+++ b/srsue/hdr/upper/rrc_common.h
@@ -0,0 +1,52 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef RRC_COMMON_H
+#define RRC_COMMON_H
+
+namespace srsue {
+
+// RRC states (3GPP 36.331 v10.0.0)
+typedef enum{
+    RRC_STATE_IDLE = 0,
+    RRC_STATE_SIB1_SEARCH,
+    RRC_STATE_SIB2_SEARCH,
+    RRC_STATE_WAIT_FOR_CON_SETUP,
+    RRC_STATE_COMPLETING_SETUP,
+    RRC_STATE_RRC_CONNECTED,
+    RRC_STATE_N_ITEMS,
+}rrc_state_t;
+static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
+                                                            "SIB1_SEARCH",
+                                                            "SIB2_SEARCH",
+                                                            "WAIT FOR CON SETUP",
+                                                            "COMPLETING SETUP",
+                                                            "RRC CONNECTED"};
+
+} // namespace srsue
+
+
+#endif // RRC_COMMON_H

From cbfcb281a971d2605ca82fccc17cca33767b27b7 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 26 Jun 2017 13:37:36 +0200
Subject: [PATCH 033/269] seperate RAT agnostic bits from various interfaces

---
 lib/include/srslte/interfaces/ue_interfaces.h | 60 +++++++++++--------
 1 file changed, 35 insertions(+), 25 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 3cffaae1e..3fb3d354b 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -104,11 +104,16 @@ public:
 };
 
 // RRC interface for MAC
-class rrc_interface_mac
+class rrc_interface_mac_common
+{
+public:
+  virtual void ra_problem() = 0;
+};
+
+class rrc_interface_mac : public rrc_interface_mac_common
 {
 public:
   virtual void release_pucch_srs() = 0;
-  virtual void ra_problem() = 0; 
 };
 
 // RRC interface for PHY
@@ -426,46 +431,51 @@ typedef struct {
   float estimator_fil_w;   
   bool rssi_sensor_enabled;
 } phy_args_t; 
-  
-/* Interface MAC -> PHY */
-class phy_interface_mac
+
+
+/* RAT agnostic Interface MAC -> PHY */
+class phy_interface_mac_common
 {
 public:
-  /* Configure PRACH using parameters written by RRC */
-  virtual void configure_prach_params() = 0;
-  
   /* Start synchronization with strongest cell in the current carrier frequency */
-  virtual void sync_start() = 0; 
+  virtual void sync_start() = 0;
   virtual void sync_stop() = 0;
-  
+
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
-  virtual void set_crnti(uint16_t rnti) = 0; 
-  
+  virtual void set_crnti(uint16_t rnti) = 0;
+
+  /* Time advance commands */
+  virtual void set_timeadv_rar(uint32_t ta_cmd) = 0;
+  virtual void set_timeadv(uint32_t ta_cmd) = 0;
+
+  /* Sets RAR grant payload */
+  virtual void set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN]) = 0;
+
+  virtual uint32_t get_current_tti() = 0;
+
+  virtual float get_phr() = 0;
+  virtual float get_pathloss_db() = 0;
+};
+
+/* Interface MAC -> PHY */
+class phy_interface_mac : public phy_interface_mac_common
+{
+public:
+  /* Configure PRACH using parameters written by RRC */
+  virtual void configure_prach_params() = 0;
+
   virtual void prach_send(uint32_t preamble_idx, int allowed_subframe, float target_power_dbm) = 0;  
   virtual int  prach_tx_tti() = 0; 
   
   /* Indicates the transmission of a SR signal in the next opportunity */
   virtual void sr_send() = 0;  
   virtual int  sr_last_tx_tti() = 0; 
-  
-  /* Time advance commands */
-  virtual void set_timeadv_rar(uint32_t ta_cmd) = 0;
-  virtual void set_timeadv(uint32_t ta_cmd) = 0;
-  
-  /* Sets RAR grant payload */
-  virtual void set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN]) = 0; 
 
   /* Instruct the PHY to decode PDCCH with the CRC scrambled with given RNTI */
   virtual void pdcch_ul_search(srslte_rnti_type_t rnti_type, uint16_t rnti, int tti_start = -1, int tti_end = -1) = 0;
   virtual void pdcch_dl_search(srslte_rnti_type_t rnti_type, uint16_t rnti, int tti_start = -1, int tti_end = -1) = 0;
   virtual void pdcch_ul_search_reset() = 0;
   virtual void pdcch_dl_search_reset() = 0;
-  
-  virtual uint32_t get_current_tti() = 0;
-  
-  virtual float get_phr() = 0; 
-  virtual float get_pathloss_db() = 0;
-    
 };
 
 class phy_interface_rrc

From 83c358d33f070eff4cf19bc9ef589c3834979c5f Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 26 Jun 2017 13:49:12 +0200
Subject: [PATCH 034/269] let demux use common interface

---
 srsue/hdr/mac/demux.h  | 10 +++++-----
 srsue/src/mac/demux.cc |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/srsue/hdr/mac/demux.h b/srsue/hdr/mac/demux.h
index 773181220..8c9fb3a02 100644
--- a/srsue/hdr/mac/demux.h
+++ b/srsue/hdr/mac/demux.h
@@ -42,7 +42,7 @@ class demux : public srslte::pdu_queue::process_callback
 {
 public:
   demux();
-  void init(phy_interface_mac* phy_h_, rlc_interface_mac *rlc, srslte::log* log_h_, srslte::timers* timers_db_);
+  void init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc, srslte::log* log_h_, srslte::timers* timers_db_);
 
   bool     process_pdus();
   uint8_t* request_buffer(uint32_t pid, uint32_t len);
@@ -73,10 +73,10 @@ private:
   
   bool       is_uecrid_successful; 
     
-  phy_interface_mac *phy_h; 
-  srslte::log       *log_h;
-  srslte::timers    *timers_db;
-  rlc_interface_mac *rlc;
+  phy_interface_mac_common *phy_h;
+  srslte::log              *log_h;
+  srslte::timers           *timers_db;
+  rlc_interface_mac        *rlc;
   
   // Buffer of PDUs
   srslte::pdu_queue pdus; 
diff --git a/srsue/src/mac/demux.cc b/srsue/src/mac/demux.cc
index 71ad0cc58..227bcc364 100644
--- a/srsue/src/mac/demux.cc
+++ b/srsue/src/mac/demux.cc
@@ -40,7 +40,7 @@ demux::demux() : mac_msg(20), pending_mac_msg(20)
 {
 }
 
-void demux::init(phy_interface_mac* phy_h_, rlc_interface_mac *rlc_, srslte::log* log_h_, srslte::timers* timers_db_)
+void demux::init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc_, srslte::log* log_h_, srslte::timers* timers_db_)
 {
   phy_h     = phy_h_; 
   log_h     = log_h_; 

From 60da04ef5843b00ff8f912d1fd19df84a179b12a Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 28 Jun 2017 13:21:56 +0200
Subject: [PATCH 035/269] make LCID of GW object a parameter

---
 lib/include/srslte/upper/gw.h | 11 ++++++-----
 lib/src/upper/gw.cc           |  5 +++--
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/lib/include/srslte/upper/gw.h b/lib/include/srslte/upper/gw.h
index a01413829..ce5936b71 100644
--- a/lib/include/srslte/upper/gw.h
+++ b/lib/include/srslte/upper/gw.h
@@ -46,7 +46,7 @@ class gw
 {
 public:
   gw();
-  void init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_);
+  void init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_);
   void stop();
 
   void get_metrics(gw_metrics_t &m);
@@ -56,11 +56,11 @@ public:
 
   // NAS interface
   error_t setup_if_addr(uint32_t ip_addr, char *err_str);
-  
+
 private:
-  
-  static const int GW_THREAD_PRIO = 7; 
-  
+
+  static const int GW_THREAD_PRIO = 7;
+
   srsue::pdcp_interface_gw  *pdcp;
   srsue::rrc_interface_gw   *rrc;
   srsue::ue_interface       *ue;
@@ -73,6 +73,7 @@ private:
   struct ifreq        ifr;
   int32               sock;
   bool                if_up;
+  uint32_t            lcid;
 
   long                ul_tput_bytes;
   long                dl_tput_bytes;
diff --git a/lib/src/upper/gw.cc b/lib/src/upper/gw.cc
index beb003c51..685803a4d 100644
--- a/lib/src/upper/gw.cc
+++ b/lib/src/upper/gw.cc
@@ -44,13 +44,14 @@ gw::gw()
   :if_up(false)
 {}
 
-void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_)
+void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_)
 {
   pool    = byte_buffer_pool::get_instance();
   pdcp    = pdcp_;
   rrc     = rrc_;
   ue      = ue_;
   gw_log  = gw_log_;
+  lcid    = lcid_;
   run_enable = true;
 
   gettimeofday(&metrics_time[1], NULL);
@@ -261,7 +262,7 @@ void gw::run_thread()
             // Send PDU directly to PDCP
             pdu->set_timestamp();
             ul_tput_bytes += pdu->N_bytes;
-            pdcp->write_sdu(RB_ID_DRB1, pdu);
+            pdcp->write_sdu(lcid, pdu);
             
             do {
               pdu = pool_allocate;

From 8dbabb2834a1d018e9bf6e5b0b267b407f030f23 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 28 Jun 2017 13:31:43 +0200
Subject: [PATCH 036/269] rework bearer/LCID handling

This is a larger patch that reworks the LCID handling throughout the
code.

- It first moves the RB/LCID mapping out of common.h into the RRC object
  because different RATs may have different mappings.
- It adds a interface to RRC that other objects like RLC/PDCP/etc. may
  use to get the bearer name of a specific LCID.
- The patch also introduces a PDCP config class.
---
 lib/include/srslte/common/common.h            | 26 ------
 lib/include/srslte/common/interfaces_common.h | 21 +++++
 lib/include/srslte/interfaces/ue_interfaces.h |  5 +-
 lib/include/srslte/upper/pdcp.h               |  4 +-
 lib/include/srslte/upper/pdcp_entity.h        | 11 +--
 lib/include/srslte/upper/rlc.h                |  5 +-
 lib/src/upper/pdcp.cc                         | 19 ++--
 lib/src/upper/pdcp_entity.cc                  | 92 +++++++------------
 lib/src/upper/rlc.cc                          | 23 +++--
 lib/src/upper/rlc_am.cc                       | 76 +++++++--------
 lib/src/upper/rlc_tm.cc                       |  8 +-
 lib/src/upper/rlc_um.cc                       | 40 ++++----
 lib/test/upper/rlc_am_test.cc                 |  1 +
 lib/test/upper/rlc_um_test.cc                 |  1 +
 srsenb/hdr/upper/common_enb.h                 | 26 ++++++
 srsenb/hdr/upper/pdcp.h                       |  1 +
 srsenb/hdr/upper/rlc.h                        |  2 +
 srsenb/src/upper/pdcp.cc                      | 21 ++++-
 srsenb/src/upper/rlc.cc                       |  8 +-
 srsenb/src/upper/rrc.cc                       | 14 ++-
 srsenb/test/upper/ip_test.cc                  |  3 +-
 srsue/hdr/upper/nas.h                         |  4 +-
 srsue/hdr/upper/rrc.h                         | 18 ++++
 srsue/src/ue.cc                               | 11 ++-
 srsue/src/upper/nas.cc                        | 16 ++--
 srsue/src/upper/rrc.cc                        | 44 +++++++--
 srsue/test/upper/ip_test.cc                   |  3 +-
 27 files changed, 287 insertions(+), 216 deletions(-)

diff --git a/lib/include/srslte/common/common.h b/lib/include/srslte/common/common.h
index 5f13ab214..ad88d9dcc 100644
--- a/lib/include/srslte/common/common.h
+++ b/lib/include/srslte/common/common.h
@@ -83,32 +83,6 @@ static const char error_text[ERROR_N_ITEMS][20] = { "None",
                                                     "Can't start",
                                                     "Already started"};
 
-typedef enum{
-  RB_ID_SRB0 = 0,
-  RB_ID_SRB1,
-  RB_ID_SRB2,
-  RB_ID_DRB1,
-  RB_ID_DRB2,
-  RB_ID_DRB3,
-  RB_ID_DRB4,
-  RB_ID_DRB5,
-  RB_ID_DRB6,
-  RB_ID_DRB7,
-  RB_ID_DRB8,
-  RB_ID_N_ITEMS,
-}rb_id_t;
-static const char rb_id_text[RB_ID_N_ITEMS][20] = { "SRB0",
-                                                    "SRB1",
-                                                    "SRB2",
-                                                    "DRB1",
-                                                    "DRB2",
-                                                    "DRB3",
-                                                    "DRB4",
-                                                    "DRB5",
-                                                    "DRB6",
-                                                    "DRB7",
-                                                    "DRB8"};
-
 /******************************************************************************
  * Byte and Bit buffers
  *
diff --git a/lib/include/srslte/common/interfaces_common.h b/lib/include/srslte/common/interfaces_common.h
index 76025a64f..5ef45cf54 100644
--- a/lib/include/srslte/common/interfaces_common.h
+++ b/lib/include/srslte/common/interfaces_common.h
@@ -28,9 +28,30 @@
 #define INTERFACE_COMMON_H
 
 #include "srslte/common/timers.h"
+#include "srslte/common/security.h"
 
 namespace srslte {
 
+class srslte_pdcp_config_t
+{
+public:
+  srslte_pdcp_config_t(bool is_control_ = false)
+    :direction(SECURITY_DIRECTION_UPLINK)
+    ,is_control(is_control_)
+    ,is_data(false)
+    ,do_security(false)
+    ,sn_len(12) {}
+
+  u_int8_t            direction;
+  bool                is_control;
+  bool                is_data;
+  bool                do_security;
+  uint8_t             sn_len;
+
+  // TODO: Support the following configurations
+  // bool do_rohc;
+};
+
 class mac_interface_timers
 {
 public: 
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 3fb3d354b..ca9b4540b 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -132,6 +132,7 @@ public:
   virtual uint16_t get_mcc() = 0;
   virtual uint16_t get_mnc() = 0;
   virtual void enable_capabilities() = 0;
+  virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
 // RRC interface for GW
@@ -151,6 +152,7 @@ public:
   virtual void write_pdu_bcch_bch(srslte::byte_buffer_t *pdu) = 0;
   virtual void write_pdu_bcch_dlsch(srslte::byte_buffer_t *pdu) = 0;
   virtual void write_pdu_pcch(srslte::byte_buffer_t *pdu) = 0;
+  virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
 // RRC interface for RLC
@@ -158,6 +160,7 @@ class rrc_interface_rlc
 {
 public:
   virtual void max_retx_attempted() = 0;
+  virtual std::string get_rb_name(uint32_t lcid) = 0;
 };
 
 // PDCP interface for GW
@@ -173,7 +176,7 @@ class pdcp_interface_rrc
 public:
   virtual void reset() = 0;
   virtual void write_sdu(uint32_t lcid, srslte::byte_buffer_t *sdu) = 0;
-  virtual void add_bearer(uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg=NULL) = 0;
+  virtual void add_bearer(uint32_t lcid, srslte::srslte_pdcp_config_t cnfg = srslte::srslte_pdcp_config_t()) = 0;
   virtual void config_security(uint32_t lcid,
                                uint8_t *k_rrc_enc_,
                                uint8_t *k_rrc_int_,
diff --git a/lib/include/srslte/upper/pdcp.h b/lib/include/srslte/upper/pdcp.h
index 857806810..abdbeeaea 100644
--- a/lib/include/srslte/upper/pdcp.h
+++ b/lib/include/srslte/upper/pdcp.h
@@ -46,13 +46,14 @@ public:
             srsue::rrc_interface_pdcp *rrc_,
             srsue::gw_interface_pdcp *gw_,
             log *pdcp_log_,
+            uint32_t lcid_,
             uint8_t direction_);
   void stop();
 
   // RRC interface
   void reset();
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
-  void add_bearer(uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg = NULL);
+  void add_bearer(uint32_t lcid, srslte_pdcp_config_t cnfg = srslte_pdcp_config_t());
   void config_security(uint32_t lcid,
                        uint8_t *k_rrc_enc,
                        uint8_t *k_rrc_int,
@@ -72,6 +73,7 @@ private:
 
   log                       *pdcp_log;
   pdcp_entity                pdcp_array[SRSLTE_N_RADIO_BEARERS];
+  uint32_t                   lcid; // default LCID that is maintained active by PDCP instance
   uint8_t                    direction;
 
   bool valid_lcid(uint32_t lcid);
diff --git a/lib/include/srslte/upper/pdcp_entity.h b/lib/include/srslte/upper/pdcp_entity.h
index 2c1c988a0..0ff005d07 100644
--- a/lib/include/srslte/upper/pdcp_entity.h
+++ b/lib/include/srslte/upper/pdcp_entity.h
@@ -67,9 +67,7 @@ public:
             srsue::gw_interface_pdcp      *gw_,
             srslte::log                   *log_,
             uint32_t                       lcid_,
-            uint8_t                        direction_,
-            LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg = NULL
-            );
+            srslte_pdcp_config_t           cfg_);
   void reset();
 
   bool is_active();
@@ -94,12 +92,7 @@ private:
 
   bool                active;
   uint32_t            lcid;
-  bool                do_security;
-  u_int8_t            direction;
-
-  uint8_t             sn_len;
-  // TODO: Support the following configurations
-  // bool do_rohc;
+  srslte_pdcp_config_t cfg;
 
   uint32_t            rx_count;
   uint32_t            tx_count;
diff --git a/lib/include/srslte/upper/rlc.h b/lib/include/srslte/upper/rlc.h
index 9d7f7ff39..4a95f1a46 100644
--- a/lib/include/srslte/upper/rlc.h
+++ b/lib/include/srslte/upper/rlc.h
@@ -55,13 +55,15 @@ public:
             srsue::rrc_interface_rlc  *rrc_,
             srsue::ue_interface       *ue_,
             log        *rlc_log_, 
-            mac_interface_timers *mac_timers_);
+            mac_interface_timers *mac_timers_,
+            uint32_t                  lcid_);
   void stop();
 
   void get_metrics(rlc_metrics_t &m);
 
   // PDCP interface
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
+  std::string get_rb_name(uint32_t lcid);
 
   // MAC interface
   uint32_t get_buffer_state(uint32_t lcid);
@@ -87,6 +89,7 @@ private:
   srslte::mac_interface_timers *mac_timers; 
   srsue::ue_interface         *ue;
   srslte::rlc_entity           rlc_array[SRSLTE_N_RADIO_BEARERS];
+  uint32_t                     default_lcid;
 
   long                ul_tput_bytes[SRSLTE_N_RADIO_BEARERS];
   long                dl_tput_bytes[SRSLTE_N_RADIO_BEARERS];
diff --git a/lib/src/upper/pdcp.cc b/lib/src/upper/pdcp.cc
index e729118ce..f28eac974 100644
--- a/lib/src/upper/pdcp.cc
+++ b/lib/src/upper/pdcp.cc
@@ -32,15 +32,16 @@ namespace srslte {
 pdcp::pdcp()
 {}
 
-void pdcp::init(srsue::rlc_interface_pdcp *rlc_, srsue::rrc_interface_pdcp *rrc_, srsue::gw_interface_pdcp *gw_, log *pdcp_log_, uint8_t direction_)
+void pdcp::init(srsue::rlc_interface_pdcp *rlc_, srsue::rrc_interface_pdcp *rrc_, srsue::gw_interface_pdcp *gw_, log *pdcp_log_, uint32_t lcid_, uint8_t direction_)
 {
   rlc       = rlc_;
   rrc       = rrc_;
   gw        = gw_;
   pdcp_log  = pdcp_log_;
+  lcid      = lcid_;
   direction = direction_;
 
-  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, RB_ID_SRB0, direction); // SRB0
+  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, lcid, direction);
 }
 
 void pdcp::stop()
@@ -52,7 +53,7 @@ void pdcp::reset()
     pdcp_array[i].reset();
   }
 
-  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, RB_ID_SRB0, direction); // SRB0
+  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, lcid, direction);
 }
 
 /*******************************************************************************
@@ -64,17 +65,17 @@ void pdcp::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
     pdcp_array[lcid].write_sdu(sdu);
 }
 
-void pdcp::add_bearer(uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg)
+void pdcp::add_bearer(uint32_t lcid, srslte_pdcp_config_t cfg)
 {
-  if(lcid < 0 || lcid >= SRSLTE_N_RADIO_BEARERS) {
+  if(lcid >= SRSLTE_N_RADIO_BEARERS) {
     pdcp_log->error("Radio bearer id must be in [0:%d] - %d\n", SRSLTE_N_RADIO_BEARERS, lcid);
     return;
   }
   if (!pdcp_array[lcid].is_active()) {
-    pdcp_array[lcid].init(rlc, rrc, gw, pdcp_log, lcid, direction, cnfg);
-    pdcp_log->info("Added bearer %s\n", rb_id_text[lcid]);
+    pdcp_array[lcid].init(rlc, rrc, gw, pdcp_log, lcid, cfg);
+    pdcp_log->info("Added bearer %s\n", rrc->get_rb_name(lcid).c_str());
   } else {
-    pdcp_log->warning("Bearer %s already configured. Reconfiguration not supported\n", rb_id_text[lcid]);
+    pdcp_log->warning("Bearer %s already configured. Reconfiguration not supported\n", rrc->get_rb_name(lcid));
   }
 }
 
@@ -116,7 +117,7 @@ void pdcp::write_pdu_pcch(byte_buffer_t *sdu)
 *******************************************************************************/
 bool pdcp::valid_lcid(uint32_t lcid)
 {
-  if(lcid < 0 || lcid >= SRSLTE_N_RADIO_BEARERS) {
+  if(lcid >= SRSLTE_N_RADIO_BEARERS) {
     pdcp_log->error("Radio bearer id must be in [0:%d] - %d", SRSLTE_N_RADIO_BEARERS, lcid);
     return false;
   }
diff --git a/lib/src/upper/pdcp_entity.cc b/lib/src/upper/pdcp_entity.cc
index 23fe8768d..9f658a577 100644
--- a/lib/src/upper/pdcp_entity.cc
+++ b/lib/src/upper/pdcp_entity.cc
@@ -34,8 +34,6 @@ pdcp_entity::pdcp_entity()
   :active(false)
   ,tx_count(0)
   ,rx_count(0)
-  ,do_security(false)
-  ,sn_len(12)
 {
   pool = byte_buffer_pool::get_instance();
 }
@@ -45,38 +43,27 @@ void pdcp_entity::init(srsue::rlc_interface_pdcp      *rlc_,
                        srsue::gw_interface_pdcp       *gw_,
                        srslte::log                    *log_,
                        uint32_t                       lcid_,
-                       u_int8_t                       direction_,
-                       LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg)
+                       srslte_pdcp_config_t           cfg_)
 {
   rlc       = rlc_;
   rrc       = rrc_;
   gw        = gw_;
   log       = log_;
   lcid      = lcid_;
-  direction = direction_;
+  cfg       = cfg_;
   active    = true;
 
   tx_count    = 0;
   rx_count    = 0;
-  do_security = false;
 
-  if(cnfg)
-  {
-    if(cnfg->rlc_um_pdcp_sn_size_present) {
-      if(LIBLTE_RRC_PDCP_SN_SIZE_7_BITS == cnfg->rlc_um_pdcp_sn_size) {
-        sn_len = 7;
-      }
-    }
-    // TODO: handle remainder of cnfg
-  }
-  log->debug("Init %s\n", rb_id_text[lcid]);
+  log->debug("Init %s\n", rrc->get_rb_name(lcid).c_str());
 }
 
 void pdcp_entity::reset()
 {
   active      = false;
   if(log)
-    log->debug("Reset %s\n", rb_id_text[lcid]);
+    log->debug("Reset %s\n", rrc->get_rb_name(lcid).c_str());
 }
 
 bool pdcp_entity::is_active()
@@ -87,44 +74,32 @@ bool pdcp_entity::is_active()
 // RRC interface
 void pdcp_entity::write_sdu(byte_buffer_t *sdu)
 {
-  log->info_hex(sdu->msg, sdu->N_bytes, "TX %s SDU, do_security = %s", rb_id_text[lcid], (do_security)?"true":"false");
+  log->info_hex(sdu->msg, sdu->N_bytes, "TX %s SDU, do_security = %s", rrc->get_rb_name(lcid).c_str(), (cfg.do_security)?"true":"false");
 
-  // Handle SRB messages
-  switch(lcid)
-  {
-  case RB_ID_SRB0:
-    rlc->write_sdu(lcid, sdu);
-    break;
-  case RB_ID_SRB1:  // Intentional fall-through
-  case RB_ID_SRB2:
+  if (cfg.is_control) {
     pdcp_pack_control_pdu(tx_count, sdu);
-    if(do_security)
+    if(cfg.do_security)
     {
       integrity_generate(&k_rrc_int[16],
                          tx_count,
                          lcid-1,
-                         direction,
+                         cfg.direction,
                          sdu->msg,
                          sdu->N_bytes-4,
                          &sdu->msg[sdu->N_bytes-4]);
     }
     tx_count++;
-    rlc->write_sdu(lcid, sdu);
-
-    break;
   }
 
-  // Handle DRB messages
-  if(lcid >= RB_ID_DRB1)
-  {
-    if(12 == sn_len)
-    {
+  if (cfg.is_data) {
+    if(12 == cfg.sn_len) {
       pdcp_pack_data_pdu_long_sn(tx_count++, sdu);
     } else {
       pdcp_pack_data_pdu_short_sn(tx_count++, sdu);
     }
-    rlc->write_sdu(lcid, sdu);
   }
+
+  rlc->write_sdu(lcid, sdu);
 }
 
 void pdcp_entity::config_security(uint8_t *k_rrc_enc_,
@@ -132,7 +107,7 @@ void pdcp_entity::config_security(uint8_t *k_rrc_enc_,
                                   CIPHERING_ALGORITHM_ID_ENUM cipher_algo_,
                                   INTEGRITY_ALGORITHM_ID_ENUM integ_algo_)
 {
-  do_security = true;
+  cfg.do_security = true;
   for(int i=0; i<32; i++)
   {
     k_rrc_enc[i] = k_rrc_enc_[i];
@@ -145,37 +120,32 @@ void pdcp_entity::config_security(uint8_t *k_rrc_enc_,
 // RLC interface
 void pdcp_entity::write_pdu(byte_buffer_t *pdu)
 {
-  // Handle SRB messages
-  switch(lcid)
-  {
-  case RB_ID_SRB0:
-    // Simply pass on to RRC
-    log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU", rb_id_text[lcid]);
-    rrc->write_pdu(RB_ID_SRB0, pdu);
-    break;
-  case RB_ID_SRB1: // Intentional fall-through
-  case RB_ID_SRB2:
-    uint32_t sn;
-    log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU", rb_id_text[lcid]);
-    pdcp_unpack_control_pdu(pdu, &sn);
-    log->info_hex(pdu->msg, pdu->N_bytes, "RX %s SDU SN: %d",
-                  rb_id_text[lcid], sn);
-    rrc->write_pdu(lcid, pdu);
-    break;
-  }
 
-  // Handle DRB messages
-  if(lcid >= RB_ID_DRB1)
-  {
+
+
+
+
+  if (cfg.is_data) {
     uint32_t sn;
-    if(12 == sn_len)
+    if(12 == cfg.sn_len)
     {
       pdcp_unpack_data_pdu_long_sn(pdu, &sn);
     } else {
       pdcp_unpack_data_pdu_short_sn(pdu, &sn);
     }
-    log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU: %d", rb_id_text[lcid], sn);
+    log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU: %d", rrc->get_rb_name(lcid).c_str(), sn);
     gw->write_pdu(lcid, pdu);
+  } else {
+    if (cfg.is_control) {
+      uint32_t sn;
+      pdcp_unpack_control_pdu(pdu, &sn);
+      log->info_hex(pdu->msg, pdu->N_bytes, "RX %s SDU SN: %d",
+                    rrc->get_rb_name(lcid).c_str(), sn);
+    } else {
+      log->info_hex(pdu->msg, pdu->N_bytes, "RX %s PDU", rrc->get_rb_name(lcid).c_str());
+    }
+    // pass to RRC
+    rrc->write_pdu(lcid, pdu);
   }
 }
 
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index becb15c37..c840de10d 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -41,18 +41,20 @@ void rlc::init(srsue::pdcp_interface_rlc *pdcp_,
                srsue::rrc_interface_rlc  *rrc_,
                srsue::ue_interface       *ue_,
                log                       *rlc_log_, 
-               mac_interface_timers      *mac_timers_)
+               mac_interface_timers      *mac_timers_,
+               uint32_t                  lcid_)
 {
   pdcp    = pdcp_;
   rrc     = rrc_;
   ue      = ue_;
   rlc_log = rlc_log_;
   mac_timers = mac_timers_;
+  default_lcid = lcid_;
 
   gettimeofday(&metrics_time[1], NULL);
   reset_metrics(); 
 
-  rlc_array[0].init(RLC_MODE_TM, rlc_log, RB_ID_SRB0, pdcp, rrc, mac_timers); // SRB0
+  rlc_array[0].init(RLC_MODE_TM, rlc_log, default_lcid, pdcp, rrc, mac_timers); // SRB0
 }
 
 void rlc::reset_metrics() 
@@ -97,7 +99,7 @@ void rlc::reset()
       rlc_array[i].reset();
   }
 
-  rlc_array[0].init(RLC_MODE_TM, rlc_log, RB_ID_SRB0, pdcp, rrc, mac_timers); // SRB0
+  rlc_array[0].init(RLC_MODE_TM, rlc_log, default_lcid, pdcp, rrc, mac_timers); // SRB0
 }
 
 /*******************************************************************************
@@ -110,6 +112,11 @@ void rlc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
   }
 }
 
+std::string rlc::get_rb_name(uint32_t lcid)
+{
+  return rrc->get_rb_name(lcid);
+}
+
 /*******************************************************************************
   MAC interface
 *******************************************************************************/
@@ -188,7 +195,7 @@ void rlc::add_bearer(uint32_t lcid)
 {
   // No config provided - use defaults for lcid
   LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
-  if(RB_ID_SRB1 == lcid || RB_ID_SRB2 == lcid)
+  if(default_lcid == lcid || (default_lcid+1) == lcid)
   {
     if (!rlc_array[lcid].active()) {
       cnfg.rlc_mode                     = LIBLTE_RRC_RLC_MODE_AM;
@@ -200,11 +207,11 @@ void rlc::add_bearer(uint32_t lcid)
       cnfg.dl_am_rlc.t_status_prohibit  = LIBLTE_RRC_T_STATUS_PROHIBIT_MS0;
       add_bearer(lcid, &cnfg);
     } else {
-      rlc_log->warning("Bearer %s already configured. Reconfiguration not supported\n", rb_id_text[lcid]);
+      rlc_log->warning("Bearer %s already configured. Reconfiguration not supported\n", get_rb_name(lcid).c_str());
     }
   }else{
     rlc_log->error("Radio bearer %s does not support default RLC configuration.",
-                   rb_id_text[lcid]);
+                   get_rb_name(lcid).c_str());
   }
 }
 
@@ -218,7 +225,7 @@ void rlc::add_bearer(uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
   
   if (!rlc_array[lcid].active()) {
     rlc_log->info("Adding radio bearer %s with mode %s\n",
-                    rb_id_text[lcid], liblte_rrc_rlc_mode_text[cnfg->rlc_mode]);  
+                  get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode]);
     switch(cnfg->rlc_mode)
     {
     case LIBLTE_RRC_RLC_MODE_AM:
@@ -238,7 +245,7 @@ void rlc::add_bearer(uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
       return;
     }
   } else {
-    rlc_log->warning("Bearer %s already created.\n", rb_id_text[lcid]);
+    rlc_log->warning("Bearer %s already created.\n", get_rb_name(lcid).c_str());
   }
   rlc_array[lcid].configure(cnfg);    
 
diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index 416a141c8..d25b2d2ef 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -86,7 +86,7 @@ void rlc_am::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
 
   log->info("%s configured: t_poll_retx=%d, poll_pdu=%d, poll_byte=%d, max_retx_thresh=%d, "
             "t_reordering=%d, t_status_prohibit=%d\n",
-            rb_id_text[lcid], t_poll_retx, poll_pdu, poll_byte, max_retx_thresh,
+            rrc->get_rb_name(lcid).c_str(), t_poll_retx, poll_pdu, poll_byte, max_retx_thresh,
             t_reordering, t_status_prohibit);
 }
 
@@ -176,7 +176,7 @@ uint32_t rlc_am::get_bearer()
 
 void rlc_am::write_sdu(byte_buffer_t *sdu)
 {
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rb_id_text[lcid]);
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rrc->get_rb_name(lcid).c_str());
   tx_sdu_queue.write(sdu);
 }
 
@@ -347,7 +347,7 @@ void rlc_am::check_reordering_timeout()
   if(reordering_timeout.is_running() && reordering_timeout.expired())
   {
     reordering_timeout.reset();
-    log->debug("%s reordering timeout expiry - updating vr_ms\n", rb_id_text[lcid]);
+    log->debug("%s reordering timeout expiry - updating vr_ms\n", rrc->get_rb_name(lcid).c_str());
 
     // 36.322 v10 Section 5.1.3.2.4
     vr_ms = vr_x;
@@ -409,7 +409,7 @@ int  rlc_am::build_status_pdu(uint8_t *payload, uint32_t nof_bytes)
   if(pdu_len > 0 && nof_bytes >= (uint32_t)pdu_len)
   {
     log->info("%s Tx status PDU - %s\n",
-              rb_id_text[lcid], rlc_am_to_string(&status).c_str());
+              rrc->get_rb_name(lcid).c_str(), rlc_am_to_string(&status).c_str());
 
     do_status     = false;
     poll_received = false;
@@ -420,7 +420,7 @@ int  rlc_am::build_status_pdu(uint8_t *payload, uint32_t nof_bytes)
     return rlc_am_write_status_pdu(&status, payload);
   }else{
     log->warning("%s Cannot tx status PDU - %d bytes available, %d bytes required\n",
-                 rb_id_text[lcid], nof_bytes, pdu_len);
+                 rrc->get_rb_name(lcid).c_str(), nof_bytes, pdu_len);
     return 0;
   }
 }
@@ -437,7 +437,7 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
 
   // Is resegmentation needed?
   if(retx.is_segment || required_buffer_size(retx) > (int)nof_bytes) {
-    log->debug("%s build_retx_pdu - resegmentation required\n", rb_id_text[lcid]);
+    log->debug("%s build_retx_pdu - resegmentation required\n", rrc->get_rb_name(lcid).c_str());
     return build_segment(payload, nof_bytes, retx);
   }
 
@@ -462,7 +462,7 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
   if(tx_window[retx.sn].retx_count >= max_retx_thresh)
     rrc->max_retx_attempted();
   log->info("%s Retx PDU scheduled for tx. SN: %d, retx count: %d\n",
-            rb_id_text[lcid], retx.sn, tx_window[retx.sn].retx_count);
+            rrc->get_rb_name(lcid).c_str(), retx.sn, tx_window[retx.sn].retx_count);
 
   debug_state();
   return (ptr-payload) + tx_window[retx.sn].buf->N_bytes;
@@ -495,7 +495,7 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
   if(nof_bytes <= head_len)
   {
     log->warning("%s Cannot build a PDU segment - %d bytes available, %d bytes required for header\n",
-                 rb_id_text[lcid], nof_bytes, head_len);
+                 rrc->get_rb_name(lcid).c_str(), nof_bytes, head_len);
     return 0;
   }
   pdu_space = nof_bytes-head_len;
@@ -561,15 +561,15 @@ int rlc_am::build_segment(uint8_t *payload, uint32_t nof_bytes, rlc_amd_retx_t r
   memcpy(ptr, data, len);
 
   log->info("%s Retx PDU segment scheduled for tx. SN: %d, SO: %d\n",
-            rb_id_text[lcid], retx.sn, retx.so_start);
+            rrc->get_rb_name(lcid).c_str(), retx.sn, retx.so_start);
 
   debug_state();
   int pdu_len = (ptr-payload) + len;
   if(pdu_len > (int)nof_bytes) {
     log->error("%s Retx PDU segment length error. Available: %d, Used: %d\n",
-               rb_id_text[lcid], nof_bytes, pdu_len);
+               rrc->get_rb_name(lcid).c_str(), nof_bytes, pdu_len);
     log->debug("%s Retx PDU segment length error. Header len: %d, Payload len: %d, N_li: %d\n",
-               rb_id_text[lcid], (ptr-payload), len, new_header.N_li);
+               rrc->get_rb_name(lcid).c_str(), (ptr-payload), len, new_header.N_li);
   }
   return pdu_len;
 
@@ -617,13 +617,13 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   if(pdu_space <= head_len)
   {
     log->warning("%s Cannot build a PDU - %d bytes available, %d bytes required for header\n",
-                 rb_id_text[lcid], nof_bytes, head_len);
+                 rrc->get_rb_name(lcid).c_str(), nof_bytes, head_len);
     pool->deallocate(pdu);
     return 0;
   }
 
   log->debug("%s Building PDU - pdu_space: %d, head_len: %d \n",
-            rb_id_text[lcid], pdu_space, head_len);
+            rrc->get_rb_name(lcid).c_str(), pdu_space, head_len);
 
   // Check for SDU segment
   if(tx_sdu)
@@ -638,7 +638,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     if(tx_sdu->N_bytes == 0)
     {
       log->info("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
-                rb_id_text[lcid], tx_sdu->get_latency_us());
+                rrc->get_rb_name(lcid).c_str(), tx_sdu->get_latency_us());
       pool->deallocate(tx_sdu);
       tx_sdu = NULL;
     }
@@ -649,7 +649,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     header.fi |= RLC_FI_FIELD_NOT_START_ALIGNED; // First byte does not correspond to first byte of SDU
 
     log->debug("%s Building PDU - added SDU segment (len:%d) - pdu_space: %d, head_len: %d \n",
-              rb_id_text[lcid], to_move, pdu_space, head_len);
+              rrc->get_rb_name(lcid).c_str(), to_move, pdu_space, head_len);
   }
 
   // Pull SDUs from queue
@@ -673,7 +673,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     if(tx_sdu->N_bytes == 0)
     {
       log->info("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
-                rb_id_text[lcid], tx_sdu->get_latency_us());
+                rrc->get_rb_name(lcid).c_str(), tx_sdu->get_latency_us());
       pool->deallocate(tx_sdu);
       tx_sdu = NULL;
     }
@@ -683,7 +683,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
       pdu_space = 0;
 
     log->debug("%s Building PDU - added SDU segment (len:%d) - pdu_space: %d, head_len: %d \n",
-              rb_id_text[lcid], to_move, pdu_space, head_len);
+              rrc->get_rb_name(lcid).c_str(), to_move, pdu_space, head_len);
   }
 
   if(tx_sdu)
@@ -692,11 +692,11 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   // Set Poll bit
   pdu_without_poll++;
   byte_without_poll += (pdu->N_bytes + head_len);
-  log->debug("%s pdu_without_poll: %d\n", rb_id_text[lcid], pdu_without_poll);
-  log->debug("%s byte_without_poll: %d\n", rb_id_text[lcid], byte_without_poll);
+  log->debug("%s pdu_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), pdu_without_poll);
+  log->debug("%s byte_without_poll: %d\n", rrc->get_rb_name(lcid).c_str(), byte_without_poll);
   if(poll_required())
   {
-    log->debug("%s setting poll bit to request status\n", rb_id_text[lcid]);
+    log->debug("%s setting poll bit to request status\n", rrc->get_rb_name(lcid).c_str());
     header.p          = 1;
     poll_sn           = vt_s;
     pdu_without_poll  = 0;
@@ -707,7 +707,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   // Set SN
   header.sn = vt_s;
   vt_s = (vt_s + 1)%MOD;
-  log->info("%s PDU scheduled for tx. SN: %d\n", rb_id_text[lcid], header.sn);
+  log->info("%s PDU scheduled for tx. SN: %d\n", rrc->get_rb_name(lcid).c_str(), header.sn);
 
   // Place PDU in tx_window, write header and TX
   tx_window[header.sn].buf        = pdu;
@@ -728,26 +728,26 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
   std::map<uint32_t, rlc_amd_rx_pdu_t>::iterator it;
 
   log->info_hex(payload, nof_bytes, "%s Rx data PDU SN: %d",
-                rb_id_text[lcid], header.sn);
+                rrc->get_rb_name(lcid).c_str(), header.sn);
 
   if(!inside_rx_window(header.sn)) {
     if(header.p) {
-      log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+      log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
       do_status = true;
     }
     log->info("%s SN: %d outside rx window [%d:%d] - discarding\n",
-              rb_id_text[lcid], header.sn, vr_r, vr_mr);
+              rrc->get_rb_name(lcid).c_str(), header.sn, vr_r, vr_mr);
     return;
   }
 
   it = rx_window.find(header.sn);
   if(rx_window.end() != it) {
     if(header.p) {
-      log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+      log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
       do_status = true;
     }
     log->info("%s Discarding duplicate SN: %d\n",
-              rb_id_text[lcid], header.sn);
+              rrc->get_rb_name(lcid).c_str(), header.sn);
     return;
   }
 
@@ -780,7 +780,7 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
   // Check poll bit
   if(header.p)
   {
-    log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+    log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
     poll_received = true;
 
     // 36.322 v10 Section 5.2.3
@@ -825,16 +825,16 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
   std::map<uint32_t, rlc_amd_rx_pdu_segments_t>::iterator it;
 
   log->info_hex(payload, nof_bytes, "%s Rx data PDU segment. SN: %d, SO: %d",
-                rb_id_text[lcid], header.sn, header.so);
+                rrc->get_rb_name(lcid).c_str(), header.sn, header.so);
 
   // Check inside rx window
   if(!inside_rx_window(header.sn)) {
     if(header.p) {
-      log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+      log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
       do_status = true;
     }
     log->info("%s SN: %d outside rx window [%d:%d] - discarding\n",
-              rb_id_text[lcid], header.sn, vr_r, vr_mr);
+              rrc->get_rb_name(lcid).c_str(), header.sn, vr_r, vr_mr);
     return;
   }
 
@@ -853,7 +853,7 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
   if(rx_segments.end() != it) {
 
     if(header.p) {
-      log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+      log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
       do_status = true;
     }
 
@@ -883,7 +883,7 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
     // Check poll bit
     if(header.p)
     {
-      log->info("%s Status packet requested through polling bit\n", rb_id_text[lcid]);
+      log->info("%s Status packet requested through polling bit\n", rrc->get_rb_name(lcid).c_str());
       poll_received = true;
 
       // 36.322 v10 Section 5.2.3
@@ -901,12 +901,12 @@ void rlc_am::handle_data_pdu_segment(uint8_t *payload, uint32_t nof_bytes, rlc_a
 
 void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
 {
-  log->info_hex(payload, nof_bytes, "%s Rx control PDU", rb_id_text[lcid]);
+  log->info_hex(payload, nof_bytes, "%s Rx control PDU", rrc->get_rb_name(lcid).c_str());
 
   rlc_status_pdu_t status;
   rlc_am_read_status_pdu(payload, nof_bytes, &status);
 
-  log->info("%s Rx Status PDU: %s\n", rb_id_text[lcid], rlc_am_to_string(&status).c_str());
+  log->info("%s Rx Status PDU: %s\n", rrc->get_rb_name(lcid).c_str(), rlc_am_to_string(&status).c_str());
 
   poll_retx_timeout.reset();
 
@@ -944,7 +944,7 @@ void rlc_am::handle_control_pdu(uint8_t *payload, uint32_t nof_bytes)
                   }
               } else {
                 log->warning("%s invalid segment NACK received for SN %d. so_start: %d, so_end: %d, N_bytes: %d\n",
-                             rb_id_text[lcid], i, status.nacks[j].so_start, status.nacks[j].so_end, it->second.buf->N_bytes);
+                             rrc->get_rb_name(lcid).c_str(), i, status.nacks[j].so_start, status.nacks[j].so_end, it->second.buf->N_bytes);
               }
             }
 
@@ -998,7 +998,7 @@ void rlc_am::reassemble_rx_sdus()
       rx_sdu->N_bytes += len;
       rx_window[vr_r].buf->msg += len;
       rx_window[vr_r].buf->N_bytes -= len;
-      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rb_id_text[lcid]);
+      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rrc->get_rb_name(lcid).c_str());
       rx_sdu->set_timestamp();
       pdcp->write_pdu(lcid, rx_sdu);
       rx_sdu = pool_allocate;
@@ -1014,7 +1014,7 @@ void rlc_am::reassemble_rx_sdus()
     rx_sdu->N_bytes += rx_window[vr_r].buf->N_bytes;
     if(rlc_am_end_aligned(rx_window[vr_r].header.fi))
     {
-      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rb_id_text[lcid]);
+      log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU", rrc->get_rb_name(lcid).c_str());
       rx_sdu->set_timestamp();
       pdcp->write_pdu(lcid, rx_sdu);
       rx_sdu = pool_allocate;
@@ -1058,7 +1058,7 @@ void rlc_am::debug_state()
 {
   log->debug("%s vt_a = %d, vt_ms = %d, vt_s = %d, poll_sn = %d "
              "vr_r = %d, vr_mr = %d, vr_x = %d, vr_ms = %d, vr_h = %d\n",
-             rb_id_text[lcid], vt_a, vt_ms, vt_s, poll_sn,
+             rrc->get_rb_name(lcid).c_str(), vt_a, vt_ms, vt_s, poll_sn,
              vr_r, vr_mr, vr_x, vr_ms, vr_h);
 
 }
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index a515a77a5..b97b78be7 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -79,7 +79,7 @@ uint32_t rlc_tm::get_bearer()
 // PDCP interface
 void rlc_tm::write_sdu(byte_buffer_t *sdu)
 {
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rb_id_text[lcid]);
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rrc->get_rb_name(lcid).c_str());
   ul_queue.write(sdu);
 }
 
@@ -99,7 +99,7 @@ int rlc_tm::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   uint32_t pdu_size = ul_queue.size_tail_bytes();
   if(pdu_size > nof_bytes)
   {
-    log->error("TX %s PDU size larger than MAC opportunity\n", rb_id_text[lcid]);
+    log->error("TX %s PDU size larger than MAC opportunity\n", rrc->get_rb_name(lcid).c_str());
     return 0;
   }
   byte_buffer_t *buf;
@@ -107,9 +107,9 @@ int rlc_tm::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   pdu_size = buf->N_bytes;
   memcpy(payload, buf->msg, buf->N_bytes);
   log->info("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
-            rb_id_text[lcid], buf->get_latency_us());
+            rrc->get_rb_name(lcid).c_str(), buf->get_latency_us());
   pool->deallocate(buf);
-  log->info_hex(payload, pdu_size, "TX %s, %s PDU", rb_id_text[lcid], rlc_mode_text[RLC_MODE_TM]);
+  log->info_hex(payload, pdu_size, "TX %s, %s PDU", rrc->get_rb_name(lcid).c_str(), rlc_mode_text[RLC_MODE_TM]);
   return pdu_size;
 }
 
diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index 78fc0a68b..8724e0f28 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -78,7 +78,7 @@ void rlc_um::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
     tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == tx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits, tx_sn_field_length=%u bits\n",
-              rb_id_text[lcid], liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
               t_reordering,
               rlc_umd_sn_size_num[rx_sn_field_length],
               rlc_umd_sn_size_num[tx_sn_field_length]);
@@ -87,7 +87,7 @@ void rlc_um::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
     tx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->ul_um_uni_rlc.sn_field_len;
     tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == tx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: tx_sn_field_length=%u bits\n",
-              rb_id_text[lcid], liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
               rlc_umd_sn_size_num[tx_sn_field_length]);
     break;
   case LIBLTE_RRC_RLC_MODE_UM_UNI_DL:
@@ -97,7 +97,7 @@ void rlc_um::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
     rx_mod              = (RLC_UMD_SN_SIZE_5_BITS == rx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits\n",
-              rb_id_text[lcid], liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
               liblte_rrc_t_reordering_num[t_reordering],
               rlc_umd_sn_size_num[rx_sn_field_length]);
     break;
@@ -159,7 +159,7 @@ uint32_t rlc_um::get_bearer()
 
 void rlc_um::write_sdu(byte_buffer_t *sdu)
 {
-  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rb_id_text[lcid]);
+  log->info_hex(sdu->msg, sdu->N_bytes, "%s Tx SDU", rrc->get_rb_name(lcid).c_str());
   tx_sdu_queue.write(sdu);
 }
 
@@ -222,7 +222,7 @@ void rlc_um::timer_expired(uint32_t timeout_id)
 
     // 36.322 v10 Section 5.1.2.2.4
     log->info("%s reordering timeout expiry - updating vr_ur and reassembling\n",
-               rb_id_text[lcid]);
+               rrc->get_rb_name(lcid).c_str());
 
     log->warning("Lost PDU SN: %d\n", vr_ur);
     pdu_lost = true;
@@ -286,7 +286,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   if(pdu_space <= head_len)
   {
     log->warning("%s Cannot build a PDU - %d bytes available, %d bytes required for header\n",
-                 rb_id_text[lcid], nof_bytes, head_len);
+                 rrc->get_rb_name(lcid).c_str(), nof_bytes, head_len);
     return 0;
   }
 
@@ -296,7 +296,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     uint32_t space = pdu_space-head_len;
     to_move = space >= tx_sdu->N_bytes ? tx_sdu->N_bytes : space;
     log->debug("%s adding remainder of SDU segment - %d bytes of %d remaining\n",
-               rb_id_text[lcid], to_move, tx_sdu->N_bytes);
+               rrc->get_rb_name(lcid).c_str(), to_move, tx_sdu->N_bytes);
     memcpy(pdu_ptr, tx_sdu->msg, to_move);
     last_li          = to_move;
     pdu_ptr         += to_move;
@@ -306,7 +306,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     if(tx_sdu->N_bytes == 0)
     {
       log->info("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
-                rb_id_text[lcid], tx_sdu->get_latency_us());
+                rrc->get_rb_name(lcid).c_str(), tx_sdu->get_latency_us());
       pool->deallocate(tx_sdu);
       tx_sdu = NULL;
     }
@@ -325,7 +325,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     uint32_t space = pdu_space-head_len;
     to_move = space >= tx_sdu->N_bytes ? tx_sdu->N_bytes : space;
     log->debug("%s adding new SDU segment - %d bytes of %d remaining\n",
-               rb_id_text[lcid], to_move, tx_sdu->N_bytes);
+               rrc->get_rb_name(lcid).c_str(), to_move, tx_sdu->N_bytes);
     memcpy(pdu_ptr, tx_sdu->msg, to_move);
     last_li          = to_move;
     pdu_ptr         += to_move;
@@ -335,7 +335,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     if(tx_sdu->N_bytes == 0)
     {
       log->info("%s Complete SDU scheduled for tx. Stack latency: %ld us\n",
-                rb_id_text[lcid], tx_sdu->get_latency_us());
+                rrc->get_rb_name(lcid).c_str(), tx_sdu->get_latency_us());
       pool->deallocate(tx_sdu);
       tx_sdu = NULL;
     }
@@ -350,11 +350,11 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   vt_us = (vt_us + 1)%tx_mod;
 
   // Add header and TX
-  log->debug("%s packing PDU with length %d\n", rb_id_text[lcid], pdu->N_bytes);
+  log->debug("%s packing PDU with length %d\n", rrc->get_rb_name(lcid).c_str(), pdu->N_bytes);
   rlc_um_write_data_pdu_header(&header, pdu);
   memcpy(payload, pdu->msg, pdu->N_bytes);
   uint32_t ret = pdu->N_bytes;
-  log->debug("%sreturning length %d\n", rb_id_text[lcid], pdu->N_bytes);
+  log->debug("%sreturning length %d\n", rrc->get_rb_name(lcid).c_str(), pdu->N_bytes);
   pool->deallocate(pdu);
 
   debug_state();
@@ -368,20 +368,20 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   rlc_um_read_data_pdu_header(payload, nof_bytes, rx_sn_field_length, &header);
 
   log->info_hex(payload, nof_bytes, "RX %s Rx data PDU SN: %d",
-                rb_id_text[lcid], header.sn);
+                rrc->get_rb_name(lcid).c_str(), header.sn);
 
   if(RX_MOD_BASE(header.sn) >= RX_MOD_BASE(vr_uh-rx_window_size) &&
      RX_MOD_BASE(header.sn) <  RX_MOD_BASE(vr_ur))
   {
     log->info("%s SN: %d outside rx window [%d:%d] - discarding\n",
-              rb_id_text[lcid], header.sn, vr_ur, vr_uh);
+              rrc->get_rb_name(lcid).c_str(), header.sn, vr_ur, vr_uh);
     return;
   }
   it = rx_window.find(header.sn);
   if(rx_window.end() != it)
   {
     log->info("%s Discarding duplicate SN: %d\n",
-              rb_id_text[lcid], header.sn);
+              rrc->get_rb_name(lcid).c_str(), header.sn);
     return;
   }
 
@@ -456,7 +456,7 @@ void rlc_um::reassemble_rx_sdus()
           log->warning("Dropping remainder of lost PDU (lower edge middle segments, vr_ur=%d, vr_ur_in_rx_sdu=%d)\n", vr_ur, vr_ur_in_rx_sdu);
           rx_sdu->reset();
         } else {
-          log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d, i=%d (lower edge middle segments)", rb_id_text[lcid], vr_ur, i);
+          log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d, i=%d (lower edge middle segments)", rrc->get_rb_name(lcid).c_str(), vr_ur, i);
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
@@ -476,7 +476,7 @@ void rlc_um::reassemble_rx_sdus()
           log->warning("Dropping remainder of lost PDU (lower edge last segments)\n");
           rx_sdu->reset();          
         } else {
-          log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d (lower edge last segments)", rb_id_text[lcid], vr_ur);
+          log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d (lower edge last segments)", rrc->get_rb_name(lcid).c_str(), vr_ur);
           rx_sdu->set_timestamp();
           pdcp->write_pdu(lcid, rx_sdu);
           rx_sdu = pool_allocate;
@@ -510,7 +510,7 @@ void rlc_um::reassemble_rx_sdus()
         log->warning("Dropping remainder of lost PDU (update vr_ur middle segments, vr_ur=%d, vr_ur_in_rx_sdu=%d)\n", vr_ur, vr_ur_in_rx_sdu);
         rx_sdu->reset();
       } else {
-        log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d, i=%d, (update vr_ur middle segments)", rb_id_text[lcid], vr_ur, i);
+        log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d, i=%d, (update vr_ur middle segments)", rrc->get_rb_name(lcid).c_str(), vr_ur, i);
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
@@ -530,7 +530,7 @@ void rlc_um::reassemble_rx_sdus()
         log->warning("Dropping remainder of lost PDU (update vr_ur last segments)\n");
         rx_sdu->reset();
       } else {
-        log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d (update vr_ur last segments)", rb_id_text[lcid], vr_ur);
+        log->info_hex(rx_sdu->msg, rx_sdu->N_bytes, "%s Rx SDU vr_ur=%d (update vr_ur last segments)", rrc->get_rb_name(lcid).c_str(), vr_ur);
         rx_sdu->set_timestamp();
         pdcp->write_pdu(lcid, rx_sdu);
         rx_sdu = pool_allocate;
@@ -560,7 +560,7 @@ bool rlc_um::inside_reordering_window(uint16_t sn)
 void rlc_um::debug_state()
 {
   log->debug("%s vt_us = %d, vr_ur = %d, vr_ux = %d, vr_uh = %d \n",
-             rb_id_text[lcid], vt_us, vr_ur, vr_ux, vr_uh);
+             rrc->get_rb_name(lcid).c_str(), vt_us, vr_ur, vr_ux, vr_uh);
 
 }
 
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index dfe62905e..261a66548 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -67,6 +67,7 @@ public:
   
   // RRC interface
   void max_retx_attempted(){}
+  std::string get_rb_name(uint32_t lcid) { return std::string(""); }
 
   byte_buffer_t *sdus[10];
   int n_sdus;
diff --git a/lib/test/upper/rlc_um_test.cc b/lib/test/upper/rlc_um_test.cc
index 491d816cc..739c9ebf8 100644
--- a/lib/test/upper/rlc_um_test.cc
+++ b/lib/test/upper/rlc_um_test.cc
@@ -71,6 +71,7 @@ public:
   
   // RRC interface
   void max_retx_attempted(){}
+  std::string get_rb_name(uint32_t lcid) { return std::string(""); }
 
   byte_buffer_t *sdus[5];
   int n_sdus;
diff --git a/srsenb/hdr/upper/common_enb.h b/srsenb/hdr/upper/common_enb.h
index 9f15b69ae..d04869c54 100644
--- a/srsenb/hdr/upper/common_enb.h
+++ b/srsenb/hdr/upper/common_enb.h
@@ -43,6 +43,32 @@ namespace srsenb {
 #define SRSENB_N_DRB           8
 #define SRSENB_N_RADIO_BEARERS 11
 
+typedef enum{
+  RB_ID_SRB0 = 0,
+  RB_ID_SRB1,
+  RB_ID_SRB2,
+  RB_ID_DRB1,
+  RB_ID_DRB2,
+  RB_ID_DRB3,
+  RB_ID_DRB4,
+  RB_ID_DRB5,
+  RB_ID_DRB6,
+  RB_ID_DRB7,
+  RB_ID_DRB8,
+  RB_ID_N_ITEMS,
+}rb_id_t;
+static const char rb_id_text[RB_ID_N_ITEMS][20] = { "SRB0",
+                                                    "SRB1",
+                                                    "SRB2",
+                                                    "DRB1",
+                                                    "DRB2",
+                                                    "DRB3",
+                                                    "DRB4",
+                                                    "DRB5",
+                                                    "DRB6",
+                                                    "DRB7",
+                                                    "DRB8"};
+
 // Cat 3 UE - Max number of DL-SCH transport block bits received within a TTI
 // 3GPP 36.306 Table 4.1.1
 #define SRSENB_MAX_BUFFER_SIZE_BITS  102048
diff --git a/srsenb/hdr/upper/pdcp.h b/srsenb/hdr/upper/pdcp.h
index 44c59b8e6..1d047db3e 100644
--- a/srsenb/hdr/upper/pdcp.h
+++ b/srsenb/hdr/upper/pdcp.h
@@ -89,6 +89,7 @@ private:
     void write_pdu_bcch_bch(srslte::byte_buffer_t *pdu);
     void write_pdu_bcch_dlsch(srslte::byte_buffer_t *pdu);
     void write_pdu_pcch(srslte::byte_buffer_t *pdu);
+    std::string get_rb_name(uint32_t lcid);
   };
   
   class user_interface 
diff --git a/srsenb/hdr/upper/rlc.h b/srsenb/hdr/upper/rlc.h
index c979f6af8..d0c165d73 100644
--- a/srsenb/hdr/upper/rlc.h
+++ b/srsenb/hdr/upper/rlc.h
@@ -54,6 +54,7 @@ public:
 
   // rlc_interface_pdcp
   void write_sdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t *sdu);
+  std::string get_rb_name(uint32_t lcid);
   
   // rlc_interface_mac
   int  read_pdu(uint16_t rnti, uint32_t lcid, uint8_t *payload, uint32_t nof_bytes);
@@ -73,6 +74,7 @@ private:
     void write_pdu_bcch_dlsch(srslte::byte_buffer_t *sdu);
     void write_pdu_pcch(srslte::byte_buffer_t *sdu);  
     void max_retx_attempted(); 
+    std::string get_rb_name(uint32_t lcid);
     uint16_t rnti; 
 
     srsenb::pdcp_interface_rlc *pdcp; 
diff --git a/srsenb/src/upper/pdcp.cc b/srsenb/src/upper/pdcp.cc
index 772184fbd..ee65ae9e8 100644
--- a/srsenb/src/upper/pdcp.cc
+++ b/srsenb/src/upper/pdcp.cc
@@ -25,6 +25,7 @@
  */
 
 #include "upper/pdcp.h"
+#include "upper/common_enb.h"
 
 namespace srsenb {
   
@@ -50,7 +51,7 @@ void pdcp::add_user(uint16_t rnti)
 {
   if (users.count(rnti) == 0) {
     srslte::pdcp *obj = new srslte::pdcp;     
-    obj->init(&users[rnti].rlc_itf, &users[rnti].rrc_itf, &users[rnti].gtpu_itf, log_h, SECURITY_DIRECTION_DOWNLINK);
+    obj->init(&users[rnti].rlc_itf, &users[rnti].rrc_itf, &users[rnti].gtpu_itf, log_h, RB_ID_SRB0, SECURITY_DIRECTION_DOWNLINK);
     users[rnti].rlc_itf.rnti  = rnti;
     users[rnti].gtpu_itf.rnti = rnti;
     users[rnti].rrc_itf.rnti  = rnti;
@@ -72,10 +73,16 @@ void pdcp::rem_user(uint16_t rnti)
   }
 }
 
-void pdcp::add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT* cnfg)
+void pdcp::add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT* cnfg_)
 {
   if (users.count(rnti)) {
-    users[rnti].pdcp->add_bearer(lcid, cnfg);
+    srslte::srslte_pdcp_config_t cfg;
+    if(cnfg_->rlc_um_pdcp_sn_size_present) {
+      if(LIBLTE_RRC_PDCP_SN_SIZE_7_BITS == cnfg_->rlc_um_pdcp_sn_size) {
+        cfg.sn_len = 7;
+      }
+    }
+    users[rnti].pdcp->add_bearer(lcid, cfg);
   }
 }
 
@@ -143,6 +150,10 @@ void pdcp::user_interface_rrc::write_pdu_pcch(srslte::byte_buffer_t* pdu)
 {
   fprintf(stderr, "Error: Received PCCH from ue=%d\n", rnti);
 }
-  
-  
+
+std::string pdcp::user_interface_rrc::get_rb_name(uint32_t lcid)
+{
+  return std::string(rb_id_text[lcid]);
+}
+
 }
diff --git a/srsenb/src/upper/rlc.cc b/srsenb/src/upper/rlc.cc
index 1af7a76de..e826e7f34 100644
--- a/srsenb/src/upper/rlc.cc
+++ b/srsenb/src/upper/rlc.cc
@@ -25,6 +25,7 @@
  */
 
 #include "upper/rlc.h"
+#include "upper/common_enb.h"
 
 namespace srsenb {
   
@@ -53,7 +54,7 @@ void rlc::add_user(uint16_t rnti)
 {
   if (users.count(rnti) == 0) {    
     srslte::rlc *obj = new srslte::rlc;     
-    obj->init(&users[rnti], &users[rnti], &users[rnti], log_h, mac_timers);
+    obj->init(&users[rnti], &users[rnti], &users[rnti], log_h, mac_timers, RB_ID_SRB0);
     users[rnti].rnti   = rnti; 
     users[rnti].pdcp   = pdcp; 
     users[rnti].rrc    = rrc; 
@@ -184,4 +185,9 @@ void rlc::user_interface::write_pdu_pcch(srslte::byte_buffer_t* sdu)
   fprintf(stderr, "Error: Received PCCH from ue=%d\n", rnti);
 }
 
+std::string rlc::user_interface::get_rb_name(uint32_t lcid)
+{
+  return std::string(rb_id_text[lcid]);
+}
+
 }
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 259fac714..3bdd46c70 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -27,10 +27,8 @@
 #include "srslte/asn1/liblte_mme.h"
 #include "upper/rrc.h"
 
-using srslte::rb_id_text; 
 using srslte::byte_buffer_t;
 using srslte::bit_buffer_t;
-using srslte::rb_id_t; 
 
 namespace srsenb {
   
@@ -607,11 +605,11 @@ void rrc::run_thread()
     }
     switch(p.lcid)
     {
-    case srslte::RB_ID_SRB0:
+    case RB_ID_SRB0:
       parse_ul_ccch(p.rnti, p.pdu);
       break;
-    case srslte::RB_ID_SRB1:
-    case srslte::RB_ID_SRB2:
+    case RB_ID_SRB1:
+    case RB_ID_SRB2:
       parse_ul_dcch(p.rnti, p.lcid, p.pdu);
       break;
     case LCID_REM_USER:
@@ -919,7 +917,7 @@ void rrc::ue::set_security_key(uint8_t* key, uint32_t length)
                           k_up_enc,
                           k_up_int);
 
-  parent->configure_security(rnti, srslte::RB_ID_SRB1,
+  parent->configure_security(rnti, RB_ID_SRB1,
                              k_rrc_enc, k_rrc_int,
                              k_up_enc,  k_up_int,
                              cipher_algo, integ_algo);
@@ -1432,7 +1430,7 @@ void rrc::ue::send_dl_ccch(LIBLTE_RRC_DL_CCCH_MSG_STRUCT *dl_ccch_msg)
                           rnti,
                           liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg->msg_type]);
     
-    parent->pdcp->write_sdu(rnti, srslte::RB_ID_SRB0, pdu);
+    parent->pdcp->write_sdu(rnti, RB_ID_SRB0, pdu);
     
   } else {
     parent->rrc_log->error("Allocating pdu\n");
@@ -1453,7 +1451,7 @@ void rrc::ue::send_dl_dcch(LIBLTE_RRC_DL_DCCH_MSG_STRUCT *dl_dcch_msg, byte_buff
                           rnti,
                           liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg->msg_type]);
     
-    parent->pdcp->write_sdu(rnti, srslte::RB_ID_SRB1, pdu);
+    parent->pdcp->write_sdu(rnti, RB_ID_SRB1, pdu);
     
   } else {
     parent->rrc_log->error("Allocating pdu\n");
diff --git a/srsenb/test/upper/ip_test.cc b/srsenb/test/upper/ip_test.cc
index 41c430f29..2b789cf27 100644
--- a/srsenb/test/upper/ip_test.cc
+++ b/srsenb/test/upper/ip_test.cc
@@ -155,6 +155,7 @@ public:
   void set_activity_user(uint16_t rnti) {}
   bool is_paging_opportunity(uint32_t tti, uint32_t *payload_len) {return false;}
   void read_pdu_pcch(uint8_t* payload, uint32_t buffer_size) {}
+  std::string get_rb_name(uint32_t lcid) { return std::string("lcid"); }
   
   void write_pdu(uint32_t lcid, srslte::byte_buffer_t *sdu)
   {
@@ -566,7 +567,7 @@ int main(int argc, char *argv[])
   
   my_phy.init(&phy_args, &phy_cfg, &my_radio, &my_mac, &log_phy);
   my_mac.init(&mac_args, &mac_cfg.cell, &my_phy, &my_tester, &my_tester, &log_mac);
-  my_rlc.init(&my_tester, &my_tester, &my_tester, &log_rlc, &my_mac);
+  my_rlc.init(&my_tester, &my_tester, &my_tester, &log_rlc, &my_mac, 0 /* SRB0 */);
   my_tester.init(&my_rlc, &my_mac, &my_phy, &log_tester, prog_args.ip_address);
     
   if (prog_args.enable_gui) {
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 5e3a8b098..ae3dfe246 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -65,7 +65,8 @@ public:
   void init(usim_interface_nas  *usim_,
             rrc_interface_nas   *rrc_,
             gw_interface_nas    *gw_,
-            srslte::log         *nas_log_);
+            srslte::log         *nas_log_,
+            uint32_t            lcid_);
   void stop();
 
   emm_state_t get_state();
@@ -83,6 +84,7 @@ private:
   rrc_interface_nas  *rrc;
   usim_interface_nas *usim;
   gw_interface_nas   *gw;
+  uint32_t           default_lcid;
 
   emm_state_t        state;
    
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index a36fdacc0..c758622fb 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -144,6 +144,23 @@ private:
   void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
   void write_pdu_pcch(byte_buffer_t *pdu);
 
+  // Radio bearers
+  typedef enum{
+    RB_ID_SRB0 = 0,
+    RB_ID_SRB1,
+    RB_ID_SRB2,
+    RB_ID_DRB1,
+    RB_ID_DRB2,
+    RB_ID_DRB3,
+    RB_ID_DRB4,
+    RB_ID_DRB5,
+    RB_ID_DRB6,
+    RB_ID_DRB7,
+    RB_ID_DRB8
+  } rb_id_t;
+  std::map<uint8_t, std::string> bearers;
+  std::string get_rb_name(uint32_t lcid) { return bearers.at(lcid); }
+
   // RLC interface
   void max_retx_attempted();
 
@@ -185,6 +202,7 @@ private:
   void          set_phy_default();
   void          set_mac_default();
   void          set_rrc_default(); 
+  void          set_bearers();
   
 };
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index dea1c3751..0110a1a14 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -161,14 +161,15 @@ bool ue::init(all_args_t *args_)
   phy_log.console("Setting frequency: DL=%.1f Mhz, UL=%.1f MHz\n", args->rf.dl_freq/1e6, args->rf.ul_freq/1e6);
 
   mac.init(&phy, &rlc, &rrc, &mac_log);
-  rlc.init(&pdcp, &rrc, this, &rlc_log, &mac);
-  pdcp.init(&rlc, &rrc, &gw, &pdcp_log, SECURITY_DIRECTION_UPLINK);
+  rlc.init(&pdcp, &rrc, this, &rlc_log, &mac, 0 /* RB_ID_SRB0 */);
+
+  pdcp.init(&rlc, &rrc, &gw, &pdcp_log, 0 /* RB_ID_SRB0 */, SECURITY_DIRECTION_UPLINK);
+
   rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
-  
   rrc.set_ue_category(atoi(args->expert.ue_cateogry.c_str()));
   
-  nas.init(&usim, &rrc, &gw, &nas_log);
-  gw.init(&pdcp, &rrc, this, &gw_log);
+  nas.init(&usim, &rrc, &gw, &nas_log, 1 /* RB_ID_SRB1 */);
+  gw.init(&pdcp, &rrc, this, &gw_log, 3 /* RB_ID_DRB1 */);
   usim.init(&args->usim, &usim_log);
 
   started = true;
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 15dc154ae..440ac77e4 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -43,13 +43,15 @@ nas::nas()
 void nas::init(usim_interface_nas *usim_,
                rrc_interface_nas  *rrc_,
                gw_interface_nas   *gw_,
-               srslte::log        *nas_log_)
+               srslte::log        *nas_log_,
+               uint32_t           lcid_)
 {
   pool    = byte_buffer_pool::get_instance();
   usim    = usim_;
   rrc     = rrc_;
   gw      = gw_;
   nas_log = nas_log_;
+  default_lcid = lcid_;
 }
 
 void nas::stop()
@@ -85,7 +87,7 @@ void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
   uint8 pd;
   uint8 msg_type;
 
-  nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rb_id_text[lcid]);
+  nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rrc->get_rb_name(lcid).c_str());
 
   // Parse the message
   liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT*)pdu, &pd, &msg_type);
@@ -492,8 +494,8 @@ void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu)
                          pdu->N_bytes-5,
                          &pdu->msg[1]);
       nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
-                   count_ul,
-                   rb_id_text[lcid]);
+                    count_ul,
+                    rrc->get_rb_name(lcid).c_str());
       success = true;
     }
   }
@@ -572,7 +574,7 @@ void nas::send_attach_request()
   liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT*)msg);
 
   nas_log->info("Sending attach request\n");
-  rrc->write_sdu(RB_ID_SRB1, msg);
+  rrc->write_sdu(default_lcid, msg);
 }
 
 void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg)
@@ -614,7 +616,7 @@ void nas::send_service_request()
   uint8_t mac[4];
   integrity_generate(&k_nas_int[16],
                       count_ul,
-                      RB_ID_SRB1-1,
+                      default_lcid-1,
                       SECURITY_DIRECTION_UPLINK,
                       &msg->msg[0],
                       2,
@@ -625,7 +627,7 @@ void nas::send_service_request()
   msg->msg[3] = mac[3];
   msg->N_bytes++;
   nas_log->info("Sending service request\n");
-  rrc->write_sdu(RB_ID_SRB1, msg);
+  rrc->write_sdu(default_lcid, msg);
 }
 
 void nas::send_esm_information_response(){}
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 144d98711..0d0bf636c 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -32,6 +32,7 @@
 #include "srslte/phy/utils/bit.h"
 #include "srslte/common/security.h"
 #include "srslte/common/bcd_helpers.h"
+#include "boost/assign.hpp"
 
 #define TIMEOUT_RESYNC_REESTABLISH 100
 
@@ -42,7 +43,9 @@ namespace srsue{
 rrc::rrc()
   :state(RRC_STATE_IDLE)
   ,drb_up(false)
-{}
+{
+  set_bearers();
+}
 
 static void liblte_rrc_handler(void *ctx, char *str) {
   rrc *r = (rrc*) ctx; 
@@ -116,7 +119,7 @@ void rrc::set_ue_category(int category)
 
 void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
 {
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", bearers.at(lcid));
 
   switch(state)
   {
@@ -227,7 +230,7 @@ bool rrc::have_drb()
 
 void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 {
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_text[lcid]);
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", bearers.at(lcid));
   rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
 
   switch(lcid)
@@ -793,7 +796,7 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu)
   liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_dcch_msg);
 
   rrc_log->info("%s - Received %s\n",
-                rb_id_text[lcid],
+                bearers.at(lcid),
                 liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
 
   // Reset and reuse pdu buffer if possible
@@ -1326,9 +1329,10 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
 void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
 {
   // Setup PDCP
-  pdcp->add_bearer(srb_cnfg->srb_id);
-  if(RB_ID_SRB2 == srb_cnfg->srb_id)
+  pdcp->add_bearer(srb_cnfg->srb_id, srslte_pdcp_config_t(true)); // Set PDCP config control flag
+  if(RB_ID_SRB2 == srb_cnfg->srb_id) {
     pdcp->config_security(srb_cnfg->srb_id, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+  }
 
   // Setup RLC
   if(srb_cnfg->rlc_cnfg_present)
@@ -1372,7 +1376,7 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
   }
 
   srbs[srb_cnfg->srb_id] = *srb_cnfg;
-  rrc_log->info("Added radio bearer %s\n", rb_id_text[srb_cnfg->srb_id]);
+  rrc_log->info("Added radio bearer %s\n", bearers.at(srb_cnfg->srb_id));
 }
 
 void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
@@ -1394,7 +1398,14 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   }
   
   // Setup PDCP
-  pdcp->add_bearer(lcid, &drb_cnfg->pdcp_cnfg);
+  srslte_pdcp_config_t pdcp_cfg;
+  pdcp_cfg.is_data = true;
+  if (drb_cnfg->pdcp_cnfg.rlc_um_pdcp_sn_size_present) {
+    if (LIBLTE_RRC_PDCP_SN_SIZE_7_BITS == drb_cnfg->pdcp_cnfg.rlc_um_pdcp_sn_size) {
+      pdcp_cfg.sn_len = 7;
+    }
+  }
+  pdcp->add_bearer(lcid, pdcp_cfg);
   // TODO: setup PDCP security (using k_up_enc)
 
   // Setup RLC
@@ -1426,7 +1437,7 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   
   drbs[lcid] = *drb_cnfg;
   drb_up     = true;
-  rrc_log->info("Added radio bearer %s\n", rb_id_text[lcid]);
+  rrc_log->info("Added radio bearer %s\n", bearers.at(lcid));
 }
 
 void rrc::release_drb(uint8_t lcid)
@@ -1486,4 +1497,19 @@ void rrc::set_rrc_default() {
   mac_timers->get(safe_reset_timer)->set(this, 10);
 }
 
+void rrc::set_bearers()
+{
+  boost::assign::insert(bearers) (RB_ID_SRB0, "SRB0")
+                                 (RB_ID_SRB1, "SRB1")
+                                 (RB_ID_SRB2, "SRB2")
+                                 (RB_ID_DRB1, "DRB1")
+                                 (RB_ID_DRB2, "DRB2")
+                                 (RB_ID_DRB3, "DRB3")
+                                 (RB_ID_DRB4, "DRB4")
+                                 (RB_ID_DRB5, "DRB5")
+                                 (RB_ID_DRB6, "DRB6")
+                                 (RB_ID_DRB7, "DRB7")
+                                 (RB_ID_DRB8, "DRB8");
+}
+
 } // namespace srsue
diff --git a/srsue/test/upper/ip_test.cc b/srsue/test/upper/ip_test.cc
index 93021cd2b..afed88ebc 100644
--- a/srsue/test/upper/ip_test.cc
+++ b/srsue/test/upper/ip_test.cc
@@ -269,6 +269,7 @@ public:
   }
   void write_pdu_pcch(srslte::byte_buffer_t *sdu) {}
   void max_retx_attempted(){}
+  std::string get_rb_name(uint32_t lcid) { return std::string("rb"); }
   void in_sync() {};
   void out_of_sync() {};
 
@@ -543,7 +544,7 @@ int main(int argc, char *argv[])
     
   my_phy.init(&my_radio, &my_mac, &my_tester, &log_phy, NULL);
   my_mac.init(&my_phy, &rlc, &my_tester, &log_mac);
-  rlc.init(&my_tester, &my_tester, &my_tester, &log_rlc, &my_mac);  
+  rlc.init(&my_tester, &my_tester, &my_tester, &log_rlc, &my_mac, 0 /* SRB0 */);
   my_tester.init(&my_phy, &my_mac, &rlc, &log_tester, prog_args.ip_address);
 
   

From 4bc7b19595f24dcbde923743d7cff13181c439c5 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 28 Jun 2017 17:10:19 +0200
Subject: [PATCH 037/269] only call PHR procedure in MUX when it's registered

---
 srsue/src/mac/mux.cc | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/srsue/src/mac/mux.cc b/srsue/src/mac/mux.cc
index 38a71649a..f81bcdfd2 100644
--- a/srsue/src/mac/mux.cc
+++ b/srsue/src/mac/mux.cc
@@ -189,13 +189,17 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz, uint32_t tx_tti, uint32
       bsr_is_inserted  = true; 
     }
   }
+
   // MAC control element for PHR
-  float phr_value; 
-  if (phr_procedure->generate_phr_on_ul_grant(&phr_value)) {
-    if (pdu_msg.new_subh()) {
-      pdu_msg.get()->set_phr(phr_value);
+  if (phr_procedure) {
+    float phr_value;
+    if (phr_procedure->generate_phr_on_ul_grant(&phr_value)) {
+      if (pdu_msg.new_subh()) {
+        pdu_msg.get()->set_phr(phr_value);
+      }
     }
   }
+
   // Update buffer states for all logical channels 
   int sdu_space = pdu_msg.get_sdu_space(); 
   for (uint32_t i=0;i<lch.size();i++) {

From 9398e0eff9c60ad731ff271ee30dc1f5d7bc24a1 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 28 Jun 2017 17:11:47 +0200
Subject: [PATCH 038/269] make number of HARQ processes a parameter for
 mux/demux objects

---
 srsue/hdr/mac/demux.h  |  4 ++--
 srsue/hdr/mac/mux.h    |  8 +++-----
 srsue/src/mac/demux.cc | 10 +++++-----
 srsue/src/mac/mac.cc   |  4 +++-
 srsue/src/mac/mux.cc   |  6 +++---
 5 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/srsue/hdr/mac/demux.h b/srsue/hdr/mac/demux.h
index 8c9fb3a02..2f38a4dfc 100644
--- a/srsue/hdr/mac/demux.h
+++ b/srsue/hdr/mac/demux.h
@@ -41,7 +41,7 @@ namespace srsue {
 class demux : public srslte::pdu_queue::process_callback
 {
 public:
-  demux();
+  demux(uint8_t nof_harq_proc_);
   void init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc, srslte::log* log_h_, srslte::timers* timers_db_);
 
   bool     process_pdus();
@@ -57,7 +57,6 @@ public:
   void     process_pdu(uint8_t *pdu, uint32_t nof_bytes, uint32_t tstamp);
   
 private:
-  const static int NOF_HARQ_PID    = 8; 
   const static int MAX_PDU_LEN     = 150*1024/8; // ~ 150 Mbps  
   const static int NOF_BUFFER_PDUS = 64; // Number of PDU buffers per HARQ pid
   uint8_t bcch_buffer[1024]; // BCCH PID has a dedicated buffer
@@ -77,6 +76,7 @@ private:
   srslte::log              *log_h;
   srslte::timers           *timers_db;
   rlc_interface_mac        *rlc;
+  uint8_t                   nof_harq_proc;
   
   // Buffer of PDUs
   srslte::pdu_queue pdus; 
diff --git a/srsue/hdr/mac/mux.h b/srsue/hdr/mac/mux.h
index b33cf0d19..1167af752 100644
--- a/srsue/hdr/mac/mux.h
+++ b/srsue/hdr/mac/mux.h
@@ -55,7 +55,7 @@ namespace srsue {
 class mux
 {
 public:
-  mux();
+  mux(uint8_t nof_harq_proc_);
   void     reset();
   void     init(rlc_interface_mac *rlc, srslte::log *log_h, bsr_interface_mux *bsr_procedure, phr_proc *phr_procedure_);
 
@@ -87,7 +87,7 @@ private:
   std::vector<lchid_t> lch; 
   
   // Keep track of the PIDs that transmitted BSR reports 
-  bool pid_has_bsr[MAX_HARQ_PROC]; 
+  std::vector<bool> pid_has_bsr;
   
   // Mutex for exclusive access
   pthread_mutex_t mutex; 
@@ -97,6 +97,7 @@ private:
   bsr_interface_mux *bsr_procedure;
   phr_proc          *phr_procedure;
   uint16_t           pending_crnti_ce;
+  uint8_t            nof_harq_proc;
   
   /* Msg3 Buffer */
   static const uint32_t MSG3_BUFF_SZ = 128; 
@@ -105,9 +106,6 @@ private:
   /* PDU Buffer */
   srslte::sch_pdu    pdu_msg; 
   bool msg3_has_been_transmitted;
-  
-  
-  
 };
 
 } // namespace srsue
diff --git a/srsue/src/mac/demux.cc b/srsue/src/mac/demux.cc
index 227bcc364..4fcb402fc 100644
--- a/srsue/src/mac/demux.cc
+++ b/srsue/src/mac/demux.cc
@@ -36,7 +36,7 @@
 
 namespace srsue {
     
-demux::demux() : mac_msg(20), pending_mac_msg(20)
+demux::demux(uint8_t nof_harq_proc_) : mac_msg(20), pending_mac_msg(20), nof_harq_proc(nof_harq_proc_)
 {
 }
 
@@ -68,9 +68,9 @@ void demux::deallocate(uint8_t* payload_buffer_ptr)
 uint8_t* demux::request_buffer(uint32_t pid, uint32_t len)
 {  
   uint8_t *buff = NULL; 
-  if (pid < NOF_HARQ_PID) {
+  if (pid < nof_harq_proc) {
     return pdus.request(len);
-  } else if (pid == NOF_HARQ_PID) {
+  } else if (pid == nof_harq_proc) {
     buff = bcch_buffer;
   } else {
     Error("Requested buffer for invalid PID=%d\n", pid);
@@ -119,9 +119,9 @@ void demux::push_pdu_temp_crnti(uint8_t *buff, uint32_t nof_bytes)
  */ 
 void demux::push_pdu(uint32_t pid, uint8_t *buff, uint32_t nof_bytes, uint32_t tstamp)
 {
-  if (pid < NOF_HARQ_PID) {    
+  if (pid < nof_harq_proc) {
     return pdus.push(buff, nof_bytes, tstamp);
-  } else if (pid == NOF_HARQ_PID) {
+  } else if (pid == nof_harq_proc) {
     /* Demultiplexing of MAC PDU associated with SI-RNTI. The PDU passes through 
     * the MAC in transparent mode. 
     * Warning: In this case function sends the message to RLC now, since SI blocks do not 
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 9fe08166f..e1426780e 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -42,7 +42,9 @@
 namespace srsue {
 
 mac::mac() : ttisync(10240), 
-             timers_db((uint32_t) NOF_MAC_TIMERS), 
+             timers_db((uint32_t) NOF_MAC_TIMERS),
+             mux_unit(MAC_NOF_HARQ_PROC),
+             demux_unit(MAC_NOF_HARQ_PROC),
              pdu_process_thread(&demux_unit)
 {
   started = false;  
diff --git a/srsue/src/mac/mux.cc b/srsue/src/mac/mux.cc
index f81bcdfd2..5ab58fb50 100644
--- a/srsue/src/mac/mux.cc
+++ b/srsue/src/mac/mux.cc
@@ -37,7 +37,7 @@
 
 namespace srsue {
 
-mux::mux() : pdu_msg(MAX_NOF_SUBHEADERS)
+mux::mux(uint8_t nof_harq_proc_) : pdu_msg(MAX_NOF_SUBHEADERS), pid_has_bsr(nof_harq_proc_), nof_harq_proc(nof_harq_proc_)
 {
   pthread_mutex_init(&mutex, NULL);
   
@@ -141,7 +141,7 @@ srslte::sch_subh::cetype bsr_format_convert(bsr_proc::bsr_format_t format) {
 
 void mux::pusch_retx(uint32_t tx_tti, uint32_t pid)
 {
-  if (pid_has_bsr[pid%MAX_HARQ_PROC]) {
+  if (pid_has_bsr[pid%nof_harq_proc]) {
     bsr_procedure->set_tx_tti(tx_tti);
   }
 }
@@ -256,7 +256,7 @@ uint8_t* mux::pdu_get(uint8_t *payload, uint32_t pdu_sz, uint32_t tx_tti, uint32
   /* Generate MAC PDU and save to buffer */
   uint8_t *ret = pdu_msg.write_packet(log_h);   
 
-  pid_has_bsr[pid%MAX_HARQ_PROC] = bsr_is_inserted; 
+  pid_has_bsr[pid%nof_harq_proc] = bsr_is_inserted;
   if (bsr_is_inserted) {
     bsr_procedure->set_tx_tti(tx_tti);
   }

From 7ca0988ea363cec4e0f6117d3248cedc316f3034 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Tue, 4 Jul 2017 16:29:33 +0200
Subject: [PATCH 039/269] refactor RLC to use RAT-agnostic config

---
 .../srslte/interfaces/enb_interfaces.h        |   3 +-
 lib/include/srslte/interfaces/ue_interfaces.h |   3 +-
 lib/include/srslte/upper/rlc.h                |   3 +-
 lib/include/srslte/upper/rlc_am.h             |  12 +-
 lib/include/srslte/upper/rlc_common.h         |  12 +-
 lib/include/srslte/upper/rlc_entity.h         |   2 +-
 lib/include/srslte/upper/rlc_interface.h      | 126 ++++++++++++++++++
 lib/include/srslte/upper/rlc_tm.h             |   2 +-
 lib/include/srslte/upper/rlc_um.h             |   9 +-
 lib/src/upper/rlc.cc                          |  13 +-
 lib/src/upper/rlc_am.cc                       |  33 ++---
 lib/src/upper/rlc_entity.cc                   |   2 +-
 lib/src/upper/rlc_tm.cc                       |   2 +-
 lib/src/upper/rlc_um.cc                       |  63 ++++-----
 lib/test/upper/rlc_am_test.cc                 |  10 ++
 srsenb/hdr/upper/rlc.h                        |   2 +-
 srsenb/src/upper/rlc.cc                       |   2 +-
 srsue/src/upper/rrc.cc                        |   4 +-
 18 files changed, 201 insertions(+), 102 deletions(-)
 create mode 100644 lib/include/srslte/upper/rlc_interface.h

diff --git a/lib/include/srslte/interfaces/enb_interfaces.h b/lib/include/srslte/interfaces/enb_interfaces.h
index 8806ded58..5ba0213c1 100644
--- a/lib/include/srslte/interfaces/enb_interfaces.h
+++ b/lib/include/srslte/interfaces/enb_interfaces.h
@@ -29,6 +29,7 @@
 #include "srslte/common/common.h"
 #include "srslte/common/security.h"
 #include "srslte/interfaces/sched_interface.h"
+#include "srslte/upper/rlc_interface.h"
 #include "srslte/asn1/liblte_rrc.h"
 #include "srslte/asn1/liblte_s1ap.h"
 
@@ -156,7 +157,7 @@ public:
   virtual void add_user(uint16_t rnti) = 0; 
   virtual void rem_user(uint16_t rnti) = 0; 
   virtual void add_bearer(uint16_t rnti, uint32_t lcid) = 0;
-  virtual void add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg) = 0;
+  virtual void add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_rlc_config_t cnfg) = 0;
 };
 
 // PDCP interface for GTPU
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index ca9b4540b..81ed67b74 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -39,6 +39,7 @@
 #include "srslte/common/interfaces_common.h"
 #include "srslte/common/common.h"
 #include "srslte/common/security.h"
+#include "srslte/upper/rlc_interface.h"
 
 namespace srsue {
 
@@ -201,7 +202,7 @@ class rlc_interface_rrc
 public:
   virtual void reset() = 0;
   virtual void add_bearer(uint32_t lcid) = 0;
-  virtual void add_bearer(uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg) = 0;
+  virtual void add_bearer(uint32_t lcid, srslte::srslte_rlc_config_t cnfg) = 0;
 };
 
 // RLC interface for PDCP
diff --git a/lib/include/srslte/upper/rlc.h b/lib/include/srslte/upper/rlc.h
index 4a95f1a46..11ce507d5 100644
--- a/lib/include/srslte/upper/rlc.h
+++ b/lib/include/srslte/upper/rlc.h
@@ -34,6 +34,7 @@
 #include "srslte/common/msg_queue.h"
 #include "srslte/upper/rlc_entity.h"
 #include "srslte/upper/rlc_metrics.h"
+#include "srslte/upper/rlc_common.h"
 
 namespace srslte {
 
@@ -77,7 +78,7 @@ public:
   // RRC interface
   void reset();
   void add_bearer(uint32_t lcid);
-  void add_bearer(uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void add_bearer(uint32_t lcid, srslte_rlc_config_t cnfg);
 
 private:
   void reset_metrics(); 
diff --git a/lib/include/srslte/upper/rlc_am.h b/lib/include/srslte/upper/rlc_am.h
index 5fcf31c94..a92c9a0ad 100644
--- a/lib/include/srslte/upper/rlc_am.h
+++ b/lib/include/srslte/upper/rlc_am.h
@@ -76,7 +76,7 @@ public:
             srsue::pdcp_interface_rlc   *pdcp_,
             srsue::rrc_interface_rlc    *rrc_,
             mac_interface_timers *mac_timers);
-  void configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void configure(srslte_rlc_config_t cnfg);
   void reset();
   void empty_queue(); 
   
@@ -128,15 +128,7 @@ private:
    * Ref: 3GPP TS 36.322 v10.0.0 Section 7
    ***************************************************************************/
 
-  // TX configs
-  int32_t    t_poll_retx;      // Poll retx timeout (ms)
-  int32_t    poll_pdu;         // Insert poll bit after this many PDUs
-  int32_t    poll_byte;        // Insert poll bit after this much data (KB)
-  uint32_t   max_retx_thresh;  // Max number of retx
-
-  // RX configs
-  int32_t   t_reordering;       // Timer used by rx to detect PDU loss  (ms)
-  int32_t   t_status_prohibit;  // Timer used by rx to prohibit tx of status PDU (ms)
+  srslte_rlc_am_config_t cfg;
 
   /****************************************************************************
    * State variables and counters
diff --git a/lib/include/srslte/upper/rlc_common.h b/lib/include/srslte/upper/rlc_common.h
index 0c545b60f..c68e3e6f3 100644
--- a/lib/include/srslte/upper/rlc_common.h
+++ b/lib/include/srslte/upper/rlc_common.h
@@ -27,6 +27,8 @@
 #ifndef RLC_COMMON_H
 #define RLC_COMMON_H
 
+#include "srslte/upper/rlc_interface.h"
+
 namespace srslte {
 
 /****************************************************************************
@@ -66,14 +68,6 @@ typedef enum{
 static const char rlc_dc_field_text[RLC_DC_FIELD_N_ITEMS][20] = {"Control PDU",
                                                                  "Data PDU"};
 
-typedef enum{
-  RLC_UMD_SN_SIZE_5_BITS = 0,
-  RLC_UMD_SN_SIZE_10_BITS,
-  RLC_UMD_SN_SIZE_N_ITEMS,
-}rlc_umd_sn_size_t;
-static const char     rlc_umd_sn_size_text[RLC_UMD_SN_SIZE_N_ITEMS][20] = {"5 bits", "10 bits"};
-static const uint16_t rlc_umd_sn_size_num[RLC_UMD_SN_SIZE_N_ITEMS] = {5, 10};
-
 // UMD PDU Header
 typedef struct{
   uint8_t           fi;                     // Framing info
@@ -162,7 +156,7 @@ public:
                     srsue::pdcp_interface_rlc         *pdcp_,
                     srsue::rrc_interface_rlc          *rrc_,
                     srslte::mac_interface_timers      *mac_timers_) = 0;
-  virtual void configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg) = 0;
+  virtual void configure(srslte_rlc_config_t cnfg) = 0;
   virtual void reset() = 0;
   virtual void empty_queue() = 0; 
 
diff --git a/lib/include/srslte/upper/rlc_entity.h b/lib/include/srslte/upper/rlc_entity.h
index cf09c19d2..3747f9410 100644
--- a/lib/include/srslte/upper/rlc_entity.h
+++ b/lib/include/srslte/upper/rlc_entity.h
@@ -54,7 +54,7 @@ public:
             srsue::rrc_interface_rlc    *rrc_,
             mac_interface_timers *mac_timers_);
 
-  void configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void configure(srslte_rlc_config_t cnfg);
   void reset();
   bool active();
 
diff --git a/lib/include/srslte/upper/rlc_interface.h b/lib/include/srslte/upper/rlc_interface.h
new file mode 100644
index 000000000..feddac1cc
--- /dev/null
+++ b/lib/include/srslte/upper/rlc_interface.h
@@ -0,0 +1,126 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef RLC_INTERFACE_H
+#define RLC_INTERFACE_H
+
+// for custom constructors
+#include "srslte/asn1/liblte_rrc.h"
+
+namespace srslte {
+
+
+typedef enum{
+  RLC_UMD_SN_SIZE_5_BITS = 0,
+  RLC_UMD_SN_SIZE_10_BITS,
+  RLC_UMD_SN_SIZE_N_ITEMS,
+}rlc_umd_sn_size_t;
+static const char     rlc_umd_sn_size_text[RLC_UMD_SN_SIZE_N_ITEMS][20] = {"5 bits", "10 bits"};
+static const uint16_t rlc_umd_sn_size_num[RLC_UMD_SN_SIZE_N_ITEMS] = {5, 10};
+
+
+typedef struct {
+  /****************************************************************************
+   * Configurable parameters
+   * Ref: 3GPP TS 36.322 v10.0.0 Section 7
+   ***************************************************************************/
+
+  // TX configs
+  int32_t    t_poll_retx;      // Poll retx timeout (ms)
+  int32_t    poll_pdu;         // Insert poll bit after this many PDUs
+  int32_t    poll_byte;        // Insert poll bit after this much data (KB)
+  uint32_t   max_retx_thresh;  // Max number of retx
+
+  // RX configs
+  int32_t   t_reordering;       // Timer used by rx to detect PDU loss  (ms)
+  int32_t   t_status_prohibit;  // Timer used by rx to prohibit tx of status PDU (ms)
+} srslte_rlc_am_config_t;
+
+
+typedef struct {
+  /****************************************************************************
+  * Configurable parameters
+  * Ref: 3GPP TS 36.322 v10.0.0 Section 7
+  ***************************************************************************/
+
+  int32_t           t_reordering;       // Timer used by rx to detect PDU loss  (ms)
+  rlc_umd_sn_size_t tx_sn_field_length; // Number of bits used for tx (UL) sequence number
+  rlc_umd_sn_size_t rx_sn_field_length; // Number of bits used for rx (DL) sequence number
+
+  uint32_t          rx_window_size;
+  uint32_t          rx_mod;             // Rx counter modulus
+  uint32_t          tx_mod;             // Tx counter modulus
+} srslte_rlc_um_config_t;
+
+
+class srslte_rlc_config_t
+{
+public:
+  LIBLTE_RRC_RLC_MODE_ENUM  rlc_mode;
+  srslte_rlc_am_config_t    am;
+  srslte_rlc_um_config_t    um;
+
+  // Constructor based on liblte's RLC config
+  srslte_rlc_config_t(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg) : rlc_mode(cnfg->rlc_mode), am(), um()
+  {
+    switch(rlc_mode)
+    {
+      case LIBLTE_RRC_RLC_MODE_AM:
+        am.t_poll_retx       = liblte_rrc_t_poll_retransmit_num[cnfg->ul_am_rlc.t_poll_retx];
+        am.poll_pdu          = liblte_rrc_poll_pdu_num[cnfg->ul_am_rlc.poll_pdu];
+        am.poll_byte         = liblte_rrc_poll_byte_num[cnfg->ul_am_rlc.poll_byte]*1000; // KB
+        am.max_retx_thresh   = liblte_rrc_max_retx_threshold_num[cnfg->ul_am_rlc.max_retx_thresh];
+        am.t_reordering      = liblte_rrc_t_reordering_num[cnfg->dl_am_rlc.t_reordering];
+        am.t_status_prohibit = liblte_rrc_t_status_prohibit_num[cnfg->dl_am_rlc.t_status_prohibit];
+        break;
+      case LIBLTE_RRC_RLC_MODE_UM_BI:
+        um.t_reordering        = liblte_rrc_t_reordering_num[cnfg->dl_um_bi_rlc.t_reordering];
+        um.rx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->dl_um_bi_rlc.sn_field_len;
+        um.rx_window_size      = (RLC_UMD_SN_SIZE_5_BITS == um.rx_sn_field_length) ? 16 : 512;
+        um.rx_mod              = (RLC_UMD_SN_SIZE_5_BITS == um.rx_sn_field_length) ? 32 : 1024;
+        um.tx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->ul_um_bi_rlc.sn_field_len;
+        um.tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == um.tx_sn_field_length) ? 32 : 1024;
+        break;
+      case LIBLTE_RRC_RLC_MODE_UM_UNI_UL:
+        um.tx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->ul_um_uni_rlc.sn_field_len;
+        um.tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == um.tx_sn_field_length) ? 32 : 1024;
+        break;
+      case LIBLTE_RRC_RLC_MODE_UM_UNI_DL:
+        um.t_reordering        = liblte_rrc_t_reordering_num[cnfg->dl_um_uni_rlc.t_reordering];
+        um.rx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->dl_um_uni_rlc.sn_field_len;
+        um.rx_window_size      = (RLC_UMD_SN_SIZE_5_BITS == um.rx_sn_field_length) ? 16 : 512;
+        um.rx_mod              = (RLC_UMD_SN_SIZE_5_BITS == um.rx_sn_field_length) ? 32 : 1024;
+        break;
+      default:
+        // Handle default case
+        break;
+    }
+  }
+};
+
+} // namespace srslte
+
+#endif // RLC_INTERFACE_H
diff --git a/lib/include/srslte/upper/rlc_tm.h b/lib/include/srslte/upper/rlc_tm.h
index d8034cd38..def968a92 100644
--- a/lib/include/srslte/upper/rlc_tm.h
+++ b/lib/include/srslte/upper/rlc_tm.h
@@ -46,7 +46,7 @@ public:
             srsue::pdcp_interface_rlc   *pdcp_,
             srsue::rrc_interface_rlc    *rrc_,
             mac_interface_timers *mac_timers);
-  void configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void configure(srslte_rlc_config_t cnfg);
   void reset();
   void empty_queue(); 
 
diff --git a/lib/include/srslte/upper/rlc_um.h b/lib/include/srslte/upper/rlc_um.h
index 8e95dcb71..4325bd029 100644
--- a/lib/include/srslte/upper/rlc_um.h
+++ b/lib/include/srslte/upper/rlc_um.h
@@ -56,7 +56,7 @@ public:
             srsue::pdcp_interface_rlc   *pdcp_,
             srsue::rrc_interface_rlc    *rrc_,
             mac_interface_timers *mac_timers_);
-  void configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void configure(srslte_rlc_config_t cnfg);
   void reset();
   void empty_queue(); 
 
@@ -92,9 +92,6 @@ private:
 
   // Rx window
   std::map<uint32_t, rlc_umd_pdu_t>  rx_window;
-  uint32_t                           rx_window_size;
-  uint32_t                           rx_mod; // Rx counter modulus
-  uint32_t                           tx_mod; // Tx counter modulus
 
   // RX SDU buffers
   byte_buffer_t      *rx_sdu;
@@ -108,9 +105,7 @@ private:
    * Ref: 3GPP TS 36.322 v10.0.0 Section 7
    ***************************************************************************/
 
-  int32_t           t_reordering;       // Timer used by rx to detect PDU loss  (ms)
-  rlc_umd_sn_size_t tx_sn_field_length; // Number of bits used for tx (UL) sequence number
-  rlc_umd_sn_size_t rx_sn_field_length; // Number of bits used for rx (DL) sequence number
+  srslte_rlc_um_config_t cfg;
 
   /****************************************************************************
    * State variables and counters
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index c840de10d..378b539b9 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -194,10 +194,10 @@ void rlc::write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes)
 void rlc::add_bearer(uint32_t lcid)
 {
   // No config provided - use defaults for lcid
-  LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
   if(default_lcid == lcid || (default_lcid+1) == lcid)
   {
     if (!rlc_array[lcid].active()) {
+      LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
       cnfg.rlc_mode                     = LIBLTE_RRC_RLC_MODE_AM;
       cnfg.ul_am_rlc.t_poll_retx        = LIBLTE_RRC_T_POLL_RETRANSMIT_MS45;
       cnfg.ul_am_rlc.poll_pdu           = LIBLTE_RRC_POLL_PDU_INFINITY;
@@ -205,7 +205,7 @@ void rlc::add_bearer(uint32_t lcid)
       cnfg.ul_am_rlc.max_retx_thresh    = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
       cnfg.dl_am_rlc.t_reordering       = LIBLTE_RRC_T_REORDERING_MS35;
       cnfg.dl_am_rlc.t_status_prohibit  = LIBLTE_RRC_T_STATUS_PROHIBIT_MS0;
-      add_bearer(lcid, &cnfg);
+      add_bearer(lcid, srslte_rlc_config_t(&cnfg));
     } else {
       rlc_log->warning("Bearer %s already configured. Reconfiguration not supported\n", get_rb_name(lcid).c_str());
     }
@@ -215,18 +215,17 @@ void rlc::add_bearer(uint32_t lcid)
   }
 }
 
-void rlc::add_bearer(uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
+void rlc::add_bearer(uint32_t lcid, srslte_rlc_config_t cnfg)
 {
   if(lcid < 0 || lcid >= SRSLTE_N_RADIO_BEARERS) {
     rlc_log->error("Radio bearer id must be in [0:%d] - %d\n", SRSLTE_N_RADIO_BEARERS, lcid);
     return;
   }
-  
-  
+
   if (!rlc_array[lcid].active()) {
     rlc_log->info("Adding radio bearer %s with mode %s\n",
-                  get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode]);
-    switch(cnfg->rlc_mode)
+                  get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg.rlc_mode]);
+    switch(cnfg.rlc_mode)
     {
     case LIBLTE_RRC_RLC_MODE_AM:
       rlc_array[lcid].init(RLC_MODE_AM, rlc_log, lcid, pdcp, rrc, mac_timers);
diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index d25b2d2ef..f0055a534 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -74,20 +74,13 @@ void rlc_am::init(srslte::log                 *log_,
   rrc  = rrc_;
 }
 
-void rlc_am::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
+void rlc_am::configure(srslte_rlc_config_t cfg_)
 {
-  t_poll_retx       = liblte_rrc_t_poll_retransmit_num[cnfg->ul_am_rlc.t_poll_retx];
-  poll_pdu          = liblte_rrc_poll_pdu_num[cnfg->ul_am_rlc.poll_pdu];
-  poll_byte         = liblte_rrc_poll_byte_num[cnfg->ul_am_rlc.poll_byte]*1000; // KB
-  max_retx_thresh   = liblte_rrc_max_retx_threshold_num[cnfg->ul_am_rlc.max_retx_thresh];
-
-  t_reordering      = liblte_rrc_t_reordering_num[cnfg->dl_am_rlc.t_reordering];
-  t_status_prohibit = liblte_rrc_t_status_prohibit_num[cnfg->dl_am_rlc.t_status_prohibit];
-
+  cfg = cfg_.am;
   log->info("%s configured: t_poll_retx=%d, poll_pdu=%d, poll_byte=%d, max_retx_thresh=%d, "
             "t_reordering=%d, t_status_prohibit=%d\n",
-            rrc->get_rb_name(lcid).c_str(), t_poll_retx, poll_pdu, poll_byte, max_retx_thresh,
-            t_reordering, t_status_prohibit);
+            rrc->get_rb_name(lcid).c_str(), cfg.t_poll_retx, cfg.poll_pdu, cfg.poll_byte, cfg.max_retx_thresh,
+            cfg.t_reordering, cfg.t_status_prohibit);
 }
 
 
@@ -362,7 +355,7 @@ void rlc_am::check_reordering_timeout()
 
     if(RX_MOD_BASE(vr_h) > RX_MOD_BASE(vr_ms))
     {
-      reordering_timeout.start(t_reordering);
+      reordering_timeout.start(cfg.t_reordering);
       vr_x = vr_h;
     }
 
@@ -376,9 +369,9 @@ void rlc_am::check_reordering_timeout()
 
 bool rlc_am::poll_required()
 {
-  if(poll_pdu > 0 && pdu_without_poll > (uint32_t)poll_pdu)
+  if(cfg.poll_pdu > 0 && pdu_without_poll > (uint32_t)cfg.poll_pdu)
     return true;
-  if(poll_byte > 0 && byte_without_poll > (uint32_t)poll_byte)
+  if(cfg.poll_byte > 0 && byte_without_poll > (uint32_t)cfg.poll_byte)
     return true;
   if(poll_retx())
     return true;
@@ -414,8 +407,8 @@ int  rlc_am::build_status_pdu(uint8_t *payload, uint32_t nof_bytes)
     do_status     = false;
     poll_received = false;
 
-    if(t_status_prohibit > 0)
-      status_prohibit_timeout.start(t_status_prohibit);
+    if(cfg.t_status_prohibit > 0)
+      status_prohibit_timeout.start(cfg.t_status_prohibit);
     debug_state();
     return rlc_am_write_status_pdu(&status, payload);
   }else{
@@ -450,7 +443,7 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
     poll_sn           = vt_s;
     pdu_without_poll  = 0;
     byte_without_poll = 0;
-    poll_retx_timeout.start(t_poll_retx);
+    poll_retx_timeout.start(cfg.t_poll_retx);
   }
 
   uint8_t *ptr = payload;
@@ -459,7 +452,7 @@ int  rlc_am::build_retx_pdu(uint8_t *payload, uint32_t nof_bytes)
 
   retx_queue.pop_front();
   tx_window[retx.sn].retx_count++;
-  if(tx_window[retx.sn].retx_count >= max_retx_thresh)
+  if(tx_window[retx.sn].retx_count >= cfg.max_retx_thresh)
     rrc->max_retx_attempted();
   log->info("%s Retx PDU scheduled for tx. SN: %d, retx count: %d\n",
             rrc->get_rb_name(lcid).c_str(), retx.sn, tx_window[retx.sn].retx_count);
@@ -701,7 +694,7 @@ int  rlc_am::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
     poll_sn           = vt_s;
     pdu_without_poll  = 0;
     byte_without_poll = 0;
-    poll_retx_timeout.start(t_poll_retx);
+    poll_retx_timeout.start(cfg.t_poll_retx);
   }
 
   // Set SN
@@ -812,7 +805,7 @@ void rlc_am::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes, rlc_amd_pdu_h
   {
     if(RX_MOD_BASE(vr_h) > RX_MOD_BASE(vr_r))
     {
-      reordering_timeout.start(t_reordering);
+      reordering_timeout.start(cfg.t_reordering);
       vr_x = vr_h;
     }
   }
diff --git a/lib/src/upper/rlc_entity.cc b/lib/src/upper/rlc_entity.cc
index ace991b10..541649897 100644
--- a/lib/src/upper/rlc_entity.cc
+++ b/lib/src/upper/rlc_entity.cc
@@ -64,7 +64,7 @@ void rlc_entity::init(rlc_mode_t                   mode,
   rlc->init(rlc_entity_log_, lcid_, pdcp_, rrc_, mac_timers_);
 }
 
-void rlc_entity::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
+void rlc_entity::configure(srslte_rlc_config_t cnfg)
 {
   if(rlc)
     rlc->configure(cnfg);
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index b97b78be7..9b7cd8f00 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -46,7 +46,7 @@ void rlc_tm::init(srslte::log               *log_,
   rrc  = rrc_;
 }
 
-void rlc_tm::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
+void rlc_tm::configure(srslte_rlc_config_t cnfg)
 {
   log->error("Attempted to configure TM RLC entity");
 }
diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index 8724e0f28..133bca9a8 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -27,7 +27,7 @@
 
 #include "srslte/upper/rlc_um.h"
 
-#define RX_MOD_BASE(x) (x-vr_uh-rx_window_size)%rx_mod
+#define RX_MOD_BASE(x) (x-vr_uh-cfg.rx_window_size)%cfg.rx_mod
 
 namespace srslte {
 
@@ -65,41 +65,28 @@ void rlc_um::init(srslte::log                 *log_,
   reordering_timeout_id = mac_timers->get_unique_id();
 }
 
-void rlc_um::configure(LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg)
+void rlc_um::configure(srslte_rlc_config_t cnfg_)
 {
-  switch(cnfg->rlc_mode)
+  cfg = cnfg_.um;
+
+  switch(cnfg_.rlc_mode)
   {
   case LIBLTE_RRC_RLC_MODE_UM_BI:
-    t_reordering        = liblte_rrc_t_reordering_num[cnfg->dl_um_bi_rlc.t_reordering];
-    rx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->dl_um_bi_rlc.sn_field_len;
-    rx_window_size      = (RLC_UMD_SN_SIZE_5_BITS == rx_sn_field_length) ? 16 : 512;
-    rx_mod              = (RLC_UMD_SN_SIZE_5_BITS == rx_sn_field_length) ? 32 : 1024;
-    tx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->ul_um_bi_rlc.sn_field_len;
-    tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == tx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits, tx_sn_field_length=%u bits\n",
-              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
-              t_reordering,
-              rlc_umd_sn_size_num[rx_sn_field_length],
-              rlc_umd_sn_size_num[tx_sn_field_length]);
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
+              cfg.t_reordering, cfg.rx_sn_field_length, cfg.tx_sn_field_length);
     break;
   case LIBLTE_RRC_RLC_MODE_UM_UNI_UL:
-    tx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->ul_um_uni_rlc.sn_field_len;
-    tx_mod              = (RLC_UMD_SN_SIZE_5_BITS == tx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: tx_sn_field_length=%u bits\n",
-              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
-              rlc_umd_sn_size_num[tx_sn_field_length]);
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
+              cfg.tx_sn_field_length);
     break;
   case LIBLTE_RRC_RLC_MODE_UM_UNI_DL:
-    t_reordering        = liblte_rrc_t_reordering_num[cnfg->dl_um_uni_rlc.t_reordering];
-    rx_sn_field_length  = (rlc_umd_sn_size_t)cnfg->dl_um_uni_rlc.sn_field_len;
-    rx_window_size      = (RLC_UMD_SN_SIZE_5_BITS == rx_sn_field_length) ? 16 : 512;
-    rx_mod              = (RLC_UMD_SN_SIZE_5_BITS == rx_sn_field_length) ? 32 : 1024;
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits\n",
-              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg->rlc_mode],
-              liblte_rrc_t_reordering_num[t_reordering],
-              rlc_umd_sn_size_num[rx_sn_field_length]);
+              rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
+              cfg.t_reordering, cfg.rx_sn_field_length);
     break;
   default:
     log->error("RLC configuration mode not recognized\n");
@@ -229,7 +216,7 @@ void rlc_um::timer_expired(uint32_t timeout_id)
     rx_sdu->reset();
     while(RX_MOD_BASE(vr_ur) < RX_MOD_BASE(vr_ux))
     {
-      vr_ur = (vr_ur + 1)%rx_mod;
+      vr_ur = (vr_ur + 1)%cfg.rx_mod;
       log->debug("Entering Reassemble from timeout id=%d\n", timeout_id);
       reassemble_rx_sdus();
       log->debug("Finished reassemble from timeout id=%d\n", timeout_id);
@@ -237,7 +224,7 @@ void rlc_um::timer_expired(uint32_t timeout_id)
     mac_timers->get(reordering_timeout_id)->stop();
     if(RX_MOD_BASE(vr_uh) > RX_MOD_BASE(vr_ur))
     {
-      mac_timers->get(reordering_timeout_id)->set(this, t_reordering);
+      mac_timers->get(reordering_timeout_id)->set(this, cfg.t_reordering);
       mac_timers->get(reordering_timeout_id)->run();
       vr_ux = vr_uh;
     }
@@ -274,7 +261,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   header.fi   = RLC_FI_FIELD_START_AND_END_ALIGNED;
   header.sn   = vt_us;
   header.N_li = 0;
-  header.sn_size = tx_sn_field_length;
+  header.sn_size = cfg.tx_sn_field_length;
 
   uint32_t to_move   = 0;
   uint32_t last_li   = 0;
@@ -347,7 +334,7 @@ int  rlc_um::build_data_pdu(uint8_t *payload, uint32_t nof_bytes)
 
   // Set SN
   header.sn = vt_us;
-  vt_us = (vt_us + 1)%tx_mod;
+  vt_us = (vt_us + 1)%cfg.tx_mod;
 
   // Add header and TX
   log->debug("%s packing PDU with length %d\n", rrc->get_rb_name(lcid).c_str(), pdu->N_bytes);
@@ -365,12 +352,12 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
 {
   std::map<uint32_t, rlc_umd_pdu_t>::iterator it;
   rlc_umd_pdu_header_t header;
-  rlc_um_read_data_pdu_header(payload, nof_bytes, rx_sn_field_length, &header);
+  rlc_um_read_data_pdu_header(payload, nof_bytes, cfg.rx_sn_field_length, &header);
 
   log->info_hex(payload, nof_bytes, "RX %s Rx data PDU SN: %d",
                 rrc->get_rb_name(lcid).c_str(), header.sn);
 
-  if(RX_MOD_BASE(header.sn) >= RX_MOD_BASE(vr_uh-rx_window_size) &&
+  if(RX_MOD_BASE(header.sn) >= RX_MOD_BASE(vr_uh-cfg.rx_window_size) &&
      RX_MOD_BASE(header.sn) <  RX_MOD_BASE(vr_ur))
   {
     log->info("%s SN: %d outside rx window [%d:%d] - discarding\n",
@@ -403,7 +390,7 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   
   // Update vr_uh
   if(!inside_reordering_window(header.sn))
-    vr_uh  = (header.sn + 1)%rx_mod;
+    vr_uh  = (header.sn + 1)%cfg.rx_mod;
 
   // Reassemble and deliver SDUs, while updating vr_ur
   log->debug("Entering Reassemble from received PDU\n");
@@ -423,7 +410,7 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   {
     if(RX_MOD_BASE(vr_uh) > RX_MOD_BASE(vr_ur))
     {
-      mac_timers->get(reordering_timeout_id)->set(this, t_reordering);
+      mac_timers->get(reordering_timeout_id)->set(this, cfg.t_reordering);
       mac_timers->get(reordering_timeout_id)->run();
       vr_ux = vr_uh;
     }
@@ -452,7 +439,7 @@ void rlc_um::reassemble_rx_sdus()
         rx_sdu->N_bytes += len;
         rx_window[vr_ur].buf->msg += len;
         rx_window[vr_ur].buf->N_bytes -= len;
-        if((pdu_lost && !rlc_um_start_aligned(rx_window[vr_ur].header.fi)) || (vr_ur != ((vr_ur_in_rx_sdu+1)%rx_mod))) {
+        if((pdu_lost && !rlc_um_start_aligned(rx_window[vr_ur].header.fi)) || (vr_ur != ((vr_ur_in_rx_sdu+1)%cfg.rx_mod))) {
           log->warning("Dropping remainder of lost PDU (lower edge middle segments, vr_ur=%d, vr_ur_in_rx_sdu=%d)\n", vr_ur, vr_ur_in_rx_sdu);
           rx_sdu->reset();
         } else {
@@ -489,7 +476,7 @@ void rlc_um::reassemble_rx_sdus()
       rx_window.erase(vr_ur);
     }
 
-    vr_ur = (vr_ur + 1)%rx_mod;
+    vr_ur = (vr_ur + 1)%cfg.rx_mod;
   }
 
 
@@ -502,11 +489,11 @@ void rlc_um::reassemble_rx_sdus()
       int len = rx_window[vr_ur].header.li[i];
       memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_ur].buf->msg, len);
       log->debug("Concatenating %d bytes in to current length %d. rx_window remaining bytes=%d, vr_ur_in_rx_sdu=%d, vr_ur=%d, rx_mod=%d, last_mod=%d\n",
-        len, rx_sdu->N_bytes, rx_window[vr_ur].buf->N_bytes, vr_ur_in_rx_sdu, vr_ur, rx_mod, (vr_ur_in_rx_sdu+1)%rx_mod);
+        len, rx_sdu->N_bytes, rx_window[vr_ur].buf->N_bytes, vr_ur_in_rx_sdu, vr_ur, cfg.rx_mod, (vr_ur_in_rx_sdu+1)%cfg.rx_mod);
       rx_sdu->N_bytes += len;      
       rx_window[vr_ur].buf->msg += len;
       rx_window[vr_ur].buf->N_bytes -= len;
-      if((pdu_lost && !rlc_um_start_aligned(rx_window[vr_ur].header.fi)) || (vr_ur != ((vr_ur_in_rx_sdu+1)%rx_mod))) {
+      if((pdu_lost && !rlc_um_start_aligned(rx_window[vr_ur].header.fi)) || (vr_ur != ((vr_ur_in_rx_sdu+1)%cfg.rx_mod))) {
         log->warning("Dropping remainder of lost PDU (update vr_ur middle segments, vr_ur=%d, vr_ur_in_rx_sdu=%d)\n", vr_ur, vr_ur_in_rx_sdu);
         rx_sdu->reset();
       } else {
@@ -542,13 +529,13 @@ void rlc_um::reassemble_rx_sdus()
     pool->deallocate(rx_window[vr_ur].buf);
     rx_window.erase(vr_ur);
 
-    vr_ur = (vr_ur + 1)%rx_mod;
+    vr_ur = (vr_ur + 1)%cfg.rx_mod;
   }
 }
 
 bool rlc_um::inside_reordering_window(uint16_t sn)
 {
-  if(RX_MOD_BASE(sn) >= RX_MOD_BASE(vr_uh-rx_window_size) &&
+  if(RX_MOD_BASE(sn) >= RX_MOD_BASE(vr_uh-cfg.rx_window_size) &&
      RX_MOD_BASE(sn) <  RX_MOD_BASE(vr_uh))
   {
     return true;
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index 261a66548..c7ab2aa74 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -102,6 +102,7 @@ void basic_test()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -181,6 +182,7 @@ void concat_test()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -245,6 +247,7 @@ void segment_test()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -327,6 +330,7 @@ void retx_test()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -423,6 +427,7 @@ void resegment_test_1()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -532,6 +537,7 @@ void resegment_test_2()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -638,6 +644,7 @@ void resegment_test_3()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -740,6 +747,7 @@ void resegment_test_4()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -842,6 +850,7 @@ void resegment_test_5()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
@@ -943,6 +952,7 @@ void resegment_test_6()
   cnfg.ul_am_rlc.max_retx_thresh = LIBLTE_RRC_MAX_RETX_THRESHOLD_T4;
   cnfg.ul_am_rlc.poll_byte = LIBLTE_RRC_POLL_BYTE_KB25;
   cnfg.ul_am_rlc.poll_pdu = LIBLTE_RRC_POLL_PDU_P4;
+  cnfg.ul_am_rlc.t_poll_retx = LIBLTE_RRC_T_POLL_RETRANSMIT_MS5;
 
   rlc1.configure(&cnfg);
   rlc2.configure(&cnfg);
diff --git a/srsenb/hdr/upper/rlc.h b/srsenb/hdr/upper/rlc.h
index d0c165d73..abbf99516 100644
--- a/srsenb/hdr/upper/rlc.h
+++ b/srsenb/hdr/upper/rlc.h
@@ -50,7 +50,7 @@ public:
   void add_user(uint16_t rnti); 
   void rem_user(uint16_t rnti);
   void add_bearer(uint16_t rnti, uint32_t lcid);
-  void add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT *cnfg);
+  void add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_rlc_config_t cnfg);
 
   // rlc_interface_pdcp
   void write_sdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t *sdu);
diff --git a/srsenb/src/upper/rlc.cc b/srsenb/src/upper/rlc.cc
index e826e7f34..758a3b5b3 100644
--- a/srsenb/src/upper/rlc.cc
+++ b/srsenb/src/upper/rlc.cc
@@ -98,7 +98,7 @@ void rlc::add_bearer(uint16_t rnti, uint32_t lcid)
   }
 }
 
-void rlc::add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_RLC_CONFIG_STRUCT* cnfg)
+void rlc::add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_rlc_config_t cnfg)
 {
   if (users.count(rnti)) {
     users[rnti].rlc->add_bearer(lcid, cnfg);
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 0d0bf636c..40826ae51 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -1341,7 +1341,7 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
     {
       rlc->add_bearer(srb_cnfg->srb_id);
     }else{
-      rlc->add_bearer(srb_cnfg->srb_id, &srb_cnfg->rlc_explicit_cnfg);
+      rlc->add_bearer(srb_cnfg->srb_id, srslte_rlc_config_t(&srb_cnfg->rlc_explicit_cnfg));
     }
   }
 
@@ -1409,7 +1409,7 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   // TODO: setup PDCP security (using k_up_enc)
 
   // Setup RLC
-  rlc->add_bearer(lcid, &drb_cnfg->rlc_cnfg);
+  rlc->add_bearer(lcid, srslte_rlc_config_t(&drb_cnfg->rlc_cnfg));
 
   // Setup MAC
   uint8_t  log_chan_group       =  0;

From 80c0d0e6946d84e5effe8f60a542bc0b3dbed9c1 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Tue, 4 Jul 2017 16:54:51 +0200
Subject: [PATCH 040/269] fix printf's

---
 srsue/src/upper/rrc.cc | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 40826ae51..8bb327b9d 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -119,7 +119,7 @@ void rrc::set_ue_category(int category)
 
 void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
 {
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", bearers.at(lcid));
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", bearers.at(lcid).c_str());
 
   switch(state)
   {
@@ -230,7 +230,7 @@ bool rrc::have_drb()
 
 void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 {
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", bearers.at(lcid));
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", bearers.at(lcid).c_str());
   rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
 
   switch(lcid)
@@ -796,7 +796,7 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu)
   liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_dcch_msg);
 
   rrc_log->info("%s - Received %s\n",
-                bearers.at(lcid),
+                bearers.at(lcid).c_str(),
                 liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
 
   // Reset and reuse pdu buffer if possible
@@ -1376,7 +1376,7 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
   }
 
   srbs[srb_cnfg->srb_id] = *srb_cnfg;
-  rrc_log->info("Added radio bearer %s\n", bearers.at(srb_cnfg->srb_id));
+  rrc_log->info("Added radio bearer %s\n", bearers.at(srb_cnfg->srb_id).c_str());
 }
 
 void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
@@ -1437,7 +1437,7 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   
   drbs[lcid] = *drb_cnfg;
   drb_up     = true;
-  rrc_log->info("Added radio bearer %s\n", bearers.at(lcid));
+  rrc_log->info("Added radio bearer %s\n", bearers.at(lcid).c_str());
 }
 
 void rrc::release_drb(uint8_t lcid)

From 3d658cfdd81f7790e5e7f93f92e24c8266669f4e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 6 Jul 2017 10:37:51 +0200
Subject: [PATCH 041/269] fixed number of ports in pdsch tx

---
 lib/src/phy/phch/pdsch.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 4da881531..8791ac8e7 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -260,7 +260,7 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
         }
       }
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
+    for (int j=0;j<SRSLTE_MAX(q->nof_rx_antennas, q->cell.nof_ports);j++) {
       q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
       if (!q->symbols[j]) {
         goto clean;

From 10c0f03f33929f5e78f88aa3c6663a5c42d112f4 Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Thu, 6 Jul 2017 12:32:54 +0100
Subject: [PATCH 042/269] Soapy(lime) tx  support

---
 lib/examples/pdsch_enodeb.c    |   5 +-
 lib/include/srslte/phy/rf/rf.h |   4 +
 lib/src/phy/rf/rf_blade_imp.c  |   7 +
 lib/src/phy/rf/rf_blade_imp.h  |   5 +
 lib/src/phy/rf/rf_dev.h        |   8 +
 lib/src/phy/rf/rf_imp.c        |  10 +
 lib/src/phy/rf/rf_soapy_imp.c  | 539 ++++++++++++++++++---------------
 lib/src/phy/rf/rf_soapy_imp.h  |   4 +
 lib/src/phy/rf/rf_uhd_imp.c    |  11 +
 lib/src/phy/rf/rf_uhd_imp.h    |   4 +
 10 files changed, 349 insertions(+), 248 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 800e04de9..db288ae1c 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -510,7 +510,7 @@ int main(int argc, char **argv) {
   sigaddset(&sigset, SIGINT);
   sigprocmask(SIG_UNBLOCK, &sigset, NULL);
   signal(SIGINT, sig_int_handler);
-
+  
   if (!output_file_name) {
     
     int srate = srslte_sampling_freq_hz(cell.nof_prb);    
@@ -534,6 +534,7 @@ int main(int argc, char **argv) {
     printf("Set TX freq: %.2f MHz\n",
         srslte_rf_set_tx_freq(&rf, rf_freq) / 1000000);
   }
+srslte_rf_start_tx_stream(&rf); 
 #endif
 
   if (update_radl(sf_idx)) {
@@ -662,7 +663,7 @@ int main(int argc, char **argv) {
     nf++;
     sfn = (sfn + 1) % 1024;
   }
-
+	srslte_rf_stop_tx_stream(&rf);
   base_free();
 
   printf("Done\n");
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index 907a5a008..e83d4b4cc 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -99,6 +99,10 @@ SRSLTE_API int srslte_rf_start_rx_stream(srslte_rf_t *h);
 
 SRSLTE_API int srslte_rf_stop_rx_stream(srslte_rf_t *h);
 
+SRSLTE_API int srslte_rf_start_tx_stream(srslte_rf_t *h);
+
+SRSLTE_API int srslte_rf_stop_tx_stream(srslte_rf_t *h);
+
 SRSLTE_API void srslte_rf_flush_buffer(srslte_rf_t *h);
 
 SRSLTE_API bool srslte_rf_has_rssi(srslte_rf_t *h);
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index f3dd471f6..04e26e509 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -164,6 +164,13 @@ int rf_blade_stop_rx_stream(void *h)
   return 0;
 }
 
+
+int rf_blade_stop_tx_stream(void *h)
+{
+ return 0;
+}
+
+
 void rf_blade_flush_buffer(void *h)
 {
 }
diff --git a/lib/src/phy/rf/rf_blade_imp.h b/lib/src/phy/rf/rf_blade_imp.h
index 07e9bb7cf..a537a86c3 100644
--- a/lib/src/phy/rf/rf_blade_imp.h
+++ b/lib/src/phy/rf/rf_blade_imp.h
@@ -51,6 +51,11 @@ SRSLTE_API int rf_blade_start_rx_stream_nsamples(void *h,
 
 SRSLTE_API int rf_blade_stop_rx_stream(void *h);
 
+SRSLTE_API int rf_blade_start_tx_stream(void *h);
+
+SRSLTE_API int rf_blade_stop_tx_stream(void *h);
+
+
 SRSLTE_API void rf_blade_flush_buffer(void *h);
 
 SRSLTE_API bool rf_blade_has_rssi(void *h);
diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index e1b761419..bc6882a49 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -32,6 +32,8 @@ typedef struct {
   bool   (*srslte_rf_rx_wait_lo_locked) (void *h);
   int    (*srslte_rf_start_rx_stream)(void *h);
   int    (*srslte_rf_stop_rx_stream)(void *h);
+  int    (*srslte_rf_start_tx_stream)(void *h);
+  int    (*srslte_rf_stop_tx_stream)(void *h);
   void   (*srslte_rf_flush_buffer)(void *h);
   bool   (*srslte_rf_has_rssi)(void *h);
   float  (*srslte_rf_get_rssi)(void *h);
@@ -75,6 +77,8 @@ static rf_dev_t dev_uhd = {
   rf_uhd_rx_wait_lo_locked,
   rf_uhd_start_rx_stream,
   rf_uhd_stop_rx_stream,
+  rf_uhd_start_tx_stream,
+  rf_uhd_stop_tx_stream,
   rf_uhd_flush_buffer,
   rf_uhd_has_rssi,
   rf_uhd_get_rssi,
@@ -113,6 +117,8 @@ static rf_dev_t dev_blade = {
   rf_blade_rx_wait_lo_locked,
   rf_blade_start_rx_stream,
   rf_blade_stop_rx_stream,
+  rf_blade_start_tx_stream,
+  rf_blade_stop_tx_stream,
   rf_blade_flush_buffer,
   rf_blade_has_rssi,
   rf_blade_get_rssi,
@@ -150,6 +156,8 @@ static rf_dev_t dev_soapy = {
   rf_soapy_rx_wait_lo_locked,
   rf_soapy_start_rx_stream,
   rf_soapy_stop_rx_stream,
+  rf_soapy_start_tx_stream,
+  rf_soapy_stop_tx_stream,
   rf_soapy_flush_buffer,
   rf_soapy_has_rssi,
   rf_soapy_get_rssi,
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 92b8143da..44f4bbeda 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -154,11 +154,21 @@ int srslte_rf_start_rx_stream(srslte_rf_t *rf)
   return ((rf_dev_t*) rf->dev)->srslte_rf_start_rx_stream(rf->handler);  
 }
 
+int srslte_rf_start_tx_stream(srslte_rf_t *rf)
+{
+  return ((rf_dev_t*) rf->dev)->srslte_rf_start_tx_stream(rf->handler);  
+}
+
 int srslte_rf_stop_rx_stream(srslte_rf_t *rf)
 {
   return ((rf_dev_t*) rf->dev)->srslte_rf_stop_rx_stream(rf->handler);  
 }
 
+int srslte_rf_stop_tx_stream(srslte_rf_t *rf)
+{
+  return ((rf_dev_t*) rf->dev)->srslte_rf_stop_tx_stream(rf->handler);  
+}
+
 void srslte_rf_flush_buffer(srslte_rf_t *rf)
 {
   ((rf_dev_t*) rf->dev)->srslte_rf_flush_buffer(rf->handler);  
diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 8fcf57c24..74d7a885a 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -32,404 +32,451 @@
 
 #include "srslte/srslte.h"
 #include "rf_soapy_imp.h"
-#include "srslte/phy/rf/rf.h"
+#include "srslte/rf/rf.h"
 
 #include <SoapySDR/Device.h>
 #include <SoapySDR/Formats.h>
+//#include "lime/LimeSuite.h"
 
 typedef struct {
-    SoapySDRKwargs args;
-    SoapySDRDevice *device;
-    SoapySDRRange *ranges;
-    SoapySDRStream *rxStream;
-    SoapySDRStream *txStream;
+ 
+ SoapySDRKwargs args; 
+ SoapySDRDevice *device;
+ SoapySDRRange *ranges;
+ 
+ SoapySDRStream *rxStream;
+ SoapySDRStream *txStream;
+ 
+ 
 } rf_soapy_handler_t;
 
+cf_t zero_mem[64*1024];
 
 int soapy_error(void *h)
 {
-  return 0;
-}
 
+}
 
 void rf_soapy_get_freq_range(void *h)
 {
-
+  
 }
 
-
 void rf_soapy_suppress_handler(const char *x)
 {
-    // not supported
+  // not supported
 }
 
-
 void rf_soapy_msg_handler(const char *msg)
 {
-    // not supported
+  // not supported
 }
 
-
 void rf_soapy_suppress_stdout(void *h)
 {
-    // not supported
+  // not supported
 }
 
-
 void rf_soapy_register_error_handler(void *notused, srslte_rf_error_handler_t new_handler)
 {
-    // not supported
+  // not supported
 }
 
+static bool isLocked(rf_soapy_handler_t *handler, char *sensor_name, void *value_h)
+{
+  // not supported
+  return true;
+}
 
 char* rf_soapy_devname(void* h)
 {
-  return "soapy";
+ 
 }
 
 bool rf_soapy_rx_wait_lo_locked(void *h)
 {
-  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
+  // not supported
   return true;
 }
 
-
 void rf_soapy_set_tx_cal(void *h, srslte_rf_cal_t *cal)
 {
-  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
   // not supported
 }
 
-
-void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal)
+void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal) 
 {
-  printf("TODO: implement rf_soapy_set_rx_cal()\n");
+  // not supported
 }
 
-
 int rf_soapy_start_rx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+	//printf("starting SOAPY rx stream \n");
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+   
 
-  if (SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0) != 0)
-    return SRSLTE_ERROR;
-  
-  return SRSLTE_SUCCESS;
+    if(SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0)!=0)//start streaming
+        return SRSLTE_ERROR;
+        
+        
+    return SRSLTE_SUCCESS;
 }
 
 
 int rf_soapy_start_tx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  
-  if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
-    return SRSLTE_ERROR;
-
-  return SRSLTE_SUCCESS;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h; 
+    if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
+        return SRSLTE_ERROR;
+        
+        
+    return SRSLTE_SUCCESS;
 }
 
-
 int rf_soapy_stop_rx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
-    return SRSLTE_ERROR;
-
-  return SRSLTE_SUCCESS;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    if(SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
+       return SRSLTE_ERROR;
+    
+    
+     
+     return SRSLTE_SUCCESS;
 }
-
-
 int rf_soapy_stop_tx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
-    return SRSLTE_ERROR;
-
-  return SRSLTE_SUCCESS;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    
+    if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
+        return SRSLTE_ERROR;
+    
+    
+ 
+    return SRSLTE_SUCCESS;
 }
 
-
 void rf_soapy_flush_buffer(void *h)
 {
-  int n;
+   int n; 
   cf_t tmp1[1024];
   cf_t tmp2[1024];
   void *data[2] = {tmp1, tmp2};
   do {
     n = rf_soapy_recv_with_time_multi(h, data, 1024, 0, NULL, NULL);
-  } while (n > 0);
+  } while (n > 0); 
 }
 
-
 bool rf_soapy_has_rssi(void *h)
 {
-  printf("TODO: implement rf_soapy_has_rssi()\n");
-  return false;
+  
 }
 
-
 float rf_soapy_get_rssi(void *h)
 {
-  printf("TODO: implement rf_soapy_get_rssi()\n");
-  return 0.0;
-}
 
+}
 
 //TODO: add multi-channel support
 int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
-{
-  size_t length;
-  const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
-  
-  if (length == 0) {
-    printf("No Soapy devices found.\n");
-    return SRSLTE_ERROR;
-  }
-  
-  for (size_t i = 0; i < length; i++) {
-    printf("Soapy Has Found device #%d: ", (int)i);
-    for (size_t j = 0; j < soapy_args[i].size; j++)
+{//SoapySDRKwargs soapy_args = {};
+    size_t length;
+    const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
+    
+    if(length == 0)
     {
-      printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
+        return SRSLTE_ERROR;
     }
-    printf("\n");
-  }
-  
-  SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
-  if (sdr == NULL) {
-    printf("failed to create SOAPY object\n");
-    return SRSLTE_ERROR;
-  }
-  
-  // create handler
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
-  bzero(handler, sizeof(rf_soapy_handler_t));
-  *h = handler;
-  handler->device = sdr;
-  
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  
-  return SRSLTE_SUCCESS;
-}
+    
+     for (size_t i = 0; i < length; i++)
+    {
+        printf("Soapy Has Found device #%d: ", (int)i);
+        for (size_t j = 0; j < soapy_args[i].size; j++)
+        {
+            printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
+        }
+        printf("\n");
+    }
+    
+   // SoapySDRrgs_set(&soapy_args, "driver", "rtlsdr");
+    SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
+    
+    if(sdr == NULL)
+    {
+		printf("failed to create SOAPY object\n");
+        return SRSLTE_ERROR;
+
+    }
+    
+    //SoapySDRKwargs_clear(&soapy_args);
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
+    *h = handler;
+    handler->device = sdr;
+	
 
 
+    //size_t channels[1];
+	//channels[0] = 0;
+    
+    if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
+    {
+        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+
+    if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
+    {
+        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+
+
+
+    return SRSLTE_SUCCESS;
+    
+}
+
 int rf_soapy_open(char *args, void **h)
 {
-  return rf_soapy_open_multi(args, h, 1);
+  return rf_soapy_open_multi(args, h, 1);  
 }
 
 
 int rf_soapy_close(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (handler->txStream) {
-    rf_soapy_stop_tx_stream(handler);
-    SoapySDRDevice_closeStream(handler->device, handler->txStream);
-  }
-  
-  if (handler->rxStream) {
-    rf_soapy_stop_rx_stream(handler);
-    SoapySDRDevice_closeStream(handler->device, handler->rxStream);
-  }
-  
-  SoapySDRDevice_unmake(handler->device);
-  free(handler);
-  
-  return SRSLTE_SUCCESS;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+ 	SoapySDRDevice_closeStream(handler->device, handler->txStream);
+ 	SoapySDRDevice_closeStream(handler->device, handler->rxStream);
+    SoapySDRDevice_unmake(handler->device);
 }
 
 void rf_soapy_set_master_clock_rate(void *h, double rate)
 {
   // Allow the soapy to automatically set the appropriate clock rate
-  // TODO: implement this function
+    
+    printf("SET MASTER CLOCK RATE\n");
 }
 
-
-bool rf_soapy_is_master_clock_dynamic(void *h)
+bool rf_soapy_is_master_clock_dynamic(void *h) 
 {
-  printf("TODO: implement rf_soapy_is_master_clock_dynamic()\n");
-  return false;
+  
 }
 
-
 double rf_soapy_set_rx_srate(void *h, double rate)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
-    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+ SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_RX,0);
+ printf("rx min gain is %f \n",range.minimum);
+ printf("rx max gain is %f \n",range.maximum);
+    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0)
+    {
+        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    
+    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
+    printf("Sampling rate is set to %f : \n",ret);
+    return ret;
 }
 
 double rf_soapy_set_tx_srate(void *h, double rate)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
-    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
-}
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+ SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_TX,0);
+ printf("tx min gain is %f \n",range.minimum);
+ printf("tx max gain is %f \n",range.maximum);
 
+    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0)
+    {
+        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
+    printf("Sampling rate is set to %f : \n",ret);
+    return ret;
+}
 
 double rf_soapy_set_rx_gain(void *h, double gain)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
-  {
-    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  return rf_soapy_get_rx_gain(h);
-}
 
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
+    {
+        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    double ret = rf_soapy_get_rx_gain(h);
+    printf("gain has been set to %f \n",ret);
+    return ret;
+}
 
 double rf_soapy_set_tx_gain(void *h, double gain)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
-  {
-    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  return rf_soapy_get_rx_gain(h);
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
+    {
+        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    double ret = rf_soapy_get_tx_gain(h);
+    printf("gain has been set to %f \n",ret);
+    return ret;
 }
 
-
 double rf_soapy_get_rx_gain(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);
-}
+    
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);   
 
+}
 
 double rf_soapy_get_tx_gain(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0);
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0); 
 }
 
-
 double rf_soapy_set_rx_freq(void *h, double freq)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
-  {
-    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
+    
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
+    {
+        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
+    printf("Frequency has been set to %f : \n",ret);
+    return ret;
   
-  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
 }
 
 double rf_soapy_set_tx_freq(void *h, double freq)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
-  {
-    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
+    {
+        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+        return SRSLTE_ERROR;
+    }
+    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
+    printf("Frequency has been set to %f : \n",ret);
+    return ret;
+  
 }
 
 
 void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs) {
-
+  
 }
 
 //TODO: add multi-channel support
 int  rf_soapy_recv_with_time_multi(void *h,
-                                   void **data,
-                                   uint32_t nsamples,
-                                   bool blocking,
-                                   time_t *secs,
-                                   double *frac_secs)
-{
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  //void *buffs[] = {buff}; //array of buffers
-  
-  int flags; //flags set by receive operation
-  
-  int num_channels = 1; // temp
-  
-  int trials = 0;
-  int ret = 0;
-  long long timeNs; //timestamp for receive buffer
-  int n = 0;
-  do {
-    size_t rx_samples = nsamples;
+                                            void **data,
+                                            uint32_t nsamples,
+                                            bool blocking,
+                                            time_t *secs,
+                                            double *frac_secs)
+{
+     rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+        //void *buffs[] = {buff}; //array of buffers
+        
+        int flags; //flags set by receive operation
+		
+	int num_channels = 1; // temp
+        
+        int trials = 0;
+        int ret = 0;
+        long long timeNs; //timestamp for receive buffer
+        int n = 0;
+        do{
+            
+            size_t rx_samples = nsamples;    
+            
+            if (rx_samples > nsamples - n) 
+            {
+                rx_samples = nsamples - n; 
+            }
+            void *buffs_ptr[4]; 
+            for (int i=0;i<num_channels;i++) 
+            {
+                cf_t *data_c = (cf_t*) data[i];
+                buffs_ptr[i] = &data_c[n];        
+            }   //(void*)(&data)
+            ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
+                
+            if(ret < 0)
+                return SRSLTE_ERROR;
+         n += ret;
+        trials++;
+        }while (n < nsamples && trials < 100);
+        
+        
+        //*secs = timeNs / 1000000000;
+        //*frac_secs = (timeNs % 1000000000)/1000000000;        
+       // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
+        return n;  
     
-    if (rx_samples > nsamples - n)
-    {
-      rx_samples = nsamples - n;
-    }
-    void *buffs_ptr[4];
-    for (int i=0; i<num_channels; i++)
-    {
-      cf_t *data_c = (cf_t*) data[i];
-      buffs_ptr[i] = &data_c[n];
-    }   //(void*)(&data)
-    ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
-    if(ret < 0) {
-      // continue when getting overflows
-      if (ret == SOAPY_SDR_OVERFLOW) {
-        fprintf(stderr, "O");
-        fflush(stderr);
-        continue;
-      } else {
-        return SRSLTE_ERROR;
-      }
-    }
-
-    n += ret;
-    trials++;
-  } while (n < nsamples && trials < 100);
-  
-  
-  //*secs = timeNs / 1000000000;
-  //*frac_secs = (timeNs % 1000000000)/1000000000;
-  // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
-  return n;
+ 
 }
 
 int rf_soapy_recv_with_time(void *h,
-                            void *data,
-                            uint32_t nsamples,
-                            bool blocking,
-                            time_t *secs,
-                            double *frac_secs)
+                                  void *data,
+                                  uint32_t nsamples,
+                                  bool blocking,
+                                  time_t *secs,
+                                  double *frac_secs) 
 {
-  return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
+       return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
 }
-
+            
 
 int rf_soapy_send_timed(void *h,
-                        void *data,
-                        int nsamples,
-                        time_t secs,
-                        double frac_secs,
-                        bool has_time_spec,
-                        bool blocking,
-                        bool is_start_of_burst,
-                        bool is_end_of_burst)
-{
-  int flags;
-  long long timeNs;
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  timeNs = secs * 1000000000;
-  timeNs = timeNs + (frac_secs * 1000000000);
-  int ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, data, nsamples, &flags, timeNs,  100000);
-  if(ret != nsamples)
-    return SRSLTE_ERROR;
-  
-  return ret;
+                     void *data,
+                     int nsamples,
+                     time_t secs,
+                     double frac_secs,                      
+                     bool has_time_spec,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst) 
+{
+    
+    int flags;
+    long long timeNs;
+        int trials = 0;
+        int ret = 0;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    timeNs = secs * 1000000000;
+    timeNs = timeNs + (frac_secs * 1000000000);
+	int num_channels = 1;
+	int n = 0;
+    cf_t *data_c = (cf_t*) data;
+ 	do{
+		size_t tx_samples = nsamples;
+		if (tx_samples > nsamples - n) 
+		{
+		    tx_samples = nsamples - n; 
+		}
+	      void *buff = (void*) &data_c[n];
+     	  const void *buffs_ptr[1] = {buff};
+			ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
+		if(ret < 0)
+		            return SRSLTE_ERROR;
+		 n += ret;
+		 trials++;
+     }while (n < nsamples && trials < 100);
+
+	
+    if(ret != nsamples)
+        return SRSLTE_ERROR;
+    
+    
+    
+    return ret;
+
 }
+
+
+
+
diff --git a/lib/src/phy/rf/rf_soapy_imp.h b/lib/src/phy/rf/rf_soapy_imp.h
index 23b59a8b3..7b081b9ff 100644
--- a/lib/src/phy/rf/rf_soapy_imp.h
+++ b/lib/src/phy/rf/rf_soapy_imp.h
@@ -49,6 +49,10 @@ SRSLTE_API int rf_soapy_start_rx_stream(void *h);
 
 SRSLTE_API int rf_soapy_stop_rx_stream(void *h);
 
+SRSLTE_API int rf_soapy_start_tx_stream(void *h);
+
+SRSLTE_API int rf_soapy_stop_tx_stream(void *h);
+
 SRSLTE_API void rf_soapy_flush_buffer(void *h);
 
 SRSLTE_API bool rf_soapy_has_rssi(void *h);
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 319093994..e6a5ea162 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -235,6 +235,17 @@ int rf_uhd_start_rx_stream(void *h)
   return 0;
 }
 
+int rf_uhd_start_tx_stream(void *h)
+{
+  return 0;
+}
+
+int rf_uhd_stop_tx_stream(void *h)
+{
+  return 0;
+}
+
+
 int rf_uhd_stop_rx_stream(void *h)
 {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h;
diff --git a/lib/src/phy/rf/rf_uhd_imp.h b/lib/src/phy/rf/rf_uhd_imp.h
index 7c26f015c..2d9808b22 100644
--- a/lib/src/phy/rf/rf_uhd_imp.h
+++ b/lib/src/phy/rf/rf_uhd_imp.h
@@ -51,6 +51,10 @@ SRSLTE_API void rf_uhd_set_rx_cal(void *h, srslte_rf_cal_t *cal);
 
 SRSLTE_API int rf_uhd_start_rx_stream(void *h);
 
+SRSLTE_API int rf_uhd_start_tx_stream(void *h);
+
+SRSLTE_API int rf_uhd_stop_tx_stream(void *h);
+
 SRSLTE_API int rf_uhd_start_rx_stream_nsamples(void *h, 
                                             uint32_t nsamples);
 

From 803077880932dcd2b158adb5465ad9389ba64609 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Thu, 6 Jul 2017 16:42:16 +0200
Subject: [PATCH 043/269] fix PDCP initialization in eNB

---
 lib/include/srslte/common/interfaces_common.h | 10 +++---
 .../srslte/interfaces/enb_interfaces.h        |  3 +-
 lib/src/upper/pdcp.cc                         |  8 ++++-
 lib/src/upper/rlc.cc                          |  7 ++---
 lib/src/upper/rlc_tm.cc                       |  2 +-
 lib/src/upper/rlc_um.cc                       |  6 ++--
 srsenb/hdr/upper/pdcp.h                       |  2 +-
 srsenb/src/upper/pdcp.cc                      |  9 +-----
 srsenb/src/upper/rrc.cc                       | 31 +++++++++++++++----
 9 files changed, 49 insertions(+), 29 deletions(-)

diff --git a/lib/include/srslte/common/interfaces_common.h b/lib/include/srslte/common/interfaces_common.h
index 5ef45cf54..44b1c5338 100644
--- a/lib/include/srslte/common/interfaces_common.h
+++ b/lib/include/srslte/common/interfaces_common.h
@@ -29,20 +29,22 @@
 
 #include "srslte/common/timers.h"
 #include "srslte/common/security.h"
+#include "srslte/asn1/liblte_rrc.h"
+
 
 namespace srslte {
 
 class srslte_pdcp_config_t
 {
 public:
-  srslte_pdcp_config_t(bool is_control_ = false)
-    :direction(SECURITY_DIRECTION_UPLINK)
+  srslte_pdcp_config_t(bool is_control_ = false, bool is_data_ = false, uint8_t direction_ = SECURITY_DIRECTION_UPLINK)
+    :direction(direction_)
     ,is_control(is_control_)
-    ,is_data(false)
+    ,is_data(is_data_)
     ,do_security(false)
     ,sn_len(12) {}
 
-  u_int8_t            direction;
+  uint8_t             direction;
   bool                is_control;
   bool                is_data;
   bool                do_security;
diff --git a/lib/include/srslte/interfaces/enb_interfaces.h b/lib/include/srslte/interfaces/enb_interfaces.h
index 5ba0213c1..ed2478896 100644
--- a/lib/include/srslte/interfaces/enb_interfaces.h
+++ b/lib/include/srslte/interfaces/enb_interfaces.h
@@ -28,6 +28,7 @@
 
 #include "srslte/common/common.h"
 #include "srslte/common/security.h"
+#include "srslte/common/interfaces_common.h"
 #include "srslte/interfaces/sched_interface.h"
 #include "srslte/upper/rlc_interface.h"
 #include "srslte/asn1/liblte_rrc.h"
@@ -175,7 +176,7 @@ public:
   virtual void add_user(uint16_t rnti) = 0; 
   virtual void rem_user(uint16_t rnti) = 0; 
   virtual void write_sdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t *sdu) = 0;
-  virtual void add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg=NULL) = 0;
+  virtual void add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_pdcp_config_t cnfg) = 0;
   virtual void config_security(uint16_t rnti, 
                                uint32_t lcid,
                                uint8_t *k_rrc_enc_,
diff --git a/lib/src/upper/pdcp.cc b/lib/src/upper/pdcp.cc
index f28eac974..7b4af6aad 100644
--- a/lib/src/upper/pdcp.cc
+++ b/lib/src/upper/pdcp.cc
@@ -41,7 +41,13 @@ void pdcp::init(srsue::rlc_interface_pdcp *rlc_, srsue::rrc_interface_pdcp *rrc_
   lcid      = lcid_;
   direction = direction_;
 
-  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, lcid, direction);
+  // Default config
+  srslte_pdcp_config_t cnfg;
+  cnfg.is_control = false;
+  cnfg.is_data = false;
+  cnfg.direction = direction_;
+
+  pdcp_array[0].init(rlc, rrc, gw, pdcp_log, lcid, cnfg);
 }
 
 void pdcp::stop()
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index 378b539b9..9a7b50448 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -193,9 +193,8 @@ void rlc::write_pdu_pcch(uint8_t *payload, uint32_t nof_bytes)
 *******************************************************************************/
 void rlc::add_bearer(uint32_t lcid)
 {
-  // No config provided - use defaults for lcid
-  if(default_lcid == lcid || (default_lcid+1) == lcid)
-  {
+  // No config provided - use defaults for SRB1 and SRB2
+  if(lcid < 3) {
     if (!rlc_array[lcid].active()) {
       LIBLTE_RRC_RLC_CONFIG_STRUCT cnfg;
       cnfg.rlc_mode                     = LIBLTE_RRC_RLC_MODE_AM;
@@ -210,7 +209,7 @@ void rlc::add_bearer(uint32_t lcid)
       rlc_log->warning("Bearer %s already configured. Reconfiguration not supported\n", get_rb_name(lcid).c_str());
     }
   }else{
-    rlc_log->error("Radio bearer %s does not support default RLC configuration.",
+    rlc_log->error("Radio bearer %s does not support default RLC configuration.\n",
                    get_rb_name(lcid).c_str());
   }
 }
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index 9b7cd8f00..e3bef0a99 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -113,7 +113,7 @@ int rlc_tm::read_pdu(uint8_t *payload, uint32_t nof_bytes)
   return pdu_size;
 }
 
-void rlc_tm:: write_pdu(uint8_t *payload, uint32_t nof_bytes)
+void rlc_tm::write_pdu(uint8_t *payload, uint32_t nof_bytes)
 {
   byte_buffer_t *buf = pool_allocate;
   memcpy(buf->msg, payload, nof_bytes);
diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index 133bca9a8..3d6bb9553 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -75,18 +75,18 @@ void rlc_um::configure(srslte_rlc_config_t cnfg_)
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits, tx_sn_field_length=%u bits\n",
               rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
-              cfg.t_reordering, cfg.rx_sn_field_length, cfg.tx_sn_field_length);
+              cfg.t_reordering, rlc_umd_sn_size_num[cfg.rx_sn_field_length], rlc_umd_sn_size_num[cfg.rx_sn_field_length]);
     break;
   case LIBLTE_RRC_RLC_MODE_UM_UNI_UL:
     log->info("%s configured in %s mode: tx_sn_field_length=%u bits\n",
               rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
-              cfg.tx_sn_field_length);
+              rlc_umd_sn_size_num[cfg.rx_sn_field_length]);
     break;
   case LIBLTE_RRC_RLC_MODE_UM_UNI_DL:
     log->info("%s configured in %s mode: "
               "t_reordering=%d ms, rx_sn_field_length=%u bits\n",
               rrc->get_rb_name(lcid).c_str(), liblte_rrc_rlc_mode_text[cnfg_.rlc_mode],
-              cfg.t_reordering, cfg.rx_sn_field_length);
+              cfg.t_reordering, rlc_umd_sn_size_num[cfg.rx_sn_field_length]);
     break;
   default:
     log->error("RLC configuration mode not recognized\n");
diff --git a/srsenb/hdr/upper/pdcp.h b/srsenb/hdr/upper/pdcp.h
index 1d047db3e..228490312 100644
--- a/srsenb/hdr/upper/pdcp.h
+++ b/srsenb/hdr/upper/pdcp.h
@@ -51,7 +51,7 @@ public:
   void add_user(uint16_t rnti);  
   void rem_user(uint16_t rnti); 
   void write_sdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t *sdu);
-  void add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT *cnfg=NULL);
+  void add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_pdcp_config_t cnfg);
   void config_security(uint16_t rnti, 
                        uint32_t lcid,
                        uint8_t *k_rrc_enc_,
diff --git a/srsenb/src/upper/pdcp.cc b/srsenb/src/upper/pdcp.cc
index ee65ae9e8..027f04909 100644
--- a/srsenb/src/upper/pdcp.cc
+++ b/srsenb/src/upper/pdcp.cc
@@ -73,20 +73,13 @@ void pdcp::rem_user(uint16_t rnti)
   }
 }
 
-void pdcp::add_bearer(uint16_t rnti, uint32_t lcid, LIBLTE_RRC_PDCP_CONFIG_STRUCT* cnfg_)
+void pdcp::add_bearer(uint16_t rnti, uint32_t lcid, srslte::srslte_pdcp_config_t cfg)
 {
   if (users.count(rnti)) {
-    srslte::srslte_pdcp_config_t cfg;
-    if(cnfg_->rlc_um_pdcp_sn_size_present) {
-      if(LIBLTE_RRC_PDCP_SN_SIZE_7_BITS == cnfg_->rlc_um_pdcp_sn_size) {
-        cfg.sn_len = 7;
-      }
-    }
     users[rnti].pdcp->add_bearer(lcid, cfg);
   }
 }
 
-
 void pdcp::reset(uint16_t rnti)
 {
   if (users.count(rnti)) {
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 3bdd46c70..331efdbd5 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -1154,9 +1154,14 @@ void rrc::ue::send_connection_setup(bool is_setup)
   // Configure MAC 
   parent->mac->ue_cfg(rnti, &sched_cfg);
     
-  // Configure SRB1 in RLC and PDCP     
+  // Configure SRB1 in RLC
   parent->rlc->add_bearer(rnti, 1);
-  parent->pdcp->add_bearer(rnti, 1);
+
+  // Configure SRB1 in PDCP
+  srslte::srslte_pdcp_config_t pdcp_cnfg;
+  pdcp_cnfg.is_control = true;
+  pdcp_cnfg.direction = SECURITY_DIRECTION_DOWNLINK;
+  parent->pdcp->add_bearer(rnti, 1, pdcp_cnfg);
 
   // Configure PHY layer
   parent->phy->set_config_dedicated(rnti, phy_cfg);
@@ -1168,7 +1173,6 @@ void rrc::ue::send_connection_setup(bool is_setup)
   rr_cfg->sps_cnfg_present = false; 
   
   send_dl_ccch(&dl_ccch_msg);
-  
 }
 
 
@@ -1312,12 +1316,27 @@ void rrc::ue::send_connection_reconf(srslte::byte_buffer_t *pdu)
   
   // Configure SRB2 in RLC and PDCP
   parent->rlc->add_bearer(rnti, 2);
-  parent->pdcp->add_bearer(rnti, 2);
-  
+
+  // Configure SRB2 in PDCP
+  srslte::srslte_pdcp_config_t pdcp_cnfg;
+  pdcp_cnfg.direction = SECURITY_DIRECTION_DOWNLINK;
+  pdcp_cnfg.is_control = true;
+  pdcp_cnfg.is_data = false;
+  parent->pdcp->add_bearer(rnti, 2, pdcp_cnfg);
+
   // Configure DRB1 in RLC
   parent->rlc->add_bearer(rnti, 3, &conn_reconf->rr_cnfg_ded.drb_to_add_mod_list[0].rlc_cnfg);
+
   // Configure DRB1 in PDCP
-  parent->pdcp->add_bearer(rnti, 3, &conn_reconf->rr_cnfg_ded.drb_to_add_mod_list[0].pdcp_cnfg);
+  pdcp_cnfg.is_control = false;
+  pdcp_cnfg.is_data = true;
+  if (conn_reconf->rr_cnfg_ded.drb_to_add_mod_list[0].pdcp_cnfg.rlc_um_pdcp_sn_size_present) {
+    if(LIBLTE_RRC_PDCP_SN_SIZE_7_BITS == conn_reconf->rr_cnfg_ded.drb_to_add_mod_list[0].pdcp_cnfg.rlc_um_pdcp_sn_size) {
+      pdcp_cnfg.sn_len = 7;
+    }
+  }
+  parent->pdcp->add_bearer(rnti, 3, pdcp_cnfg);
+
   // DRB1 has already been configured in GTPU through bearer setup
 
   // Add NAS Attach accept 

From 1e3b5f553ee62bb80233154820f4c851fc2a923e Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Thu, 6 Jul 2017 18:34:00 +0200
Subject: [PATCH 044/269] fix printf argument

---
 lib/src/upper/pdcp.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/upper/pdcp.cc b/lib/src/upper/pdcp.cc
index 7b4af6aad..5f4afbd62 100644
--- a/lib/src/upper/pdcp.cc
+++ b/lib/src/upper/pdcp.cc
@@ -81,7 +81,7 @@ void pdcp::add_bearer(uint32_t lcid, srslte_pdcp_config_t cfg)
     pdcp_array[lcid].init(rlc, rrc, gw, pdcp_log, lcid, cfg);
     pdcp_log->info("Added bearer %s\n", rrc->get_rb_name(lcid).c_str());
   } else {
-    pdcp_log->warning("Bearer %s already configured. Reconfiguration not supported\n", rrc->get_rb_name(lcid));
+    pdcp_log->warning("Bearer %s already configured. Reconfiguration not supported\n", rrc->get_rb_name(lcid).c_str());
   }
 }
 

From 14916fdc9a757c1b9a594a0453b409e28d4628ea Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 7 Jul 2017 18:04:59 +0200
Subject: [PATCH 045/269] missing return statement

---
 lib/include/srslte/common/metrics_hub.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/lib/include/srslte/common/metrics_hub.h b/lib/include/srslte/common/metrics_hub.h
index 8443ef65a..9575347a2 100644
--- a/lib/include/srslte/common/metrics_hub.h
+++ b/lib/include/srslte/common/metrics_hub.h
@@ -34,6 +34,7 @@ public:
   bool init(metrics_interface<metrics_t> *m_, float report_period_secs=1.0) {
     m = m_; 
     start_periodic(report_period_secs*1e6);
+    return true;
   }
   void stop() {
     thread_cancel();
@@ -47,7 +48,7 @@ private:
   void run_period() {
     metrics_t metric; 
     m->get_metrics(metric);
-    for (int i=0;i<listeners.size();i++) {
+    for (uint32_t i=0;i<listeners.size();i++) {
       listeners[i]->set_metrics(metric);
     }
   }

From 41edfcc7abd81b8897259204313a2abda4b6f2ac Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 11 Jul 2017 13:17:26 +0200
Subject: [PATCH 046/269] fixed segfault due to race condition in scrambling
 sequence pre-generation

---
 lib/include/srslte/phy/phch/pdsch.h |  3 ++-
 lib/include/srslte/phy/phch/pucch.h |  3 ++-
 lib/include/srslte/phy/phch/pusch.h |  3 ++-
 lib/src/phy/phch/pdsch.c            | 24 +++++++++++++-----------
 lib/src/phy/phch/pucch.c            |  8 +++++---
 lib/src/phy/phch/pusch.c            | 27 ++++++++++++++-------------
 6 files changed, 38 insertions(+), 30 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7730d2fa1..ad01c4ef8 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -48,7 +48,8 @@
 #include "srslte/phy/phch/pdsch_cfg.h"
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];  
+  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequence_generated;
 } srslte_pdsch_user_t;
 
 /* PDSCH object */
diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index 3542dc53f..56d512418 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -80,7 +80,8 @@ typedef struct SRSLTE_API {
 } srslte_pucch_cfg_t;
 
 typedef struct  {
-  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME];   
+  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequence_generated;
 } srslte_pucch_user_t; 
 
 /* PUCCH object */
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index bf04a4781..e5ee43995 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -61,7 +61,8 @@ typedef struct {
 } srslte_pusch_hopping_cfg_t;
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];  
+  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequences_generated;
 } srslte_pusch_user_t; 
 
 /* PUSCH object */
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 8791ac8e7..9b6128c64 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/phch/pdsch.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
@@ -362,6 +363,7 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
           return SRSLTE_ERROR; 
         }
       }
+      q->users[rnti]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
@@ -467,15 +469,15 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
     
     /* descramble */
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
+      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);      
+      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {    
-      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);      
     }
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
@@ -537,15 +539,15 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     }
 
     /* scramble */
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
+      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
       srslte_scrambling_bytes(&seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {    
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
     }
     
     srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e, q->d, cfg->nbits.nof_bits);
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index c58f69871..6a889b89c 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <math.h>
 #include <complex.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
 #include "srslte/phy/phch/pucch.h"
@@ -489,7 +490,7 @@ void srslte_pucch_clear_rnti(srslte_pucch_t *q, uint16_t rnti) {
 
 int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
   if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_pucch_user_t));
+    q->users[rnti] = calloc(1, sizeof(srslte_pucch_user_t));
     if (q->users[rnti]) {
       for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
         // Precompute scrambling sequence for pucch format 2    
@@ -498,6 +499,7 @@ int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
           return SRSLTE_ERROR; 
         }        
       }
+      q->users[rnti]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS; 
@@ -591,7 +593,7 @@ static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t
     case SRSLTE_PUCCH_FORMAT_2:
     case SRSLTE_PUCCH_FORMAT_2A:
     case SRSLTE_PUCCH_FORMAT_2B:
-      if (q->users[rnti]) {
+      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
         memcpy(q->bits_scram, bits, SRSLTE_PUCCH2_NOF_BITS*sizeof(uint8_t));
         srslte_scrambling_b(&q->users[rnti]->seq_f2[sf_idx], q->bits_scram);
         srslte_mod_modulate(&q->mod, q->bits_scram, q->d, SRSLTE_PUCCH2_NOF_BITS);
@@ -796,7 +798,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
       case SRSLTE_PUCCH_FORMAT_2:
       case SRSLTE_PUCCH_FORMAT_2A:
       case SRSLTE_PUCCH_FORMAT_2B:
-        if (q->users[rnti]) {
+        if (q->users[rnti] && q->users[rnti]->sequence_generated) {
           pucch_encode_(q, format, n_pucch, sf_idx, rnti, NULL, ref, true);
           srslte_vec_prod_conj_ccc(q->z, ref, q->z_tmp, SRSLTE_PUCCH_MAX_SYMBOLS);
           for (int i=0;i<SRSLTE_PUCCH2_NOF_BITS/2;i++) {
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 5e1cbd922..9e0df307b 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -400,7 +400,8 @@ int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
             q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
           return SRSLTE_ERROR; 
         }
-      }      
+      }
+      q->users[rnti]->sequences_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
@@ -444,15 +445,15 @@ int srslte_pusch_encode(srslte_pusch_t *q, srslte_pusch_cfg_t *cfg, srslte_softb
       return SRSLTE_ERROR;
     }
 
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
+      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pusch(&seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);      
+      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);            
     }
     
     // Correct UCI placeholder/repetition bits    
@@ -535,13 +536,13 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     srslte_sequence_t *seq = NULL;
 
     // Create sequence if does not exist
-    if (!q->users[rnti]) {
-      seq = &q->tmp_seq; 
+    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
+      seq = &q->users[rnti]->seq[cfg->sf_idx];
+    } else {
+      seq = &q->tmp_seq;
       if (srslte_sequence_pusch(seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-    } else {
-      seq = &q->users[rnti]->seq[cfg->sf_idx]; 
     }
     
     // Decode RI/HARQ bits before descrambling 
@@ -553,7 +554,7 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     // Descrambling
     srslte_scrambling_s_offset(seq, q->q, 0, cfg->nbits.nof_bits);
         
-    if (!q->users[rnti]) {
+    if (!(q->users[rnti] && q->users[rnti]->sequences_generated)) {
       srslte_sequence_free(seq);
     }
     

From 7a02efe0e5415ef23ffd33bc6327129a09eeaad2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 11 Jul 2017 13:17:26 +0200
Subject: [PATCH 047/269] fixed segfault due to race condition in scrambling
 sequence pre-generation

---
 lib/include/srslte/phy/phch/pdsch.h |  3 ++-
 lib/include/srslte/phy/phch/pucch.h |  3 ++-
 lib/include/srslte/phy/phch/pusch.h |  3 ++-
 lib/src/phy/phch/pdsch.c            | 24 +++++++++++++-----------
 lib/src/phy/phch/pucch.c            |  8 +++++---
 lib/src/phy/phch/pusch.c            | 27 ++++++++++++++-------------
 6 files changed, 38 insertions(+), 30 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7730d2fa1..ad01c4ef8 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -48,7 +48,8 @@
 #include "srslte/phy/phch/pdsch_cfg.h"
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];  
+  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequence_generated;
 } srslte_pdsch_user_t;
 
 /* PDSCH object */
diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index 3542dc53f..56d512418 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -80,7 +80,8 @@ typedef struct SRSLTE_API {
 } srslte_pucch_cfg_t;
 
 typedef struct  {
-  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME];   
+  srslte_sequence_t seq_f2[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequence_generated;
 } srslte_pucch_user_t; 
 
 /* PUCCH object */
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index bf04a4781..e5ee43995 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -61,7 +61,8 @@ typedef struct {
 } srslte_pusch_hopping_cfg_t;
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];  
+  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  bool sequences_generated;
 } srslte_pusch_user_t; 
 
 /* PUSCH object */
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 4da881531..63e7dbfa0 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/phch/pdsch.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
@@ -362,6 +363,7 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
           return SRSLTE_ERROR; 
         }
       }
+      q->users[rnti]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
@@ -467,15 +469,15 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
     
     /* descramble */
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
+      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);      
+      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {    
-      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);      
     }
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
@@ -537,15 +539,15 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     }
 
     /* scramble */
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
+      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
       srslte_scrambling_bytes(&seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {    
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
     }
     
     srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e, q->d, cfg->nbits.nof_bits);
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index c58f69871..6a889b89c 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <math.h>
 #include <complex.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
 #include "srslte/phy/phch/pucch.h"
@@ -489,7 +490,7 @@ void srslte_pucch_clear_rnti(srslte_pucch_t *q, uint16_t rnti) {
 
 int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
   if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_pucch_user_t));
+    q->users[rnti] = calloc(1, sizeof(srslte_pucch_user_t));
     if (q->users[rnti]) {
       for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
         // Precompute scrambling sequence for pucch format 2    
@@ -498,6 +499,7 @@ int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
           return SRSLTE_ERROR; 
         }        
       }
+      q->users[rnti]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS; 
@@ -591,7 +593,7 @@ static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t
     case SRSLTE_PUCCH_FORMAT_2:
     case SRSLTE_PUCCH_FORMAT_2A:
     case SRSLTE_PUCCH_FORMAT_2B:
-      if (q->users[rnti]) {
+      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
         memcpy(q->bits_scram, bits, SRSLTE_PUCCH2_NOF_BITS*sizeof(uint8_t));
         srslte_scrambling_b(&q->users[rnti]->seq_f2[sf_idx], q->bits_scram);
         srslte_mod_modulate(&q->mod, q->bits_scram, q->d, SRSLTE_PUCCH2_NOF_BITS);
@@ -796,7 +798,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
       case SRSLTE_PUCCH_FORMAT_2:
       case SRSLTE_PUCCH_FORMAT_2A:
       case SRSLTE_PUCCH_FORMAT_2B:
-        if (q->users[rnti]) {
+        if (q->users[rnti] && q->users[rnti]->sequence_generated) {
           pucch_encode_(q, format, n_pucch, sf_idx, rnti, NULL, ref, true);
           srslte_vec_prod_conj_ccc(q->z, ref, q->z_tmp, SRSLTE_PUCCH_MAX_SYMBOLS);
           for (int i=0;i<SRSLTE_PUCCH2_NOF_BITS/2;i++) {
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 65ab08411..3fbb7627e 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -400,7 +400,8 @@ int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
             q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
           return SRSLTE_ERROR; 
         }
-      }      
+      }
+      q->users[rnti]->sequences_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
@@ -444,15 +445,15 @@ int srslte_pusch_encode(srslte_pusch_t *q, srslte_pusch_cfg_t *cfg, srslte_softb
       return SRSLTE_ERROR;
     }
 
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq; 
+    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
+      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);
+    } else {
+      srslte_sequence_t seq;
       if (srslte_sequence_pusch(&seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);      
+      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
-    } else {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);            
     }
     
     // Correct UCI placeholder/repetition bits    
@@ -535,13 +536,13 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     srslte_sequence_t *seq = NULL;
 
     // Create sequence if does not exist
-    if (!q->users[rnti]) {
-      seq = &q->tmp_seq; 
+    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
+      seq = &q->users[rnti]->seq[cfg->sf_idx];
+    } else {
+      seq = &q->tmp_seq;
       if (srslte_sequence_pusch(seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
-    } else {
-      seq = &q->users[rnti]->seq[cfg->sf_idx]; 
     }
     
     // Decode RI/HARQ bits before descrambling 
@@ -553,7 +554,7 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     // Descrambling
     srslte_scrambling_s_offset(seq, q->q, 0, cfg->nbits.nof_bits);
         
-    if (!q->users[rnti]) {
+    if (!(q->users[rnti] && q->users[rnti]->sequences_generated)) {
       srslte_sequence_free(seq);
     }
     

From 8ad26bda07fd904e760d1aea5d2aa1e909d03fd1 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 11 Jul 2017 15:47:08 +0200
Subject: [PATCH 048/269] eNodeB 2x time advance

---
 srsenb/src/mac/mac.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 8bf88a662..d45330863 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -384,7 +384,7 @@ int mac::rach_detected(uint32_t tti, uint32_t preamble_idx, uint32_t time_adv)
   }
   // Save RA info
   pending_rars[ra_id].preamble_idx = preamble_idx; 
-  pending_rars[ra_id].ta_cmd       = time_adv; 
+  pending_rars[ra_id].ta_cmd       = 2*time_adv;
   pending_rars[ra_id].temp_crnti   = last_rnti;   
   
   // Add new user to the scheduler so that it can RX/TX SRB0

From e8c41d4620c38fb901d0b1b1aa5774d0821427c1 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 12 Jul 2017 17:34:14 +0200
Subject: [PATCH 049/269] Revert "Soapy(lime) tx  support"

This reverts commit 10c0f03f33929f5e78f88aa3c6663a5c42d112f4.
---
 lib/examples/pdsch_enodeb.c    |   5 +-
 lib/include/srslte/phy/rf/rf.h |   4 -
 lib/src/phy/rf/rf_blade_imp.c  |   7 -
 lib/src/phy/rf/rf_blade_imp.h  |   5 -
 lib/src/phy/rf/rf_dev.h        |   8 -
 lib/src/phy/rf/rf_imp.c        |  10 -
 lib/src/phy/rf/rf_soapy_imp.c  | 539 +++++++++++++++------------------
 lib/src/phy/rf/rf_soapy_imp.h  |   4 -
 lib/src/phy/rf/rf_uhd_imp.c    |  11 -
 lib/src/phy/rf/rf_uhd_imp.h    |   4 -
 10 files changed, 248 insertions(+), 349 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index db288ae1c..800e04de9 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -510,7 +510,7 @@ int main(int argc, char **argv) {
   sigaddset(&sigset, SIGINT);
   sigprocmask(SIG_UNBLOCK, &sigset, NULL);
   signal(SIGINT, sig_int_handler);
-  
+
   if (!output_file_name) {
     
     int srate = srslte_sampling_freq_hz(cell.nof_prb);    
@@ -534,7 +534,6 @@ int main(int argc, char **argv) {
     printf("Set TX freq: %.2f MHz\n",
         srslte_rf_set_tx_freq(&rf, rf_freq) / 1000000);
   }
-srslte_rf_start_tx_stream(&rf); 
 #endif
 
   if (update_radl(sf_idx)) {
@@ -663,7 +662,7 @@ srslte_rf_start_tx_stream(&rf);
     nf++;
     sfn = (sfn + 1) % 1024;
   }
-	srslte_rf_stop_tx_stream(&rf);
+
   base_free();
 
   printf("Done\n");
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index e83d4b4cc..907a5a008 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -99,10 +99,6 @@ SRSLTE_API int srslte_rf_start_rx_stream(srslte_rf_t *h);
 
 SRSLTE_API int srslte_rf_stop_rx_stream(srslte_rf_t *h);
 
-SRSLTE_API int srslte_rf_start_tx_stream(srslte_rf_t *h);
-
-SRSLTE_API int srslte_rf_stop_tx_stream(srslte_rf_t *h);
-
 SRSLTE_API void srslte_rf_flush_buffer(srslte_rf_t *h);
 
 SRSLTE_API bool srslte_rf_has_rssi(srslte_rf_t *h);
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index 04e26e509..f3dd471f6 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -164,13 +164,6 @@ int rf_blade_stop_rx_stream(void *h)
   return 0;
 }
 
-
-int rf_blade_stop_tx_stream(void *h)
-{
- return 0;
-}
-
-
 void rf_blade_flush_buffer(void *h)
 {
 }
diff --git a/lib/src/phy/rf/rf_blade_imp.h b/lib/src/phy/rf/rf_blade_imp.h
index a537a86c3..07e9bb7cf 100644
--- a/lib/src/phy/rf/rf_blade_imp.h
+++ b/lib/src/phy/rf/rf_blade_imp.h
@@ -51,11 +51,6 @@ SRSLTE_API int rf_blade_start_rx_stream_nsamples(void *h,
 
 SRSLTE_API int rf_blade_stop_rx_stream(void *h);
 
-SRSLTE_API int rf_blade_start_tx_stream(void *h);
-
-SRSLTE_API int rf_blade_stop_tx_stream(void *h);
-
-
 SRSLTE_API void rf_blade_flush_buffer(void *h);
 
 SRSLTE_API bool rf_blade_has_rssi(void *h);
diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index bc6882a49..e1b761419 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -32,8 +32,6 @@ typedef struct {
   bool   (*srslte_rf_rx_wait_lo_locked) (void *h);
   int    (*srslte_rf_start_rx_stream)(void *h);
   int    (*srslte_rf_stop_rx_stream)(void *h);
-  int    (*srslte_rf_start_tx_stream)(void *h);
-  int    (*srslte_rf_stop_tx_stream)(void *h);
   void   (*srslte_rf_flush_buffer)(void *h);
   bool   (*srslte_rf_has_rssi)(void *h);
   float  (*srslte_rf_get_rssi)(void *h);
@@ -77,8 +75,6 @@ static rf_dev_t dev_uhd = {
   rf_uhd_rx_wait_lo_locked,
   rf_uhd_start_rx_stream,
   rf_uhd_stop_rx_stream,
-  rf_uhd_start_tx_stream,
-  rf_uhd_stop_tx_stream,
   rf_uhd_flush_buffer,
   rf_uhd_has_rssi,
   rf_uhd_get_rssi,
@@ -117,8 +113,6 @@ static rf_dev_t dev_blade = {
   rf_blade_rx_wait_lo_locked,
   rf_blade_start_rx_stream,
   rf_blade_stop_rx_stream,
-  rf_blade_start_tx_stream,
-  rf_blade_stop_tx_stream,
   rf_blade_flush_buffer,
   rf_blade_has_rssi,
   rf_blade_get_rssi,
@@ -156,8 +150,6 @@ static rf_dev_t dev_soapy = {
   rf_soapy_rx_wait_lo_locked,
   rf_soapy_start_rx_stream,
   rf_soapy_stop_rx_stream,
-  rf_soapy_start_tx_stream,
-  rf_soapy_stop_tx_stream,
   rf_soapy_flush_buffer,
   rf_soapy_has_rssi,
   rf_soapy_get_rssi,
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 44f4bbeda..92b8143da 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -154,21 +154,11 @@ int srslte_rf_start_rx_stream(srslte_rf_t *rf)
   return ((rf_dev_t*) rf->dev)->srslte_rf_start_rx_stream(rf->handler);  
 }
 
-int srslte_rf_start_tx_stream(srslte_rf_t *rf)
-{
-  return ((rf_dev_t*) rf->dev)->srslte_rf_start_tx_stream(rf->handler);  
-}
-
 int srslte_rf_stop_rx_stream(srslte_rf_t *rf)
 {
   return ((rf_dev_t*) rf->dev)->srslte_rf_stop_rx_stream(rf->handler);  
 }
 
-int srslte_rf_stop_tx_stream(srslte_rf_t *rf)
-{
-  return ((rf_dev_t*) rf->dev)->srslte_rf_stop_tx_stream(rf->handler);  
-}
-
 void srslte_rf_flush_buffer(srslte_rf_t *rf)
 {
   ((rf_dev_t*) rf->dev)->srslte_rf_flush_buffer(rf->handler);  
diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 74d7a885a..8fcf57c24 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -32,451 +32,404 @@
 
 #include "srslte/srslte.h"
 #include "rf_soapy_imp.h"
-#include "srslte/rf/rf.h"
+#include "srslte/phy/rf/rf.h"
 
 #include <SoapySDR/Device.h>
 #include <SoapySDR/Formats.h>
-//#include "lime/LimeSuite.h"
 
 typedef struct {
- 
- SoapySDRKwargs args; 
- SoapySDRDevice *device;
- SoapySDRRange *ranges;
- 
- SoapySDRStream *rxStream;
- SoapySDRStream *txStream;
- 
- 
+    SoapySDRKwargs args;
+    SoapySDRDevice *device;
+    SoapySDRRange *ranges;
+    SoapySDRStream *rxStream;
+    SoapySDRStream *txStream;
 } rf_soapy_handler_t;
 
-cf_t zero_mem[64*1024];
 
 int soapy_error(void *h)
 {
-
+  return 0;
 }
 
+
 void rf_soapy_get_freq_range(void *h)
 {
-  
+
 }
 
+
 void rf_soapy_suppress_handler(const char *x)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_msg_handler(const char *msg)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_suppress_stdout(void *h)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_register_error_handler(void *notused, srslte_rf_error_handler_t new_handler)
 {
-  // not supported
+    // not supported
 }
 
-static bool isLocked(rf_soapy_handler_t *handler, char *sensor_name, void *value_h)
-{
-  // not supported
-  return true;
-}
 
 char* rf_soapy_devname(void* h)
 {
- 
+  return "soapy";
 }
 
 bool rf_soapy_rx_wait_lo_locked(void *h)
 {
-  // not supported
+  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
   return true;
 }
 
+
 void rf_soapy_set_tx_cal(void *h, srslte_rf_cal_t *cal)
 {
+  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
   // not supported
 }
 
-void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal) 
+
+void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal)
 {
-  // not supported
+  printf("TODO: implement rf_soapy_set_rx_cal()\n");
 }
 
+
 int rf_soapy_start_rx_stream(void *h)
 {
-	//printf("starting SOAPY rx stream \n");
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-   
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
 
-    if(SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0)!=0)//start streaming
-        return SRSLTE_ERROR;
-        
-        
-    return SRSLTE_SUCCESS;
+  if (SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0) != 0)
+    return SRSLTE_ERROR;
+  
+  return SRSLTE_SUCCESS;
 }
 
 
 int rf_soapy_start_tx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h; 
-    if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
-        return SRSLTE_ERROR;
-        
-        
-    return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  
+  if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
 
+
 int rf_soapy_stop_rx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if(SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
-       return SRSLTE_ERROR;
-    
-    
-     
-     return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
+
+
 int rf_soapy_stop_tx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    
-    if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
-        return SRSLTE_ERROR;
-    
-    
- 
-    return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
 
+
 void rf_soapy_flush_buffer(void *h)
 {
-   int n; 
+  int n;
   cf_t tmp1[1024];
   cf_t tmp2[1024];
   void *data[2] = {tmp1, tmp2};
   do {
     n = rf_soapy_recv_with_time_multi(h, data, 1024, 0, NULL, NULL);
-  } while (n > 0); 
+  } while (n > 0);
 }
 
+
 bool rf_soapy_has_rssi(void *h)
 {
-  
+  printf("TODO: implement rf_soapy_has_rssi()\n");
+  return false;
 }
 
+
 float rf_soapy_get_rssi(void *h)
 {
-
+  printf("TODO: implement rf_soapy_get_rssi()\n");
+  return 0.0;
 }
 
+
 //TODO: add multi-channel support
 int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
-{//SoapySDRKwargs soapy_args = {};
-    size_t length;
-    const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
-    
-    if(length == 0)
-    {
-        return SRSLTE_ERROR;
-    }
-    
-     for (size_t i = 0; i < length; i++)
-    {
-        printf("Soapy Has Found device #%d: ", (int)i);
-        for (size_t j = 0; j < soapy_args[i].size; j++)
-        {
-            printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
-        }
-        printf("\n");
-    }
-    
-   // SoapySDRrgs_set(&soapy_args, "driver", "rtlsdr");
-    SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
-    
-    if(sdr == NULL)
-    {
-		printf("failed to create SOAPY object\n");
-        return SRSLTE_ERROR;
-
-    }
-    
-    //SoapySDRKwargs_clear(&soapy_args);
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
-    *h = handler;
-    handler->device = sdr;
-	
-
-
-    //size_t channels[1];
-	//channels[0] = 0;
-    
-    if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
-    {
-        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-
-    if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
+{
+  size_t length;
+  const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
+  
+  if (length == 0) {
+    printf("No Soapy devices found.\n");
+    return SRSLTE_ERROR;
+  }
+  
+  for (size_t i = 0; i < length; i++) {
+    printf("Soapy Has Found device #%d: ", (int)i);
+    for (size_t j = 0; j < soapy_args[i].size; j++)
     {
-        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
+      printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
     }
-
-
-
-    return SRSLTE_SUCCESS;
-    
+    printf("\n");
+  }
+  
+  SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
+  if (sdr == NULL) {
+    printf("failed to create SOAPY object\n");
+    return SRSLTE_ERROR;
+  }
+  
+  // create handler
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
+  bzero(handler, sizeof(rf_soapy_handler_t));
+  *h = handler;
+  handler->device = sdr;
+  
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  
+  return SRSLTE_SUCCESS;
 }
 
+
 int rf_soapy_open(char *args, void **h)
 {
-  return rf_soapy_open_multi(args, h, 1);  
+  return rf_soapy_open_multi(args, h, 1);
 }
 
 
 int rf_soapy_close(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- 	SoapySDRDevice_closeStream(handler->device, handler->txStream);
- 	SoapySDRDevice_closeStream(handler->device, handler->rxStream);
-    SoapySDRDevice_unmake(handler->device);
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (handler->txStream) {
+    rf_soapy_stop_tx_stream(handler);
+    SoapySDRDevice_closeStream(handler->device, handler->txStream);
+  }
+  
+  if (handler->rxStream) {
+    rf_soapy_stop_rx_stream(handler);
+    SoapySDRDevice_closeStream(handler->device, handler->rxStream);
+  }
+  
+  SoapySDRDevice_unmake(handler->device);
+  free(handler);
+  
+  return SRSLTE_SUCCESS;
 }
 
 void rf_soapy_set_master_clock_rate(void *h, double rate)
 {
   // Allow the soapy to automatically set the appropriate clock rate
-    
-    printf("SET MASTER CLOCK RATE\n");
+  // TODO: implement this function
 }
 
-bool rf_soapy_is_master_clock_dynamic(void *h) 
+
+bool rf_soapy_is_master_clock_dynamic(void *h)
 {
-  
+  printf("TODO: implement rf_soapy_is_master_clock_dynamic()\n");
+  return false;
 }
 
+
 double rf_soapy_set_rx_srate(void *h, double rate)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_RX,0);
- printf("rx min gain is %f \n",range.minimum);
- printf("rx max gain is %f \n",range.maximum);
-    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0)
-    {
-        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    
-    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
-    printf("Sampling rate is set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
+    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
 }
 
 double rf_soapy_set_tx_srate(void *h, double rate)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_TX,0);
- printf("tx min gain is %f \n",range.minimum);
- printf("tx max gain is %f \n",range.maximum);
-
-    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0)
-    {
-        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
-    printf("Sampling rate is set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
+    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
 }
 
+
 double rf_soapy_set_rx_gain(void *h, double gain)
 {
-
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
-    {
-        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = rf_soapy_get_rx_gain(h);
-    printf("gain has been set to %f \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
+  {
+    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return rf_soapy_get_rx_gain(h);
 }
 
+
 double rf_soapy_set_tx_gain(void *h, double gain)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
-    {
-        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = rf_soapy_get_tx_gain(h);
-    printf("gain has been set to %f \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
+  {
+    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return rf_soapy_get_rx_gain(h);
 }
 
+
 double rf_soapy_get_rx_gain(void *h)
 {
-    
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);   
-
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);
 }
 
+
 double rf_soapy_get_tx_gain(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0); 
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0);
 }
 
+
 double rf_soapy_set_rx_freq(void *h, double freq)
 {
-    
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
-    {
-        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
-    printf("Frequency has been set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
+  {
+    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
   
+  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
 }
 
 double rf_soapy_set_tx_freq(void *h, double freq)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
-    {
-        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
-    printf("Frequency has been set to %f : \n",ret);
-    return ret;
-  
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
+  {
+    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
 }
 
 
 void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs) {
-  
+
 }
 
 //TODO: add multi-channel support
 int  rf_soapy_recv_with_time_multi(void *h,
-                                            void **data,
-                                            uint32_t nsamples,
-                                            bool blocking,
-                                            time_t *secs,
-                                            double *frac_secs)
-{
-     rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-        //void *buffs[] = {buff}; //array of buffers
-        
-        int flags; //flags set by receive operation
-		
-	int num_channels = 1; // temp
-        
-        int trials = 0;
-        int ret = 0;
-        long long timeNs; //timestamp for receive buffer
-        int n = 0;
-        do{
-            
-            size_t rx_samples = nsamples;    
-            
-            if (rx_samples > nsamples - n) 
-            {
-                rx_samples = nsamples - n; 
-            }
-            void *buffs_ptr[4]; 
-            for (int i=0;i<num_channels;i++) 
-            {
-                cf_t *data_c = (cf_t*) data[i];
-                buffs_ptr[i] = &data_c[n];        
-            }   //(void*)(&data)
-            ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
-                
-            if(ret < 0)
-                return SRSLTE_ERROR;
-         n += ret;
-        trials++;
-        }while (n < nsamples && trials < 100);
-        
-        
-        //*secs = timeNs / 1000000000;
-        //*frac_secs = (timeNs % 1000000000)/1000000000;        
-       // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
-        return n;  
+                                   void **data,
+                                   uint32_t nsamples,
+                                   bool blocking,
+                                   time_t *secs,
+                                   double *frac_secs)
+{
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  //void *buffs[] = {buff}; //array of buffers
+  
+  int flags; //flags set by receive operation
+  
+  int num_channels = 1; // temp
+  
+  int trials = 0;
+  int ret = 0;
+  long long timeNs; //timestamp for receive buffer
+  int n = 0;
+  do {
+    size_t rx_samples = nsamples;
     
- 
+    if (rx_samples > nsamples - n)
+    {
+      rx_samples = nsamples - n;
+    }
+    void *buffs_ptr[4];
+    for (int i=0; i<num_channels; i++)
+    {
+      cf_t *data_c = (cf_t*) data[i];
+      buffs_ptr[i] = &data_c[n];
+    }   //(void*)(&data)
+    ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
+    if(ret < 0) {
+      // continue when getting overflows
+      if (ret == SOAPY_SDR_OVERFLOW) {
+        fprintf(stderr, "O");
+        fflush(stderr);
+        continue;
+      } else {
+        return SRSLTE_ERROR;
+      }
+    }
+
+    n += ret;
+    trials++;
+  } while (n < nsamples && trials < 100);
+  
+  
+  //*secs = timeNs / 1000000000;
+  //*frac_secs = (timeNs % 1000000000)/1000000000;
+  // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
+  return n;
 }
 
 int rf_soapy_recv_with_time(void *h,
-                                  void *data,
-                                  uint32_t nsamples,
-                                  bool blocking,
-                                  time_t *secs,
-                                  double *frac_secs) 
+                            void *data,
+                            uint32_t nsamples,
+                            bool blocking,
+                            time_t *secs,
+                            double *frac_secs)
 {
-       return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
+  return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
 }
-            
 
-int rf_soapy_send_timed(void *h,
-                     void *data,
-                     int nsamples,
-                     time_t secs,
-                     double frac_secs,                      
-                     bool has_time_spec,
-                     bool blocking,
-                     bool is_start_of_burst,
-                     bool is_end_of_burst) 
-{
-    
-    int flags;
-    long long timeNs;
-        int trials = 0;
-        int ret = 0;
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    timeNs = secs * 1000000000;
-    timeNs = timeNs + (frac_secs * 1000000000);
-	int num_channels = 1;
-	int n = 0;
-    cf_t *data_c = (cf_t*) data;
- 	do{
-		size_t tx_samples = nsamples;
-		if (tx_samples > nsamples - n) 
-		{
-		    tx_samples = nsamples - n; 
-		}
-	      void *buff = (void*) &data_c[n];
-     	  const void *buffs_ptr[1] = {buff};
-			ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
-		if(ret < 0)
-		            return SRSLTE_ERROR;
-		 n += ret;
-		 trials++;
-     }while (n < nsamples && trials < 100);
-
-	
-    if(ret != nsamples)
-        return SRSLTE_ERROR;
-    
-    
-    
-    return ret;
 
+int rf_soapy_send_timed(void *h,
+                        void *data,
+                        int nsamples,
+                        time_t secs,
+                        double frac_secs,
+                        bool has_time_spec,
+                        bool blocking,
+                        bool is_start_of_burst,
+                        bool is_end_of_burst)
+{
+  int flags;
+  long long timeNs;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  timeNs = secs * 1000000000;
+  timeNs = timeNs + (frac_secs * 1000000000);
+  int ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, data, nsamples, &flags, timeNs,  100000);
+  if(ret != nsamples)
+    return SRSLTE_ERROR;
+  
+  return ret;
 }
-
-
-
-
diff --git a/lib/src/phy/rf/rf_soapy_imp.h b/lib/src/phy/rf/rf_soapy_imp.h
index 7b081b9ff..23b59a8b3 100644
--- a/lib/src/phy/rf/rf_soapy_imp.h
+++ b/lib/src/phy/rf/rf_soapy_imp.h
@@ -49,10 +49,6 @@ SRSLTE_API int rf_soapy_start_rx_stream(void *h);
 
 SRSLTE_API int rf_soapy_stop_rx_stream(void *h);
 
-SRSLTE_API int rf_soapy_start_tx_stream(void *h);
-
-SRSLTE_API int rf_soapy_stop_tx_stream(void *h);
-
 SRSLTE_API void rf_soapy_flush_buffer(void *h);
 
 SRSLTE_API bool rf_soapy_has_rssi(void *h);
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index e6a5ea162..319093994 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -235,17 +235,6 @@ int rf_uhd_start_rx_stream(void *h)
   return 0;
 }
 
-int rf_uhd_start_tx_stream(void *h)
-{
-  return 0;
-}
-
-int rf_uhd_stop_tx_stream(void *h)
-{
-  return 0;
-}
-
-
 int rf_uhd_stop_rx_stream(void *h)
 {
   rf_uhd_handler_t *handler = (rf_uhd_handler_t*) h;
diff --git a/lib/src/phy/rf/rf_uhd_imp.h b/lib/src/phy/rf/rf_uhd_imp.h
index 2d9808b22..7c26f015c 100644
--- a/lib/src/phy/rf/rf_uhd_imp.h
+++ b/lib/src/phy/rf/rf_uhd_imp.h
@@ -51,10 +51,6 @@ SRSLTE_API void rf_uhd_set_rx_cal(void *h, srslte_rf_cal_t *cal);
 
 SRSLTE_API int rf_uhd_start_rx_stream(void *h);
 
-SRSLTE_API int rf_uhd_start_tx_stream(void *h);
-
-SRSLTE_API int rf_uhd_stop_tx_stream(void *h);
-
 SRSLTE_API int rf_uhd_start_rx_stream_nsamples(void *h, 
                                             uint32_t nsamples);
 

From 266f1b08fbf508cda5f1d2db3451776fd14c56cb Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 12 Jul 2017 16:14:17 +0200
Subject: [PATCH 050/269] rlc_am: print LCID in buffer status msg

---
 lib/src/upper/rlc_am.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index f0055a534..e8e2e2803 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -218,7 +218,7 @@ uint32_t rlc_am::get_total_buffer_state()
   // Room needed for fixed header?
   if(n_bytes > 0) {
     n_bytes += 2;
-    log->debug("Buffer state - tx SDUs: %d bytes\n", n_bytes);
+    log->debug("Total buffer state LCID=%d - tx SDUs: %d bytes\n", lcid, n_bytes);
   }
 
   pthread_mutex_unlock(&mutex);
@@ -235,7 +235,7 @@ uint32_t rlc_am::get_buffer_state()
   check_reordering_timeout();
   if(do_status && !status_prohibited()) {
     n_bytes = prepare_status();
-    log->debug("Buffer state - status report: %d bytes\n", n_bytes);
+    log->debug("Buffer state LCID=%d - status report: %d bytes\n", lcid, n_bytes);
     goto unlock_and_return;
   }
 
@@ -268,7 +268,7 @@ uint32_t rlc_am::get_buffer_state()
   // Room needed for fixed header?
   if(n_bytes > 0) {
     n_bytes += 2;
-    log->debug("Buffer state - tx SDUs: %d bytes\n", n_bytes);
+    log->debug("Buffer state LCID=%d - tx SDUs: %d bytes\n", lcid, n_bytes);
   }
 
 unlock_and_return:

From bc337ee281ac685358aa9aa0ce5663eb6b83f26c Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Thu, 13 Jul 2017 13:54:31 +0100
Subject: [PATCH 051/269] fixing soapy lime and rtlsdr compatibility

---
 lib/src/phy/rf/rf_soapy_imp.c | 512 +++++++++++++++++-----------------
 1 file changed, 252 insertions(+), 260 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 74d7a885a..484b412ae 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -32,54 +32,56 @@
 
 #include "srslte/srslte.h"
 #include "rf_soapy_imp.h"
-#include "srslte/rf/rf.h"
+#include "srslte/phy/rf/rf.h"
 
 #include <SoapySDR/Device.h>
 #include <SoapySDR/Formats.h>
-//#include "lime/LimeSuite.h"
 
 typedef struct {
- 
- SoapySDRKwargs args; 
- SoapySDRDevice *device;
- SoapySDRRange *ranges;
- 
- SoapySDRStream *rxStream;
- SoapySDRStream *txStream;
- 
- 
+    SoapySDRKwargs args;
+    SoapySDRDevice *device;
+    SoapySDRRange *ranges;
+    SoapySDRStream *rxStream;
+    SoapySDRStream *txStream;
+    bool tx_stream_active;
 } rf_soapy_handler_t;
 
+
 cf_t zero_mem[64*1024];
 
 int soapy_error(void *h)
 {
-
+  return 0;
 }
 
+
 void rf_soapy_get_freq_range(void *h)
 {
-  
+
 }
 
+
 void rf_soapy_suppress_handler(const char *x)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_msg_handler(const char *msg)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_suppress_stdout(void *h)
 {
-  // not supported
+    // not supported
 }
 
+
 void rf_soapy_register_error_handler(void *notused, srslte_rf_error_handler_t new_handler)
 {
-  // not supported
+    // not supported
 }
 
 static bool isLocked(rf_soapy_handler_t *handler, char *sensor_name, void *value_h)
@@ -88,348 +90,336 @@ static bool isLocked(rf_soapy_handler_t *handler, char *sensor_name, void *value
   return true;
 }
 
+
 char* rf_soapy_devname(void* h)
 {
- 
+  return "soapy";
 }
 
 bool rf_soapy_rx_wait_lo_locked(void *h)
 {
-  // not supported
+  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
   return true;
 }
 
+
 void rf_soapy_set_tx_cal(void *h, srslte_rf_cal_t *cal)
 {
+  printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
   // not supported
 }
 
-void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal) 
+
+void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal)
 {
-  // not supported
+  printf("TODO: implement rf_soapy_set_rx_cal()\n");
 }
 
+
 int rf_soapy_start_rx_stream(void *h)
 {
-	//printf("starting SOAPY rx stream \n");
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-   
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
 
-    if(SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0)!=0)//start streaming
-        return SRSLTE_ERROR;
-        
-        
-    return SRSLTE_SUCCESS;
+  if (SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0) != 0)
+    return SRSLTE_ERROR;
+  
+  return SRSLTE_SUCCESS;
 }
 
 
 int rf_soapy_start_tx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h; 
-    if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
-        return SRSLTE_ERROR;
-        
-        
-    return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  
+  if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
 
+
 int rf_soapy_stop_rx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if(SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
-       return SRSLTE_ERROR;
-    
-    
-     
-     return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_deactivateStream(handler->device, handler->rxStream, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
+
+
 int rf_soapy_stop_tx_stream(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    
-    if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
-        return SRSLTE_ERROR;
-    
-    
- 
-    return SRSLTE_SUCCESS;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
+    return SRSLTE_ERROR;
+
+  return SRSLTE_SUCCESS;
 }
 
+
 void rf_soapy_flush_buffer(void *h)
 {
-   int n; 
+  int n;
   cf_t tmp1[1024];
   cf_t tmp2[1024];
   void *data[2] = {tmp1, tmp2};
   do {
     n = rf_soapy_recv_with_time_multi(h, data, 1024, 0, NULL, NULL);
-  } while (n > 0); 
+  } while (n > 0);
 }
 
+
 bool rf_soapy_has_rssi(void *h)
 {
-  
+  printf("TODO: implement rf_soapy_has_rssi()\n");
+  return false;
 }
 
+
 float rf_soapy_get_rssi(void *h)
 {
-
+  printf("TODO: implement rf_soapy_get_rssi()\n");
+  return 0.0;
 }
 
+
 //TODO: add multi-channel support
 int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
-{//SoapySDRKwargs soapy_args = {};
-    size_t length;
-    const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
-    
-    if(length == 0)
-    {
-        return SRSLTE_ERROR;
-    }
-    
-     for (size_t i = 0; i < length; i++)
-    {
-        printf("Soapy Has Found device #%d: ", (int)i);
-        for (size_t j = 0; j < soapy_args[i].size; j++)
-        {
-            printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
-        }
-        printf("\n");
-    }
-    
-   // SoapySDRrgs_set(&soapy_args, "driver", "rtlsdr");
-    SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
-    
-    if(sdr == NULL)
-    {
-		printf("failed to create SOAPY object\n");
-        return SRSLTE_ERROR;
-
-    }
-    
-    //SoapySDRKwargs_clear(&soapy_args);
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
-    *h = handler;
-    handler->device = sdr;
-	
-
-
-    //size_t channels[1];
-	//channels[0] = 0;
-    
-    if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
-    {
-        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-
-    if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0)
+{
+  size_t length;
+  const SoapySDRKwargs *soapy_args = SoapySDRDevice_enumerate(NULL, &length);
+  
+  if (length == 0) {
+    printf("No Soapy devices found.\n");
+    return SRSLTE_ERROR;
+  }
+  
+  for (size_t i = 0; i < length; i++) {
+    printf("Soapy Has Found device #%d: ", (int)i);
+    for (size_t j = 0; j < soapy_args[i].size; j++)
     {
-        printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
+      printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
     }
-
-
-
-    return SRSLTE_SUCCESS;
-    
+    printf("\n");
+  }
+  
+  SoapySDRDevice *sdr = SoapySDRDevice_make(&(soapy_args[0]));
+  if (sdr == NULL) {
+    printf("failed to create SOAPY object\n");
+    return SRSLTE_ERROR;
+  }
+  
+  // create handler
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) malloc(sizeof(rf_soapy_handler_t));
+  bzero(handler, sizeof(rf_soapy_handler_t));
+  *h = handler;
+  handler->device = sdr;
+  handler->tx_stream_active = false;
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  
+  return SRSLTE_SUCCESS;
 }
 
+
 int rf_soapy_open(char *args, void **h)
 {
-  return rf_soapy_open_multi(args, h, 1);  
+  return rf_soapy_open_multi(args, h, 1);
 }
 
 
 int rf_soapy_close(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- 	SoapySDRDevice_closeStream(handler->device, handler->txStream);
- 	SoapySDRDevice_closeStream(handler->device, handler->rxStream);
-    SoapySDRDevice_unmake(handler->device);
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (handler->txStream) {
+    rf_soapy_stop_tx_stream(handler);
+    SoapySDRDevice_closeStream(handler->device, handler->txStream);
+  }
+  
+  if (handler->rxStream) {
+    rf_soapy_stop_rx_stream(handler);
+    SoapySDRDevice_closeStream(handler->device, handler->rxStream);
+  }
+  
+  SoapySDRDevice_unmake(handler->device);
+  free(handler);
+  
+  return SRSLTE_SUCCESS;
 }
 
 void rf_soapy_set_master_clock_rate(void *h, double rate)
 {
   // Allow the soapy to automatically set the appropriate clock rate
-    
-    printf("SET MASTER CLOCK RATE\n");
+  // TODO: implement this function
 }
 
-bool rf_soapy_is_master_clock_dynamic(void *h) 
+
+bool rf_soapy_is_master_clock_dynamic(void *h)
 {
-  
+  printf("TODO: implement rf_soapy_is_master_clock_dynamic()\n");
+  return false;
 }
 
+
 double rf_soapy_set_rx_srate(void *h, double rate)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_RX,0);
- printf("rx min gain is %f \n",range.minimum);
- printf("rx max gain is %f \n",range.maximum);
-    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0)
-    {
-        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    
-    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
-    printf("Sampling rate is set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
+    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
 }
 
 double rf_soapy_set_tx_srate(void *h, double rate)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
- SoapySDRRange range = SoapySDRDevice_getGainRange(handler->device, SOAPY_SDR_TX,0);
- printf("tx min gain is %f \n",range.minimum);
- printf("tx max gain is %f \n",range.maximum);
-
-    if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0)
-    {
-        printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
-    printf("Sampling rate is set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
+    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
 }
 
+
 double rf_soapy_set_rx_gain(void *h, double gain)
 {
-
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
-    {
-        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = rf_soapy_get_rx_gain(h);
-    printf("gain has been set to %f \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_RX, 0, gain) != 0)
+  {
+    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return rf_soapy_get_rx_gain(h);
 }
 
+
 double rf_soapy_set_tx_gain(void *h, double gain)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
-    {
-        printf("setGain fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = rf_soapy_get_tx_gain(h);
-    printf("gain has been set to %f \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setGain(handler->device, SOAPY_SDR_TX, 0, gain) != 0)
+  {
+    printf("setGain fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return rf_soapy_get_tx_gain(h);
 }
 
+
 double rf_soapy_get_rx_gain(void *h)
 {
-    
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);   
-
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_RX,0);
 }
 
+
 double rf_soapy_get_tx_gain(void *h)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0); 
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  return SoapySDRDevice_getGain(handler->device,SOAPY_SDR_TX,0);
 }
 
+
 double rf_soapy_set_rx_freq(void *h, double freq)
 {
-    
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
-    {
-        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
-    printf("Frequency has been set to %f : \n",ret);
-    return ret;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_RX, 0, freq, NULL) != 0)
+  {
+    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
   
+  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
 }
 
 double rf_soapy_set_tx_freq(void *h, double freq)
 {
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
-    {
-        printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
-        return SRSLTE_ERROR;
-    }
-    double ret = SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
-    printf("Frequency has been set to %f : \n",ret);
-    return ret;
-  
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if (SoapySDRDevice_setFrequency(handler->device, SOAPY_SDR_TX, 0, freq, NULL) != 0)
+  {
+    printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
 }
 
 
 void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs) {
-  
+
 }
 
 //TODO: add multi-channel support
 int  rf_soapy_recv_with_time_multi(void *h,
-                                            void **data,
-                                            uint32_t nsamples,
-                                            bool blocking,
-                                            time_t *secs,
-                                            double *frac_secs)
-{
-     rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-        //void *buffs[] = {buff}; //array of buffers
-        
-        int flags; //flags set by receive operation
-		
-	int num_channels = 1; // temp
-        
-        int trials = 0;
-        int ret = 0;
-        long long timeNs; //timestamp for receive buffer
-        int n = 0;
-        do{
-            
-            size_t rx_samples = nsamples;    
-            
-            if (rx_samples > nsamples - n) 
-            {
-                rx_samples = nsamples - n; 
-            }
-            void *buffs_ptr[4]; 
-            for (int i=0;i<num_channels;i++) 
-            {
-                cf_t *data_c = (cf_t*) data[i];
-                buffs_ptr[i] = &data_c[n];        
-            }   //(void*)(&data)
-            ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
-                
-            if(ret < 0)
-                return SRSLTE_ERROR;
-         n += ret;
-        trials++;
-        }while (n < nsamples && trials < 100);
-        
-        
-        //*secs = timeNs / 1000000000;
-        //*frac_secs = (timeNs % 1000000000)/1000000000;        
-       // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
-        return n;  
+                                   void **data,
+                                   uint32_t nsamples,
+                                   bool blocking,
+                                   time_t *secs,
+                                   double *frac_secs)
+{
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  //void *buffs[] = {buff}; //array of buffers
+  
+  int flags; //flags set by receive operation
+  
+  int num_channels = 1; // temp
+  
+  int trials = 0;
+  int ret = 0;
+  long long timeNs; //timestamp for receive buffer
+  int n = 0;
+  do {
+    size_t rx_samples = nsamples;
     
- 
+    if (rx_samples > nsamples - n)
+    {
+      rx_samples = nsamples - n;
+    }
+    void *buffs_ptr[4];
+    for (int i=0; i<num_channels; i++)
+    {
+      cf_t *data_c = (cf_t*) data[i];
+      buffs_ptr[i] = &data_c[n];
+    }   //(void*)(&data)
+    ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
+    if(ret < 0) {
+      // continue when getting overflows
+      if (ret == SOAPY_SDR_OVERFLOW) {
+        fprintf(stderr, "O");
+        fflush(stderr);
+        continue;
+      } else {
+        return SRSLTE_ERROR;
+      }
+    }
+
+    n += ret;
+    trials++;
+  } while (n < nsamples && trials < 100);
+  
+  
+  //*secs = timeNs / 1000000000;
+  //*frac_secs = (timeNs % 1000000000)/1000000000;
+  // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
+  return n;
 }
 
 int rf_soapy_recv_with_time(void *h,
-                                  void *data,
-                                  uint32_t nsamples,
-                                  bool blocking,
-                                  time_t *secs,
-                                  double *frac_secs) 
+                            void *data,
+                            uint32_t nsamples,
+                            bool blocking,
+                            time_t *secs,
+                            double *frac_secs)
 {
-       return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
+  return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
 }
-            
+
 
 int rf_soapy_send_timed(void *h,
                      void *data,
@@ -444,27 +434,33 @@ int rf_soapy_send_timed(void *h,
     
     int flags;
     long long timeNs;
-        int trials = 0;
-        int ret = 0;
+    int trials = 0;
+    int ret = 0;
     rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
     timeNs = secs * 1000000000;
     timeNs = timeNs + (frac_secs * 1000000000);
-	int num_channels = 1;
-	int n = 0;
+    int num_channels = 1;
+    int n = 0;
+    
+    if(!handler->tx_stream_active){
+      rf_soapy_start_tx_stream(h);
+    }
+    
+    
     cf_t *data_c = (cf_t*) data;
  	do{
-		size_t tx_samples = nsamples;
-		if (tx_samples > nsamples - n) 
-		{
-		    tx_samples = nsamples - n; 
-		}
-	      void *buff = (void*) &data_c[n];
-     	  const void *buffs_ptr[1] = {buff};
-			ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
-		if(ret < 0)
-		            return SRSLTE_ERROR;
-		 n += ret;
-		 trials++;
+      size_t tx_samples = nsamples;
+      if (tx_samples > nsamples - n) 
+      {
+          tx_samples = nsamples - n; 
+      }
+          void *buff = (void*) &data_c[n];
+          const void *buffs_ptr[1] = {buff};
+        ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
+      if(ret < 0)
+                  return SRSLTE_ERROR;
+       n += ret;
+       trials++;
      }while (n < nsamples && trials < 100);
 
 	
@@ -476,7 +472,3 @@ int rf_soapy_send_timed(void *h,
     return ret;
 
 }
-
-
-
-

From 151f219e41b5f6aff3543757c8f9111db357b2a8 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 13 Jul 2017 13:32:58 +0100
Subject: [PATCH 052/269] small soapy fix

---
 lib/src/phy/rf/rf_soapy_imp.c | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 484b412ae..94e5c7e2e 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -129,15 +129,12 @@ int rf_soapy_start_rx_stream(void *h)
 
 int rf_soapy_start_tx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  
+rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
   if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
     return SRSLTE_ERROR;
 
+  handler->tx_stream_active = true;
+
   return SRSLTE_SUCCESS;
 }
 
@@ -224,6 +221,13 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
     printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
+
+  
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+  
   
   return SRSLTE_SUCCESS;
 }

From c9ced0b2e1473e448b4f409339d2af373b470957 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 13 Jul 2017 13:44:09 +0100
Subject: [PATCH 053/269] fixing commits

---
 lib/src/phy/rf/rf_soapy_imp.c | 55 -----------------------------------
 1 file changed, 55 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 40fc37441..760b9b880 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -126,24 +126,12 @@ int rf_soapy_start_rx_stream(void *h)
 
 int rf_soapy_start_tx_stream(void *h)
 {
-<<<<<<< HEAD
 rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
   if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
     return SRSLTE_ERROR;
 
   handler->tx_stream_active = true;
 
-=======
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  
-  if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
-    return SRSLTE_ERROR;
-
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
   return SRSLTE_SUCCESS;
 }
 
@@ -225,27 +213,16 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
   bzero(handler, sizeof(rf_soapy_handler_t));
   *h = handler;
   handler->device = sdr;
-<<<<<<< HEAD
   handler->tx_stream_active = false;
   if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
     printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
 
-  
   if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-=======
-  
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
     printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-  
-<<<<<<< HEAD
-  
-=======
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
   return SRSLTE_SUCCESS;
 }
 
@@ -330,11 +307,7 @@ double rf_soapy_set_tx_gain(void *h, double gain)
     printf("setGain fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-<<<<<<< HEAD
   return rf_soapy_get_tx_gain(h);
-=======
-  return rf_soapy_get_rx_gain(h);
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
 }
 
 
@@ -372,11 +345,7 @@ double rf_soapy_set_tx_freq(void *h, double freq)
     printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-<<<<<<< HEAD
   return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
-=======
-  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
 }
 
 
@@ -448,7 +417,6 @@ int rf_soapy_recv_with_time(void *h,
 {
   return rf_soapy_recv_with_time_multi(h, &data, nsamples, blocking, secs, frac_secs);
 }
-<<<<<<< HEAD
 
 
 int rf_soapy_send_timed(void *h,
@@ -500,28 +468,5 @@ int rf_soapy_send_timed(void *h,
     
     
     return ret;
-=======
 
->>>>>>> 266f1b08fbf508cda5f1d2db3451776fd14c56cb
-
-int rf_soapy_send_timed(void *h,
-                        void *data,
-                        int nsamples,
-                        time_t secs,
-                        double frac_secs,
-                        bool has_time_spec,
-                        bool blocking,
-                        bool is_start_of_burst,
-                        bool is_end_of_burst)
-{
-  int flags;
-  long long timeNs;
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  timeNs = secs * 1000000000;
-  timeNs = timeNs + (frac_secs * 1000000000);
-  int ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, data, nsamples, &flags, timeNs,  100000);
-  if(ret != nsamples)
-    return SRSLTE_ERROR;
-  
-  return ret;
 }

From 639227557fb26a2d04d5e018521cf0f3a5362062 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 13 Jul 2017 13:51:09 +0100
Subject: [PATCH 054/269] fixing small conflicts

---
 lib/src/upper/rlc_am.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index e8e2e2803..f0055a534 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -218,7 +218,7 @@ uint32_t rlc_am::get_total_buffer_state()
   // Room needed for fixed header?
   if(n_bytes > 0) {
     n_bytes += 2;
-    log->debug("Total buffer state LCID=%d - tx SDUs: %d bytes\n", lcid, n_bytes);
+    log->debug("Buffer state - tx SDUs: %d bytes\n", n_bytes);
   }
 
   pthread_mutex_unlock(&mutex);
@@ -235,7 +235,7 @@ uint32_t rlc_am::get_buffer_state()
   check_reordering_timeout();
   if(do_status && !status_prohibited()) {
     n_bytes = prepare_status();
-    log->debug("Buffer state LCID=%d - status report: %d bytes\n", lcid, n_bytes);
+    log->debug("Buffer state - status report: %d bytes\n", n_bytes);
     goto unlock_and_return;
   }
 
@@ -268,7 +268,7 @@ uint32_t rlc_am::get_buffer_state()
   // Room needed for fixed header?
   if(n_bytes > 0) {
     n_bytes += 2;
-    log->debug("Buffer state LCID=%d - tx SDUs: %d bytes\n", lcid, n_bytes);
+    log->debug("Buffer state - tx SDUs: %d bytes\n", n_bytes);
   }
 
 unlock_and_return:

From 12d58bbb625395cc276e04e99a59ccfde94c24c4 Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Thu, 13 Jul 2017 15:14:22 +0100
Subject: [PATCH 055/269] fixing soapy Tx for lime and RTL-SDR

---
 lib/src/phy/rf/rf_soapy_imp.c | 95 ++++++++++++++++++++++++-----------
 1 file changed, 66 insertions(+), 29 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 8fcf57c24..760b9b880 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -43,9 +43,14 @@ typedef struct {
     SoapySDRRange *ranges;
     SoapySDRStream *rxStream;
     SoapySDRStream *txStream;
+    bool tx_stream_active;
 } rf_soapy_handler_t;
 
 
+cf_t zero_mem[64*1024];
+
+
+
 int soapy_error(void *h)
 {
   return 0;
@@ -82,6 +87,7 @@ void rf_soapy_register_error_handler(void *notused, srslte_rf_error_handler_t ne
 }
 
 
+
 char* rf_soapy_devname(void* h)
 {
   return "soapy";
@@ -120,15 +126,12 @@ int rf_soapy_start_rx_stream(void *h)
 
 int rf_soapy_start_tx_stream(void *h)
 {
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
-  
+rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
   if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
     return SRSLTE_ERROR;
 
+  handler->tx_stream_active = true;
+
   return SRSLTE_SUCCESS;
 }
 
@@ -210,12 +213,16 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
   bzero(handler, sizeof(rf_soapy_handler_t));
   *h = handler;
   handler->device = sdr;
-  
+  handler->tx_stream_active = false;
   if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
     printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-  
+
+  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
   return SRSLTE_SUCCESS;
 }
 
@@ -300,7 +307,7 @@ double rf_soapy_set_tx_gain(void *h, double gain)
     printf("setGain fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-  return rf_soapy_get_rx_gain(h);
+  return rf_soapy_get_tx_gain(h);
 }
 
 
@@ -338,7 +345,7 @@ double rf_soapy_set_tx_freq(void *h, double freq)
     printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
+  return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
 }
 
 
@@ -413,23 +420,53 @@ int rf_soapy_recv_with_time(void *h,
 
 
 int rf_soapy_send_timed(void *h,
-                        void *data,
-                        int nsamples,
-                        time_t secs,
-                        double frac_secs,
-                        bool has_time_spec,
-                        bool blocking,
-                        bool is_start_of_burst,
-                        bool is_end_of_burst)
-{
-  int flags;
-  long long timeNs;
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  timeNs = secs * 1000000000;
-  timeNs = timeNs + (frac_secs * 1000000000);
-  int ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, data, nsamples, &flags, timeNs,  100000);
-  if(ret != nsamples)
-    return SRSLTE_ERROR;
-  
-  return ret;
+                     void *data,
+                     int nsamples,
+                     time_t secs,
+                     double frac_secs,                      
+                     bool has_time_spec,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst) 
+{
+    
+    int flags;
+    long long timeNs;
+    int trials = 0;
+    int ret = 0;
+    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+    timeNs = secs * 1000000000;
+    timeNs = timeNs + (frac_secs * 1000000000);
+    int num_channels = 1;
+    int n = 0;
+    
+    if(!handler->tx_stream_active){
+      rf_soapy_start_tx_stream(h);
+    }
+    
+    
+    cf_t *data_c = (cf_t*) data;
+ 	do{
+      size_t tx_samples = nsamples;
+      if (tx_samples > nsamples - n) 
+      {
+          tx_samples = nsamples - n; 
+      }
+          void *buff = (void*) &data_c[n];
+          const void *buffs_ptr[1] = {buff};
+        ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
+      if(ret < 0)
+                  return SRSLTE_ERROR;
+       n += ret;
+       trials++;
+     }while (n < nsamples && trials < 100);
+
+	
+    if(ret != nsamples)
+        return SRSLTE_ERROR;
+    
+    
+    
+    return ret;
+
 }

From 8c049b2981ec3a53e4ef218a42c8dafc729e68f6 Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Fri, 14 Jul 2017 09:41:00 +0100
Subject: [PATCH 056/269] fix for both lime and rtl

---
 lib/src/phy/rf/rf_soapy_imp.c | 42 +++++++++++++++++++++++++----------
 1 file changed, 30 insertions(+), 12 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 760b9b880..ff57870ed 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -44,6 +44,7 @@ typedef struct {
     SoapySDRStream *rxStream;
     SoapySDRStream *txStream;
     bool tx_stream_active;
+    bool rx_stream_active;
 } rf_soapy_handler_t;
 
 
@@ -116,7 +117,16 @@ void rf_soapy_set_rx_cal(void *h, srslte_rf_cal_t *cal)
 int rf_soapy_start_rx_stream(void *h)
 {
   rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-
+    if(!handler->rxStream){
+      if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+      printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+      return SRSLTE_ERROR;
+      
+      
+      usleep(100000); 
+      handler->rx_stream_active = true;
+    }
+  }
   if (SoapySDRDevice_activateStream(handler->device, handler->rxStream, 0, 0, 0) != 0)
     return SRSLTE_ERROR;
   
@@ -126,11 +136,22 @@ int rf_soapy_start_rx_stream(void *h)
 
 int rf_soapy_start_tx_stream(void *h)
 {
-rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
+  if(!handler->txStream){
+      if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
+      printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
+      return SRSLTE_ERROR;
+    }
+      
+      usleep(100000); 
+      handler->tx_stream_active = true;
+  }
+  
+  
   if(SoapySDRDevice_activateStream(handler->device, handler->txStream, 0, 0, 0) != 0)
     return SRSLTE_ERROR;
 
-  handler->tx_stream_active = true;
+  
 
   return SRSLTE_SUCCESS;
 }
@@ -214,15 +235,8 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
   *h = handler;
   handler->device = sdr;
   handler->tx_stream_active = false;
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
 
-  if (SoapySDRDevice_setupStream(handler->device, &(handler->txStream), SOAPY_SDR_TX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
-    printf("setupStream fail: %s\n", SoapySDRDevice_lastError());
-    return SRSLTE_ERROR;
-  }
+
   return SRSLTE_SUCCESS;
 }
 
@@ -372,6 +386,10 @@ int  rf_soapy_recv_with_time_multi(void *h,
   int ret = 0;
   long long timeNs; //timestamp for receive buffer
   int n = 0;
+  
+  if(!handler->rxStream){
+      rf_soapy_start_tx_stream(h);
+    }
   do {
     size_t rx_samples = nsamples;
     
@@ -440,7 +458,7 @@ int rf_soapy_send_timed(void *h,
     int num_channels = 1;
     int n = 0;
     
-    if(!handler->tx_stream_active){
+    if(!handler->txStream){
       rf_soapy_start_tx_stream(h);
     }
     

From bb34be24aca51d45d3069bfd967a4c4f4cfdbcd7 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 14 Jul 2017 10:41:44 +0200
Subject: [PATCH 057/269] fix bug with uninitalized number of rx antennas

---
 lib/src/phy/ue/ue_cell_search.c | 2 +-
 srsue/src/phy/phy.cc            | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c
index f8c38a1bb..f7d244387 100644
--- a/lib/src/phy/ue/ue_cell_search.c
+++ b/lib/src/phy/ue/ue_cell_search.c
@@ -95,7 +95,7 @@ int srslte_ue_cellsearch_init_multi(srslte_ue_cellsearch_t * q, uint32_t max_fra
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL) {
+  if (q != NULL && nof_rx_antennas > 0) {
     ret = SRSLTE_ERROR;
     srslte_cell_t cell;
 
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index cf673cce6..dea66346b 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -59,6 +59,7 @@ phy::phy() : workers_pool(MAX_WORKERS),
 
 void phy::set_default_args(phy_args_t *args)
 {
+  args->nof_rx_ant          = 1;
   args->ul_pwr_ctrl_en      = false; 
   args->prach_gain          = -1;
   args->cqi_max             = -1; 

From 588dc7978d27b9cf44b730558bb6e87587997011 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 14 Jul 2017 10:46:27 +0200
Subject: [PATCH 058/269] gracefully exit MAC test when radio init fails

---
 srsue/test/mac/mac_test.cc | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/srsue/test/mac/mac_test.cc b/srsue/test/mac/mac_test.cc
index cb2ad2945..368a7f35e 100644
--- a/srsue/test/mac/mac_test.cc
+++ b/srsue/test/mac/mac_test.cc
@@ -458,7 +458,9 @@ int main(int argc, char *argv[])
   }
   
   // Init Radio and PHY
-  radio.init();
+  if (!radio.init()) {
+    exit(1);
+  }
   phy.init(&radio, &mac, NULL, &phy_log);
   if (prog_args.rf_rx_gain > 0 && prog_args.rf_tx_gain > 0) {
     radio.set_rx_gain(prog_args.rf_rx_gain);

From fa741f24e705eee752735704fdc36725c7e507a2 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 25 Jul 2017 10:43:20 +0200
Subject: [PATCH 059/269] Added .gitignore

---
 .gitignore | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)
 create mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 000000000..694c93182
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,21 @@
+# Ignore build directories
+build
+debug
+cmake-build-debug
+
+# Ignore CLion IDE project files
+.idea
+
+# Generated data files
+*.dat
+
+# Temporal files
+*~
+
+# Internal development
+matlab
+mex
+cmake/modules/BuildMex.cmake
+cmake/modules/FindMATLAB.cmake
+cmake/modules/FindOCTAVE.cmake
+

From 6142a5f9e50ae522dff3a2bc669ab08adbae1ab5 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 25 Jul 2017 13:17:36 +0200
Subject: [PATCH 060/269] Merge to next_with_matlab

---
 lib/examples/cell_measurement.c               |  13 +-
 lib/examples/pdsch_enodeb.c                   | 202 +++++++---
 lib/examples/pdsch_ue.c                       |  73 +++-
 lib/examples/usrp_capture.c                   |  36 +-
 lib/examples/usrp_capture_sync.c              |  25 +-
 .../srslte/phy/ch_estimation/chest_dl.h       |   3 +
 lib/include/srslte/phy/common/phy_common.h    |   2 +
 lib/include/srslte/phy/io/filesink.h          |   5 +
 lib/include/srslte/phy/io/filesource.h        |   5 +
 lib/include/srslte/phy/phch/pdcch.h           |  15 +
 lib/include/srslte/phy/phch/pdsch.h           |  39 +-
 lib/include/srslte/phy/phch/pdsch_cfg.h       |   5 +
 lib/include/srslte/phy/phch/ra.h              |   7 +
 lib/include/srslte/phy/phch/sch.h             |  12 +
 lib/include/srslte/phy/rf/rf.h                |  18 +
 lib/include/srslte/phy/ue/ue_dl.h             |  13 +-
 lib/include/srslte/phy/ue/ue_sync.h           |   7 +
 lib/include/srslte/phy/utils/debug.h          |   1 +
 lib/src/phy/ch_estimation/chest_dl.c          |  15 +-
 lib/src/phy/ch_estimation/test/CMakeLists.txt |   1 -
 lib/src/phy/common/phy_common.c               |  19 +-
 lib/src/phy/io/filesink.c                     |  57 +++
 lib/src/phy/io/filesource.c                   |  26 ++
 lib/src/phy/mimo/precoding.c                  | 156 +++++++-
 lib/src/phy/mimo/test/CMakeLists.txt          |   3 +-
 lib/src/phy/mimo/test/layermap_test.c         |   2 +-
 lib/src/phy/mimo/test/precoder_mex.c          |  25 +-
 lib/src/phy/mimo/test/precoder_test.c         | 187 ++++++---
 lib/src/phy/mimo/test/predecoder_mex.c        | 137 ++++---
 lib/src/phy/phch/dci.c                        |  10 +
 lib/src/phy/phch/pdcch.c                      |  34 +-
 lib/src/phy/phch/pdsch.c                      | 297 +++++++++++++--
 lib/src/phy/phch/pucch.c                      |   2 +-
 lib/src/phy/phch/ra.c                         |  48 ++-
 lib/src/phy/phch/sch.c                        |  55 +++
 lib/src/phy/phch/test/CMakeLists.txt          |  21 +
 lib/src/phy/phch/test/dlsch_encode_test_mex.c |  32 +-
 lib/src/phy/phch/test/pdcch_test.c            | 224 ++++++++---
 lib/src/phy/phch/test/pdcch_test_mex.c        |  14 +-
 lib/src/phy/phch/test/pdsch_test.c            | 359 +++++++++++++-----
 lib/src/phy/phch/test/pdsch_test_mex.c        |   2 +-
 lib/src/phy/phch/test/prach_detect_test_mex.c |   2 +
 lib/src/phy/phch/test/pucch_encode_test_mex.c |   2 +-
 lib/src/phy/phch/test/pucch_test_mex.c        |   6 +-
 lib/src/phy/rf/rf_blade_imp.c                 |  16 +-
 lib/src/phy/rf/rf_blade_imp.h                 |  12 +-
 lib/src/phy/rf/rf_dev.h                       |  12 +-
 lib/src/phy/rf/rf_imp.c                       |  25 +-
 lib/src/phy/rf/rf_uhd_imp.c                   |  48 ++-
 lib/src/phy/rf/rf_uhd_imp.h                   |  11 +
 lib/src/phy/ue/ue_dl.c                        |  63 ++-
 lib/src/phy/ue/ue_sync.c                      |  31 +-
 srsue/hdr/phy/phch_worker.h                   |   1 +
 srsue/src/phy/phch_worker.cc                  |  10 +-
 54 files changed, 1945 insertions(+), 501 deletions(-)

diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index bc8262b29..9c7856daa 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -118,7 +118,7 @@ int  parse_args(prog_args_t *args, int argc, char **argv) {
 /**********************************************************************/
 
 /* TODO: Do something with the output data */
-uint8_t data[1000000];
+uint8_t *data[SRSLTE_MAX_CODEWORDS];
 
 bool go_exit = false; 
 void sig_int_handler(int signo)
@@ -183,6 +183,9 @@ int main(int argc, char **argv) {
   }
   
   sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t) * 1500*8);
+  }
 
   sigset_t sigset;
   sigemptyset(&sigset);
@@ -322,7 +325,7 @@ int main(int argc, char **argv) {
               nof_trials++; 
             } else {
               printf("Decoded SIB1. Payload: ");
-              srslte_vec_fprint_byte(stdout, data, n/8);;
+              srslte_vec_fprint_byte(stdout, data[0], n/8);;
               state = MEASURE;
             }
           }
@@ -386,6 +389,12 @@ int main(int argc, char **argv) {
     sf_cnt++;                  
   } // Main loop
 
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
+  }
+
   srslte_ue_sync_free(&ue_sync);
   srslte_rf_close(&rf);
   printf("\nBye\n");
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 800e04de9..01e06c4c3 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -27,12 +27,12 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
-#include <sys/select.h>
 #include <pthread.h>
 #include <semaphore.h>
 #include <signal.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
 
 #include "srslte/srslte.h"
 
@@ -55,19 +55,21 @@ char *output_file_name = NULL;
 #define DOWN_KEY  66
 
 srslte_cell_t cell = {
-  25,            // nof_prb
-  1,            // nof_ports
-  0,            // cell_id
-  SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+    25,                 // nof_prb
+    1,                  // nof_ports
+    0,                  // cell_id
+    SRSLTE_CP_NORM,     // cyclic prefix
+    SRSLTE_PHICH_NORM,  // PHICH length
+    SRSLTE_PHICH_R_1    // PHICH resources
 };
-  
+
 int net_port = -1; // -1 generates random dataThat means there is some problem sending samples to the device
 
 uint32_t cfi=3;
 uint32_t mcs_idx = 1, last_mcs_idx = 1;
 int nof_frames = -1;
+char mimo_type_str[32] = "single";
+uint32_t nof_tb = 1;
 
 char *rf_args = "";
 float rf_amp = 0.8, rf_gain = 70.0, rf_freq = 2400000000;
@@ -80,14 +82,14 @@ srslte_pcfich_t pcfich;
 srslte_pdcch_t pdcch;
 srslte_pdsch_t pdsch;
 srslte_pdsch_cfg_t pdsch_cfg; 
-srslte_softbuffer_tx_t softbuffer; 
+srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS];
 srslte_regs_t regs;
 srslte_ra_dl_dci_t ra_dl;  
 
-cf_t *sf_buffer = NULL, *output_buffer = NULL;
+cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL}, *output_buffer [SRSLTE_MAX_PORTS] = {NULL};
 int sf_n_re, sf_n_samples;
 
-pthread_t net_thread; 
+pthread_t net_thread;
 void *net_thread_fnc(void *arg);
 sem_t net_sem;
 bool net_packet_ready = false; 
@@ -99,7 +101,7 @@ int prbset_orig = 0;
 
 
 void usage(char *prog) {
-  printf("Usage: %s [agmfoncvpu]\n", prog);
+  printf("Usage: %s [agmfoncvpuM]\n", prog);
 #ifndef DISABLE_RF
   printf("\t-a RF args [Default %s]\n", rf_args);
   printf("\t-l RF amplitude [Default %.2f]\n", rf_amp);
@@ -113,13 +115,14 @@ void usage(char *prog) {
   printf("\t-n number of frames [Default %d]\n", nof_frames);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-p nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-M Transmission mode[single|diversity|cdd] [Default %s]\n", mimo_type_str);
   printf("\t-u listen TCP port for input data (-1 is random) [Default %d]\n", net_port);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "aglfmoncpvu")) != -1) {
+  while ((opt = getopt(argc, argv, "aglfmoncpvutM")) != -1) {
     switch (opt) {
     case 'a':
       rf_args = argv[optind];
@@ -151,6 +154,9 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
+    case 'M':
+      strncpy(mimo_type_str, argv[optind], 32);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -168,18 +174,54 @@ void parse_args(int argc, char **argv) {
 }
 
 void base_init() {
-  
-  /* init memory */
-  sf_buffer = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
-  if (!sf_buffer) {
-    perror("malloc");
+  int i;
+
+  /* Select transmission mode */
+  if (srslte_str2mimotype(mimo_type_str, &pdsch_cfg.mimo_type)) {
+    ERROR("Wrong transmission mode! Allowed modes: single, diversity, cdd");
     exit(-1);
   }
-  output_buffer = srslte_vec_malloc(sizeof(cf_t) * sf_n_samples);
-  if (!output_buffer) {
-    perror("malloc");
-    exit(-1);
+
+  /* Configure cell and PDSCH in function of the transmission mode */
+  switch(pdsch_cfg.mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      cell.nof_ports = 1;
+      pdsch_cfg.nof_layers = 1;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      cell.nof_ports = 2;
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      cell.nof_ports = 2;
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 2;
+      break;
+    default:
+      ERROR("Transmission mode not implemented.");
+      exit(-1);
   }
+
+  /* init memory */
+  for (i = 0; i < cell.nof_ports; i++) {
+    sf_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
+    if (!sf_buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+  }
+
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    output_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_samples);
+    if (!output_buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+    bzero(output_buffer[i], sizeof(cf_t) * sf_n_samples);
+  }
+
   /* open file or USRP */
   if (output_file_name) {
     if (strcmp(output_file_name, "NULL")) {
@@ -194,7 +236,7 @@ void base_init() {
   } else {
 #ifndef DISABLE_RF
     printf("Opening RF device...\n");
-    if (srslte_rf_open(&rf, rf_args)) {
+    if (srslte_rf_open_multi2(&rf, rf_args, cell.nof_ports, 1)) {
       fprintf(stderr, "Error opening rf\n");
       exit(-1);
     }
@@ -247,27 +289,32 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init_tx(&pdcch, &regs, cell)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
 
-  if (srslte_pdsch_init(&pdsch, cell)) {
+  bzero(&pdsch, sizeof(srslte_pdsch_t));
+  if (srslte_pdsch_init_tx_multi(&pdsch, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  
+
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
-  
-  if (srslte_softbuffer_tx_init(&softbuffer, cell.nof_prb)) {
-    fprintf(stderr, "Error initiating soft buffer\n");
-    exit(-1);
+
+  for (i = 0; i < nof_tb; i++) {
+    if (srslte_softbuffer_tx_init(&softbuffers[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating soft buffer\n");
+      exit(-1);
+    }
   }
 }
 
 void base_free() {
-
-  srslte_softbuffer_tx_free(&softbuffer);
+  int i;
+  for (i = 0; i < nof_tb; i++) {
+    srslte_softbuffer_tx_free(&softbuffers[i]);
+  }
   srslte_pdsch_free(&pdsch);
   srslte_pdcch_free(&pdcch);
   srslte_regs_free(&regs);
@@ -275,11 +322,14 @@ void base_free() {
 
   srslte_ofdm_tx_free(&ifft);
 
-  if (sf_buffer) {
-    free(sf_buffer);
-  }
-  if (output_buffer) {
-    free(output_buffer);
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
+
+    if (output_buffer[i]) {
+      free(output_buffer[i]);
+    }
   }
   if (output_file_name) {
     if (!null_file_sink) {
@@ -340,7 +390,14 @@ int update_radl() {
   ra_dl.rv_idx = 0;
   ra_dl.alloc_type = SRSLTE_RA_ALLOC_TYPE0;
   ra_dl.type0_alloc.rbg_bitmask = prbset_to_bitmask();
-  ra_dl.tb_en[0] = 1; 
+  ra_dl.tb_en[0] = 1;
+
+  if (nof_tb > 1) {
+    ra_dl.mcs_idx_1 = mcs_idx;
+    ra_dl.ndi_1 = 0;
+    ra_dl.rv_idx_1 = 0;
+    ra_dl.tb_en[1] = 1;
+  }
 
   srslte_ra_pdsch_fprint(stdout, &ra_dl, cell.nof_prb);
   srslte_ra_dl_grant_t dummy_grant; 
@@ -497,9 +554,9 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  for (i = 0; i < SRSLTE_MAX_PORTS; i++) { // now there's only 1 port
-    sf_symbols[i] = sf_buffer;
-    slot1_symbols[i] = &sf_buffer[SRSLTE_SLOT_LEN_RE(cell.nof_prb, cell.cp)];
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    sf_symbols[i] = sf_buffer[i%cell.nof_ports];
+    slot1_symbols[i] = &sf_buffer[i%cell.nof_ports][SRSLTE_SLOT_LEN_RE(cell.nof_prb, cell.cp)];
   }
 
 #ifndef DISABLE_RF
@@ -556,7 +613,9 @@ int main(int argc, char **argv) {
   nf = 0;
   
   bool send_data = false; 
-  srslte_softbuffer_tx_reset(&softbuffer);
+  for (i = 0; i < nof_tb; i++) {
+    srslte_softbuffer_tx_reset(&softbuffers[i]);
+  }
 
 #ifndef DISABLE_RF
   bool start_of_burst = true; 
@@ -564,15 +623,25 @@ int main(int argc, char **argv) {
   
   while ((nf < nof_frames || nof_frames == -1) && !go_exit) {
     for (sf_idx = 0; sf_idx < SRSLTE_NSUBFRAMES_X_FRAME && (nf < nof_frames || nof_frames == -1); sf_idx++) {
-      bzero(sf_buffer, sizeof(cf_t) * sf_n_re);
+      /* Set Antenna port resource elements to zero */
+      bzero(sf_symbols[0], sizeof(cf_t) * sf_n_re);
 
+      /* Populate Synchronization signals if required */
       if (sf_idx == 0 || sf_idx == 5) {
-        srslte_pss_put_slot(pss_signal, sf_buffer, cell.nof_prb, SRSLTE_CP_NORM);
-        srslte_sss_put_slot(sf_idx ? sss_signal5 : sss_signal0, sf_buffer, cell.nof_prb,
+        srslte_pss_put_slot(pss_signal, sf_symbols[0], cell.nof_prb, SRSLTE_CP_NORM);
+        srslte_sss_put_slot(sf_idx ? sss_signal5 : sss_signal0, sf_symbols[0], cell.nof_prb,
             SRSLTE_CP_NORM);
       }
 
-      srslte_refsignal_cs_put_sf(cell, 0, est.csr_signal.pilots[0][sf_idx], sf_buffer);
+      /* Copy zeros, SSS, PSS into the rest of antenna ports */
+      for (i = 1; i < cell.nof_ports; i++) {
+        memcpy(sf_symbols[i], sf_symbols[0], sizeof(cf_t) * sf_n_re);
+      }
+
+      /* Put reference signals */
+      for (i = 0; i < cell.nof_ports; i++) {
+        srslte_refsignal_cs_put_sf(cell, (uint32_t) i, est.csr_signal.pilots[i / 2][sf_idx], sf_symbols[i]);
+      }
 
       srslte_pbch_mib_pack(&cell, sfn, bch_payload);
       if (sf_idx == 0) {
@@ -606,10 +675,23 @@ int main(int argc, char **argv) {
       }        
       
       if (send_data) {
-              
+        srslte_dci_format_t dci_format = SRSLTE_DCI_FORMAT1;
+        switch(pdsch_cfg.mimo_type) {
+          case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+            dci_format = SRSLTE_DCI_FORMAT1;
+            break;
+          case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+          case SRSLTE_MIMO_TYPE_CDD:
+            dci_format = SRSLTE_DCI_FORMAT2A;
+            break;
+          case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+          default:
+            fprintf(stderr, "Wrong MIMO configuration\n");
+            exit(SRSLTE_ERROR);
+        }
         /* Encode PDCCH */
         INFO("Putting DCI to location: n=%d, L=%d\n", locations[sf_idx][0].ncce, locations[sf_idx][0].L);
-        srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_msg, cell.nof_prb, cell.nof_ports, false);
+        srslte_dci_msg_pack_pdsch(&ra_dl, dci_format, &dci_msg, cell.nof_prb, cell.nof_ports, false);
         if (srslte_pdcch_encode(&pdcch, &dci_msg, locations[sf_idx][0], UE_CRNTI, sf_symbols, sf_idx, cfi)) {
           fprintf(stderr, "Error encoding DCI message\n");
           exit(-1);
@@ -618,13 +700,14 @@ int main(int argc, char **argv) {
         /* Configure pdsch_cfg parameters */
         srslte_ra_dl_grant_t grant; 
         srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0)) {
+        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0, 0)) {
           fprintf(stderr, "Error configuring PDSCH\n");
           exit(-1);
         }
        
         /* Encode PDSCH */
-        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer, data, UE_CRNTI, sf_symbols)) {
+        if (srslte_pdsch_encode_multi(&pdsch, &pdsch_cfg, softbuffers, (uint8_t*[2]){data, data}, UE_CRNTI,
+                                      sf_symbols)) {
           fprintf(stderr, "Error encoding PDSCH\n");
           exit(-1);
         }        
@@ -641,21 +724,24 @@ int main(int argc, char **argv) {
       }
       
       /* Transform to OFDM symbols */
-      srslte_ofdm_tx_sf(&ifft, sf_buffer, output_buffer);
-      
+      for (i = 0; i < cell.nof_ports; i++) {
+        srslte_ofdm_tx_sf(&ifft, sf_buffer[i], output_buffer[i]);
+      }
+
       /* send to file or usrp */
       if (output_file_name) {
         if (!null_file_sink) {
-          srslte_filesink_write(&fsink, output_buffer, sf_n_samples);          
+          srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);
         }
         usleep(1000);
       } else {
 #ifndef DISABLE_RF
-        // FIXME
         float norm_factor = (float) cell.nof_prb/15/sqrtf(pdsch_cfg.grant.nof_prb);
-        srslte_vec_sc_prod_cfc(output_buffer, rf_amp*norm_factor, output_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        srslte_rf_send2(&rf, output_buffer, sf_n_samples, true, start_of_burst, false);
-        start_of_burst=false; 
+        for (i = 0; i < cell.nof_ports; i++) {
+          srslte_vec_sc_prod_cfc(output_buffer[i], rf_amp * norm_factor, output_buffer[i], SRSLTE_SF_LEN_PRB(cell.nof_prb));
+        }
+        srslte_rf_send_multi(&rf, (void**) output_buffer, sf_n_samples, true, start_of_burst, false);
+        start_of_burst=false;
 #endif
       }
     }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 72dc36731..bab9d7219 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -258,7 +258,7 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
 /**********************************************************************/
 
 /* TODO: Do something with the output data */
-uint8_t data[20000];
+uint8_t *data[SRSLTE_MAX_CODEWORDS];
 
 bool go_exit = false; 
 void sig_int_handler(int signo)
@@ -266,10 +266,12 @@ void sig_int_handler(int signo)
   printf("SIGINT received. Exiting...\n");
   if (signo == SIGINT) {
     go_exit = true;
+  } else if (signo == SIGSEGV) {
+    exit(1);
   }
 }
 
-cf_t *sf_buffer[2] = {NULL, NULL}; 
+cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL};
 
 #ifndef DISABLE_RF
 int srslte_rf_recv_wrapper(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *t) {
@@ -315,6 +317,14 @@ int main(int argc, char **argv) {
   
   parse_args(&prog_args, argc, argv);
   
+  for (int i = 0; i< SRSLTE_MAX_CODEWORDS; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t)*1500*8);
+    if (!data[i]) {
+      ERROR("Allocating data");
+      go_exit = true;
+    }
+  }
+
   if(prog_args.cpu_affinity > -1) {
     
     cpu_set_t cpuset;
@@ -435,8 +445,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_time, prog_args.file_offset_freq)) {
+    if (srslte_ue_sync_init_file_multi(&ue_sync, prog_args.file_nof_prb,
+      prog_args.input_file_name, prog_args.file_offset_time, prog_args.file_offset_freq, prog_args.rf_nof_rx_ant)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
     }
@@ -498,7 +508,7 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  float rsrp=0.0, rsrq=0.0, noise=0.0;
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0;
   bool decode_pdsch = false; 
 
 #ifndef DISABLE_RF
@@ -596,8 +606,9 @@ int main(int argc, char **argv) {
             nof_trials++; 
             
             rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1);
-            rsrp = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp(&ue_dl.chest), rsrp, 0.05);      
-            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);      
+            rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05);
+            rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05);
+            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -605,9 +616,12 @@ int main(int argc, char **argv) {
             if (isnan(noise)) {
               noise = 0; 
             }
-            if (isnan(rsrp)) {
-              rsrp = 0; 
-            }        
+            if (isnan(rsrp0)) {
+              rsrp1 = 0;
+            }
+            if (isnan(rsrp0)) {
+              rsrp1 = 0;
+            }
           }
 
           // Plot and Printf
@@ -616,14 +630,27 @@ int main(int argc, char **argv) {
             if (gain < 0) {
               gain = 10*log10(srslte_agc_get_gain(&ue_sync.agc)); 
             }
-            printf("CFO: %+6.2f kHz, "
-                   "SNR: %4.1f dB, "
-                   "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
-                   
-                  srslte_ue_sync_get_cfo(&ue_sync)/1000,
-                  10*log10(rsrp/noise), 
-                  100*(1-(float) ue_dl.nof_detected/nof_trials), 
-                  (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total);                        
+            if (cell.nof_ports == 1) {
+              printf("CFO: %+6.2f kHz, "
+                         "SNR: %4.1f dB, "
+                         "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
+
+                     srslte_ue_sync_get_cfo(&ue_sync) / 1000,
+                     10 * log10(rsrp0 / noise),
+                     100 * (1 - (float) ue_dl.nof_detected / nof_trials),
+                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+            } else {
+              printf("CFO: %+6.2f kHz, "
+                         "SNR port 0: %4.1f dB, "
+                         "SNR port 1: %4.1f dB, "
+                         "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
+
+                     srslte_ue_sync_get_cfo(&ue_sync) / 1000,
+                     10 * log10(rsrp0 / noise),
+                     10 * log10(rsrp1 / noise),
+                     100 * (1 - (float) ue_dl.nof_detected / nof_trials),
+                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+            }
           }
           break;
       }
@@ -674,6 +701,16 @@ int main(int argc, char **argv) {
 #endif
   srslte_ue_dl_free(&ue_dl);
   srslte_ue_sync_free(&ue_sync);
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
+  }
+  for (int i = 0; i < prog_args.rf_nof_rx_ant; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
+  }
   
 #ifndef DISABLE_RF
   if (!prog_args.input_file_name) {
diff --git a/lib/examples/usrp_capture.c b/lib/examples/usrp_capture.c
index 2692deb0d..02b543c0e 100644
--- a/lib/examples/usrp_capture.c
+++ b/lib/examples/usrp_capture.c
@@ -44,6 +44,7 @@ char *output_file_name;
 char *rf_args="";
 float rf_gain=40.0, rf_freq=-1.0, rf_rate=0.96e6;
 int nof_samples = -1;
+int nof_rx_antennas = 1;
 
 void int_handler(int dummy) {
   keep_running = false;
@@ -55,12 +56,13 @@ void usage(char *prog) {
   printf("\t-g RF Gain [Default %.2f dB]\n", rf_gain);
   printf("\t-r RF Rate [Default %.6f Hz]\n", rf_rate);
   printf("\t-n nof_samples [Default %d]\n", nof_samples);
+  printf("\t-A nof_rx_antennas [Default %d]\n", nof_rx_antennas);
   printf("\t-v srslte_verbose\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "agrnvfo")) != -1) {
+  while ((opt = getopt(argc, argv, "agrnvfoA")) != -1) {
     switch (opt) {
     case 'o':
       output_file_name = argv[optind];
@@ -80,6 +82,9 @@ void parse_args(int argc, char **argv) {
     case 'n':
       nof_samples = atoi(argv[optind]);
       break;
+    case 'A':
+      nof_rx_antennas = atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -95,11 +100,11 @@ void parse_args(int argc, char **argv) {
 }
 
 int main(int argc, char **argv) {
-  cf_t *buffer; 
+  cf_t *buffer[SRSLTE_MAX_PORTS];
   int sample_count, n;
   srslte_rf_t rf;
   srslte_filesink_t sink;
-  int32_t buflen;
+  uint32_t buflen;
 
   signal(SIGINT, int_handler);
 
@@ -107,17 +112,19 @@ int main(int argc, char **argv) {
   
   buflen = 4800;
   sample_count = 0;
-  
-  buffer = malloc(sizeof(cf_t) * buflen);
-  if (!buffer) {
-    perror("malloc");
-    exit(-1);
+
+  for (int i = 0; i < nof_rx_antennas; i++) {
+    buffer[i] = malloc(sizeof(cf_t) * buflen);
+    if (!buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
   }
 
   srslte_filesink_init(&sink, output_file_name, SRSLTE_COMPLEX_FLOAT_BIN);
 
   printf("Opening RF device...\n");
-  if (srslte_rf_open(&rf, rf_args)) {
+  if (srslte_rf_open_multi(&rf, rf_args, nof_rx_antennas)) {
     fprintf(stderr, "Error opening rf\n");
     exit(-1);
   }
@@ -151,18 +158,23 @@ int main(int argc, char **argv) {
   
   while((sample_count < nof_samples || nof_samples == -1)
         && keep_running){
-    n = srslte_rf_recv(&rf, buffer, buflen, 1);
+    n = srslte_rf_recv_with_time_multi(&rf, (void**) buffer, buflen, true, NULL, NULL);
     if (n < 0) {
       fprintf(stderr, "Error receiving samples\n");
       exit(-1);
     }
     
-    srslte_filesink_write(&sink, buffer, buflen);
+    srslte_filesink_write_multi(&sink, (void**) buffer, buflen, nof_rx_antennas);
     sample_count += buflen;
   }
   
+  for (int i = 0; i < nof_rx_antennas; i++) {
+    if (buffer[i]) {
+      free(buffer[i]);
+    }
+  }
+
   srslte_filesink_free(&sink);
-  free(buffer);
   srslte_rf_close(&rf);
 
   printf("Ok - wrote %d samples\n", sample_count);
diff --git a/lib/examples/usrp_capture_sync.c b/lib/examples/usrp_capture_sync.c
index 6d2ec18dc..41c233be7 100644
--- a/lib/examples/usrp_capture_sync.c
+++ b/lib/examples/usrp_capture_sync.c
@@ -45,6 +45,7 @@ float rf_gain=60.0, rf_freq=-1.0;
 int nof_prb = 6;
 int nof_subframes = -1;
 int N_id_2 = -1; 
+uint32_t nof_rx_antennas = 1;
 
 void int_handler(int dummy) {
   keep_running = false;
@@ -56,12 +57,13 @@ void usage(char *prog) {
   printf("\t-g RF Gain [Default %.2f dB]\n", rf_gain);
   printf("\t-p nof_prb [Default %d]\n", nof_prb);
   printf("\t-n nof_subframes [Default %d]\n", nof_subframes);
+  printf("\t-A nof_rx_antennas [Default %d]\n", nof_rx_antennas);
   printf("\t-v verbose\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "agpnvfol")) != -1) {
+  while ((opt = getopt(argc, argv, "agpnvfolA")) != -1) {
     switch (opt) {
     case 'o':
       output_file_name = argv[optind];
@@ -84,6 +86,9 @@ void parse_args(int argc, char **argv) {
     case 'l':
       N_id_2 = atoi(argv[optind]);
       break;
+    case 'A':
+      nof_rx_antennas = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -100,7 +105,7 @@ void parse_args(int argc, char **argv) {
 
 int srslte_rf_recv_wrapper(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *t) {
   DEBUG(" ----  Receive %d samples  ---- \n", nsamples);
-  return srslte_rf_recv(h, data[0], nsamples, 1);
+  return srslte_rf_recv_with_time_multi(h, (void**) data, nsamples, true, NULL, NULL);
 }
 
 int main(int argc, char **argv) {
@@ -118,13 +123,15 @@ int main(int argc, char **argv) {
   srslte_filesink_init(&sink, output_file_name, SRSLTE_COMPLEX_FLOAT_BIN);
 
   printf("Opening RF device...\n");
-  if (srslte_rf_open(&rf, rf_args)) {
+  if (srslte_rf_open_multi(&rf, rf_args, nof_rx_antennas)) {
     fprintf(stderr, "Error opening rf\n");
     exit(-1);
   }
   srslte_rf_set_master_clock_rate(&rf, 30.72e6);        
 
-  buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
+  for (int i = 0; i< SRSLTE_MAX_PORTS; i++) {
+    buffer[i] = srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(100));
+  }
   
   sigset_t sigset;
   sigemptyset(&sigset);
@@ -158,7 +165,7 @@ int main(int argc, char **argv) {
   cell.nof_prb = nof_prb; 
   cell.nof_ports = 1; 
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, nof_rx_antennas, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     exit(-1); 
   }
@@ -181,7 +188,7 @@ int main(int argc, char **argv) {
         }        
       } else {
         printf("Writing to file %6d subframes...\r", subframe_count);
-        srslte_filesink_write(&sink, buffer[0], SRSLTE_SF_LEN_PRB(nof_prb));
+        srslte_filesink_write_multi(&sink, (void**) buffer, SRSLTE_SF_LEN_PRB(nof_prb),nof_rx_antennas);
         subframe_count++;                              
       }      
     }
@@ -196,6 +203,12 @@ int main(int argc, char **argv) {
   srslte_rf_close(&rf);
   srslte_ue_sync_free(&ue_sync);
 
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    if (buffer[i]) {
+      free(buffer[i]);
+    }
+  }
+
   printf("Ok - wrote %d subframes\n", subframe_count);
   exit(0);
 }
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index a454f939c..5d0820aa4 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -132,6 +132,9 @@ SRSLTE_API float srslte_chest_dl_get_rssi(srslte_chest_dl_t *q);
 
 SRSLTE_API float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q);
 
+SRSLTE_API float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q,
+                                                     uint32_t port);
+
 SRSLTE_API float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q);
 
 #endif
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index efd498c86..b891b7330 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -244,6 +244,8 @@ SRSLTE_API uint32_t srslte_N_ta_new(uint32_t N_ta_old,
 
 SRSLTE_API char *srslte_cp_string(srslte_cp_t cp); 
 
+SRSLTE_API srslte_mod_t srslte_str2mod (char * mod_str);
+
 SRSLTE_API char *srslte_mod_string(srslte_mod_t mod);
 
 SRSLTE_API uint32_t srslte_mod_bits_x_symbol(srslte_mod_t mod);
diff --git a/lib/include/srslte/phy/io/filesink.h b/lib/include/srslte/phy/io/filesink.h
index b8ca8db51..921f1e440 100644
--- a/lib/include/srslte/phy/io/filesink.h
+++ b/lib/include/srslte/phy/io/filesink.h
@@ -59,4 +59,9 @@ SRSLTE_API int srslte_filesink_write(srslte_filesink_t *q,
                                      void *buffer, 
                                      int nsamples);
 
+SRSLTE_API int srslte_filesink_write_multi(srslte_filesink_t *q,
+                                           void **buffer,
+                                           int nsamples,
+                                           int nchannels);
+
 #endif // FILESINK_
diff --git a/lib/include/srslte/phy/io/filesource.h b/lib/include/srslte/phy/io/filesource.h
index ee8bc080d..f515d7034 100644
--- a/lib/include/srslte/phy/io/filesource.h
+++ b/lib/include/srslte/phy/io/filesource.h
@@ -62,4 +62,9 @@ SRSLTE_API int srslte_filesource_read(srslte_filesource_t *q,
                                       void *buffer, 
                                       int nsamples);
 
+SRSLTE_API int srslte_filesource_read_multi(srslte_filesource_t *q,
+                                            void **buffer,
+                                            int nsamples,
+                                            int nof_channels);
+
 #endif // FILESOURCE_
diff --git a/lib/include/srslte/phy/phch/pdcch.h b/lib/include/srslte/phy/phch/pdcch.h
index 8d4aba790..f67d29dcb 100644
--- a/lib/include/srslte/phy/phch/pdcch.h
+++ b/lib/include/srslte/phy/phch/pdcch.h
@@ -88,11 +88,26 @@ SRSLTE_API int srslte_pdcch_init(srslte_pdcch_t *q,
                                  srslte_regs_t *regs, 
                                  srslte_cell_t cell);
 
+SRSLTE_API int srslte_pdcch_init_tx(srslte_pdcch_t *q,
+                         srslte_regs_t *regs,
+                         srslte_cell_t cell);
+
+SRSLTE_API int srslte_pdcch_init_rx(srslte_pdcch_t *q,
+                         srslte_regs_t *regs,
+                         srslte_cell_t cell,
+                         uint32_t nof_rx_antennas);
+
 SRSLTE_API int srslte_pdcch_init_multi(srslte_pdcch_t *q, 
                                        srslte_regs_t *regs, 
                                        srslte_cell_t cell, 
                                        uint32_t nof_rx_antennas);
 
+SRSLTE_API int srslte_pdcch_init_txrx(srslte_pdcch_t *q,
+                                      srslte_regs_t *regs,
+                                      srslte_cell_t cell,
+                                      uint32_t nof_rx_antennas,
+                                      bool isReceiver);
+
 SRSLTE_API void srslte_pdcch_free(srslte_pdcch_t *q);
 
 
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index ad01c4ef8..7bdaaef9c 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -49,6 +49,7 @@
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  srslte_sequence_t seq2[SRSLTE_NSUBFRAMES_X_FRAME];
   bool sequence_generated;
 } srslte_pdsch_user_t;
 
@@ -62,11 +63,13 @@ typedef struct SRSLTE_API {
   
   /* buffers */
   // void buffers are shared for tx and rx
-  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  cf_t *symbols[SRSLTE_MAX_PORTS];
-  cf_t *x[SRSLTE_MAX_PORTS];
-  cf_t *d;
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; /* Channel estimation (Rx only) */
+  cf_t *symbols[SRSLTE_MAX_PORTS];              /* PDSCH Encoded/Decoded Symbols */
+  cf_t *x[SRSLTE_MAX_LAYERS];                   /* Layer mapped */
+  cf_t *d;                                      /* Modulated/Demodulated codeword 1 */
+  cf_t *d2;                                     /* Modulated/Demodulated codeword 2 */
   void *e;
+  void *e2;
 
   /* tx & rx objects */
   srslte_modem_table_t mod[4];
@@ -81,9 +84,12 @@ typedef struct SRSLTE_API {
 SRSLTE_API int srslte_pdsch_init(srslte_pdsch_t *q, 
                                  srslte_cell_t cell);
 
-SRSLTE_API int srslte_pdsch_init_multi(srslte_pdsch_t *q, 
-                                       srslte_cell_t cell, 
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_pdsch_init_tx_multi(srslte_pdsch_t *q,
+                                          srslte_cell_t cell);
+
+SRSLTE_API int srslte_pdsch_init_rx_multi(srslte_pdsch_t *q,
+                                          srslte_cell_t cell,
+                                          uint32_t nof_antennas);
 
 SRSLTE_API void srslte_pdsch_free(srslte_pdsch_t *q);
 
@@ -103,6 +109,14 @@ SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg,
                                 uint32_t sf_idx, 
                                 uint32_t rvidx); 
 
+SRSLTE_API int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg,
+                                      srslte_cell_t cell,
+                                      srslte_ra_dl_grant_t *grant,
+                                      uint32_t cfi,
+                                      uint32_t sf_idx,
+                                      uint32_t rvidx,
+                                      uint32_t rvidx2);
+
 SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
                                    srslte_pdsch_cfg_t *cfg,
                                    srslte_softbuffer_tx_t *softbuffer,
@@ -110,6 +124,13 @@ SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
                                    uint16_t rnti,
                                    cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
 
+SRSLTE_API int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
+                                         srslte_pdsch_cfg_t *cfg,
+                                         srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                         uint16_t rnti,
+                                         cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
+
 SRSLTE_API int srslte_pdsch_decode(srslte_pdsch_t *q, 
                                    srslte_pdsch_cfg_t *cfg, 
                                    srslte_softbuffer_rx_t *softbuffer,
@@ -121,12 +142,12 @@ SRSLTE_API int srslte_pdsch_decode(srslte_pdsch_t *q,
 
 SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
                                          srslte_pdsch_cfg_t *cfg, 
-                                         srslte_softbuffer_rx_t *softbuffer,
+                                         srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
                                          cf_t *sf_symbols[SRSLTE_MAX_PORTS], 
                                          cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                          float noise_estimate, 
                                          uint16_t rnti,
-                                         uint8_t *data);
+                                         uint8_t *data[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q); 
 
diff --git a/lib/include/srslte/phy/phch/pdsch_cfg.h b/lib/include/srslte/phy/phch/pdsch_cfg.h
index 2745470f8..bec326338 100644
--- a/lib/include/srslte/phy/phch/pdsch_cfg.h
+++ b/lib/include/srslte/phy/phch/pdsch_cfg.h
@@ -41,10 +41,15 @@
 
 typedef struct SRSLTE_API {
   srslte_cbsegm_t cb_segm; 
+  srslte_cbsegm_t cb_segm2;
   srslte_ra_dl_grant_t grant; 
   srslte_ra_nbits_t nbits; 
+  srslte_ra_nbits_t nbits2;
   uint32_t rv; 
+  uint32_t rv2;
   uint32_t sf_idx;  
+  uint32_t nof_layers;
+  srslte_mimo_type_t mimo_type;
 } srslte_pdsch_cfg_t;
 
 #endif
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index cec2e6087..3c7c3d93c 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -209,6 +209,13 @@ SRSLTE_API void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant,
                                             uint32_t sf_idx, 
                                             srslte_ra_nbits_t *nbits); 
 
+SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
+                                                  uint32_t cfi,
+                                                  srslte_cell_t cell,
+                                                  uint32_t sf_idx,
+                                                  srslte_ra_nbits_t *nbits,
+                                                  srslte_ra_nbits_t *nbits2);
+
 SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, 
                                                uint32_t nof_prb, 
                                                uint32_t nof_ctrl_symbols); 
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index 4ef0b3070..0e59ecbc9 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -96,12 +96,24 @@ SRSLTE_API int srslte_dlsch_encode(srslte_sch_t *q,
                                    uint8_t *data, 
                                    uint8_t *e_bits);
 
+SRSLTE_API int srslte_dlsch_encode_multi(srslte_sch_t *q,
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                   uint8_t *e_bits[SRSLTE_MAX_CODEWORDS]);
+
 SRSLTE_API int srslte_dlsch_decode(srslte_sch_t *q, 
                                    srslte_pdsch_cfg_t *cfg,
                                    srslte_softbuffer_rx_t *softbuffer,
                                    int16_t *e_bits, 
                                    uint8_t *data);
 
+SRSLTE_API int srslte_dlsch_decode_multi(srslte_sch_t *q,
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                   int16_t *e_bits[SRSLTE_MAX_CODEWORDS],
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS]);
+
 SRSLTE_API int srslte_ulsch_encode(srslte_sch_t *q, 
                                    srslte_pusch_cfg_t *cfg,
                                    srslte_softbuffer_tx_t *softbuffer,
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index 907a5a008..d37750749 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -75,6 +75,11 @@ SRSLTE_API int srslte_rf_open_multi(srslte_rf_t *h,
                                     char *args, 
                                     uint32_t nof_rx_antennas); 
 
+SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
+                                     char *args,
+                                     uint32_t nof_tx_antennas,
+                                     uint32_t nof_rx_antennas);
+
 SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h, 
                                       char *devname, 
                                       char *args);
@@ -84,6 +89,12 @@ SRSLTE_API int srslte_rf_open_devname_multi(srslte_rf_t *h,
                                             char *args, 
                                             uint32_t nof_rx_antennas);
 
+SRSLTE_API int srslte_rf_open_devname_multi2(srslte_rf_t *rf,
+                                             char *devname,
+                                             char *args,
+                                             uint32_t nof_tx_antennas,
+                                             uint32_t nof_rx_antennas);
+
 SRSLTE_API const char *srslte_rf_name(srslte_rf_t *h); 
 
 SRSLTE_API int srslte_rf_start_gain_thread(srslte_rf_t *rf, 
@@ -200,5 +211,12 @@ SRSLTE_API int srslte_rf_send_timed2(srslte_rf_t *h,
                               bool is_start_of_burst,
                               bool is_end_of_burst);
 
+SRSLTE_API int srslte_rf_send_multi(srslte_rf_t *rf,
+                                    void *data[4],
+                                    int nsamples,
+                                    bool blocking,
+                                    bool is_start_of_burst,
+                                    bool is_end_of_burst);
+
 #endif
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 7c3f83320..408ba0c65 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -84,7 +84,7 @@ typedef struct SRSLTE_API {
   srslte_cfo_t sfo_correct; 
   
   srslte_pdsch_cfg_t pdsch_cfg; 
-  srslte_softbuffer_rx_t softbuffer;
+  srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS];
   srslte_ra_dl_dci_t dl_dci;
   srslte_cell_t cell;
 
@@ -142,6 +142,13 @@ SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q,
                                       uint32_t sf_idx, 
                                       uint32_t rvidx); 
 
+SRSLTE_API int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q,
+                                            srslte_ra_dl_grant_t *grant,
+                                            uint32_t cfi,
+                                            uint32_t sf_idx,
+                                            uint32_t rvidx,
+                                            uint32_t rvidx2);
+
 SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, 
                                         uint32_t cfi, 
                                         uint32_t sf_idx, 
@@ -173,7 +180,7 @@ SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t * q,
 
 SRSLTE_API int srslte_ue_dl_decode_multi(srslte_ue_dl_t * q, 
                                          cf_t *input[SRSLTE_MAX_PORTS], 
-                                         uint8_t *data,
+                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                          uint32_t tti);
 
 SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q, 
@@ -184,7 +191,7 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q,
 
 SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q, 
                                               cf_t *input[SRSLTE_MAX_PORTS], 
-                                              uint8_t *data,
+                                              uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                               uint32_t tti,
                                               uint16_t rnti);
 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 94d4f9136..3570e5a30 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -148,6 +148,13 @@ SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q,
                                         int offset_time, 
                                         float offset_freq);
 
+SRSLTE_API int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q,
+                                              uint32_t nof_prb,
+                                              char *file_name,
+                                              int offset_time,
+                                              float offset_freq,
+                                              uint32_t nof_rx_ant);
+
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
 SRSLTE_API int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, 
diff --git a/lib/include/srslte/phy/utils/debug.h b/lib/include/srslte/phy/utils/debug.h
index 9233d9c61..e1021bc2a 100644
--- a/lib/include/srslte/phy/utils/debug.h
+++ b/lib/include/srslte/phy/utils/debug.h
@@ -63,5 +63,6 @@ SRSLTE_API extern int srslte_verbose;
 #define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO) \
   fprintf(stdout, "[INFO]:  " _fmt, __VA_ARGS__)
 
+#define ERROR(_fmt) fprintf(stderr, "%s.%d: " _fmt "\n", __FILE__, __LINE__)
 
 #endif // DEBUG_H
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 690721b3c..3c2f4210c 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -417,12 +417,15 @@ float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q) {
   
 }
 
-float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {  
-  // Note: use only port 0 but average across antennas
-  float n = 0; 
-  for (int i=0;i<q->last_nof_antennas;i++) {
-    n += q->rsrp[i][0]; 
+float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q, uint32_t port) {
+  float n = 0;
+  for (int i = 0; i < q->last_nof_antennas; i++) {
+      n += q->rsrp[i][port];
   }
-  return n/q->last_nof_antennas;
+  return n / q->last_nof_antennas;
 }
 
+float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {
+  // Note: use only port 0 but average across antennas
+  return srslte_chest_dl_get_rsrp_port(q, 0);
+}
diff --git a/lib/src/phy/ch_estimation/test/CMakeLists.txt b/lib/src/phy/ch_estimation/test/CMakeLists.txt
index fd293c273..8e1208ef5 100644
--- a/lib/src/phy/ch_estimation/test/CMakeLists.txt
+++ b/lib/src/phy/ch_estimation/test/CMakeLists.txt
@@ -52,4 +52,3 @@ add_test(chest_test_ul_cellid1 chest_test_ul -c 2 -r 50)
 
 
 
-
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 8386ae18b..69e39b49a 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -124,6 +124,19 @@ bool srslte_N_id_1_isvalid(uint32_t N_id_1) {
   }
 }
 
+srslte_mod_t srslte_str2mod (char * mod_str) {
+  if (!strcmp(mod_str, "QPSK")) {
+    return SRSLTE_MOD_QPSK;
+  } else if (!strcmp(mod_str, "16QAM")) {
+    return SRSLTE_MOD_16QAM;
+  } else if (!strcmp(mod_str, "64QAM")) {
+    return SRSLTE_MOD_64QAM;
+  } else {
+    return (srslte_mod_t) SRSLTE_ERROR_INVALID_INPUTS;
+  }
+};
+
+
 char *srslte_mod_string(srslte_mod_t mod) {
   switch (mod) {
   case SRSLTE_MOD_BPSK:
@@ -419,12 +432,14 @@ struct lte_band lte_bands[SRSLTE_NOF_LTE_BANDS] = {
 
 
 int srslte_str2mimotype(char *mimo_type_str, srslte_mimo_type_t *type) {
-  if (!strcmp(mimo_type_str, "single")) {
+  if (!strcmp(mimo_type_str, "single") || !strcmp(mimo_type_str, "Port0")) {
     *type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-  } else if (!strcmp(mimo_type_str, "diversity")) {
+  } else if (!strcmp(mimo_type_str, "diversity") || !strcmp(mimo_type_str, "TxDiversity")) {
     *type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
   } else if (!strcmp(mimo_type_str, "multiplex")) {
     *type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+  } else if (!strcmp(mimo_type_str, "cdd") || !strcmp(mimo_type_str, "CDD")) {
+    *type = SRSLTE_MIMO_TYPE_CDD;
   } else {
     return SRSLTE_ERROR;
   }
diff --git a/lib/src/phy/io/filesink.c b/lib/src/phy/io/filesink.c
index 60a8c386e..a61c417fd 100644
--- a/lib/src/phy/io/filesink.c
+++ b/lib/src/phy/io/filesink.c
@@ -99,3 +99,60 @@ int srslte_filesink_write(srslte_filesink_t *q, void *buffer, int nsamples) {
   return i;
 }
 
+int srslte_filesink_write_multi(srslte_filesink_t *q, void **buffer, int nsamples, int nchannels) {
+  int i, j;
+  float **fbuf = (float**) buffer;
+  _Complex float **cbuf = (_Complex float**) buffer;
+  _Complex short **sbuf = (_Complex short**) buffer;
+  int size;
+
+  switch(q->type) {
+    case SRSLTE_FLOAT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%g%c", fbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_COMPLEX_FLOAT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%g%+gi%c", __real__ cbuf[j][i], __imag__ cbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_COMPLEX_SHORT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%hd%+hdi%c", __real__ sbuf[j][i], __imag__ sbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_FLOAT_BIN:
+    case SRSLTE_COMPLEX_FLOAT_BIN:
+    case SRSLTE_COMPLEX_SHORT_BIN:
+      if (q->type == SRSLTE_FLOAT_BIN) {
+        size = sizeof(float);
+      } else if (q->type == SRSLTE_COMPLEX_FLOAT_BIN) {
+        size = sizeof(_Complex float);
+      } else if (q->type == SRSLTE_COMPLEX_SHORT_BIN) {
+        size = sizeof(_Complex short);
+      }
+      if (nchannels > 1) {
+        uint32_t count = 0;
+        for (i = 0; i < nsamples; i++) {
+          for (j = 0; j < nchannels; j++) {
+            count += fwrite(&cbuf[j][i], size, 1, q->f);
+          }
+        }
+        return count;
+      } else {
+        return fwrite(buffer[0], size, nsamples, q->f);
+      }
+      break;
+    default:
+      i = -1;
+      break;
+  }
+  return i;
+}
\ No newline at end of file
diff --git a/lib/src/phy/io/filesource.c b/lib/src/phy/io/filesource.c
index 4010f8da4..048ecb584 100644
--- a/lib/src/phy/io/filesource.c
+++ b/lib/src/phy/io/filesource.c
@@ -116,3 +116,29 @@ int srslte_filesource_read(srslte_filesource_t *q, void *buffer, int nsamples) {
   return i;
 }
 
+int srslte_filesource_read_multi(srslte_filesource_t *q, void **buffer, int nsamples, int nof_channels) {
+  int i, j, count = 0;
+  _Complex float **cbuf = (_Complex float **) buffer;
+
+  switch (q->type) {
+    case SRSLTE_FLOAT:
+    case SRSLTE_COMPLEX_FLOAT:
+    case SRSLTE_COMPLEX_SHORT:
+    case SRSLTE_FLOAT_BIN:
+    case SRSLTE_COMPLEX_SHORT_BIN:
+      fprintf(stderr, "%s.%d:Read Mode not implemented\n", __FILE__, __LINE__);
+      count = SRSLTE_ERROR;
+      break;
+    case SRSLTE_COMPLEX_FLOAT_BIN:
+      for (i = 0; i < nsamples; i++) {
+        for (j = 0; j < nof_channels; j++) {
+          count += fread(&cbuf[j][i], sizeof(cf_t), (size_t) 1, q->f);
+        }
+      }
+      break;
+    default:
+      count = SRSLTE_ERROR;
+      break;
+  }
+  return count;
+}
\ No newline at end of file
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 8781dbad2..f93c63479 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -530,6 +530,154 @@ int srslte_predecoding_type(cf_t *y_, cf_t *h_[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE
   return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, type, noise_estimate);  
 }
 
+
+int srslte_precoding_mimo_2x2_gen(cf_t W[2][2], cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], 
+                              int nof_symbols, float noise_estimate)
+{
+ 
+  cf_t G[2][2], Gx[2][2]; 
+  
+  for (int i=0; i<nof_symbols; i++) {
+  
+    // G=H*W
+    G[0][0] = h[0][0][i]*W[0][0]+h[0][1][i]*W[1][0];        
+    G[0][1] = h[0][0][i]*W[1][0]+h[0][1][i]*W[1][1]; 
+    G[1][0] = h[1][0][i]*W[0][0]+h[1][1][i]*W[1][0];
+    G[1][1] = h[1][0][i]*W[1][0]+h[1][1][i]*W[1][1]; 
+    
+    if (noise_estimate == 0) {
+      // MF equalizer: Gx = G'
+      Gx[0][0] = conjf(G[0][0]); 
+      Gx[0][1] = conjf(G[1][0]); 
+      Gx[1][0] = conjf(G[0][1]); 
+      Gx[1][1] = conjf(G[1][1]); 
+    } else {
+      // MMSE equalizer: Gx = (G'G+I)      
+      fprintf(stderr, "MMSE MIMO decoder not implemented\n");
+      return -1; 
+    }
+    
+    // x=G*y
+    x[0][i] = Gx[0][0]*y[0][i] + Gx[0][1]*y[1][i];
+    x[1][i] = Gx[1][0]*y[0][i] + Gx[1][1]*y[1][i];
+  }
+  
+  return SRSLTE_SUCCESS; 
+}
+
+
+
+
+
+
+
+// SSE implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_SSE
+
+int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols) {
+  uint32_t i = 0;
+
+  /* Conjugate mask */
+  __m128 conj_mask = (__m128) {+0.0f, -0.0f, +0.0f, -0.0f};
+
+  for (i = 0; i < nof_symbols; i += 2) {
+    /* Load channel */
+    __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
+    __m128 h01i = _mm_load_ps((float *) &h[0][1][i]);
+    __m128 h10i = _mm_load_ps((float *) &h[1][0][i]);
+    __m128 h11i = _mm_load_ps((float *) &h[1][1][i]);
+
+    /* Apply precoding */
+    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, (__m128) {+0.0f, +0.0f, -0.0f, -0.0f}));
+    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, (__m128) {+0.0f, +0.0f, -0.0f, -0.0f}));
+    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
+    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
+
+    __m128 detmult1 = PROD(h00, h11);
+    __m128 detmult2 = PROD(h01, h10);
+
+    __m128 det = _mm_sub_ps(detmult1, detmult2);
+    __m128 detconj = _mm_xor_ps(det, conj_mask);
+    __m128 detabs2 = PROD(det, detconj);
+    __m128 detabs2rec = _mm_rcp_ps(detabs2);
+    detabs2rec = _mm_shuffle_ps(detabs2rec, detabs2rec, _MM_SHUFFLE(2, 2, 0, 0));
+    __m128 detrec = _mm_mul_ps(_mm_mul_ps(detconj, detabs2rec), (__m128) {2.0f, 2.0f, 2.0f, 2.0f});
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0 = PROD(_mm_sub_ps(PROD(h11, y0), PROD(h01, y1)), detrec);
+    __m128 x1 = PROD(_mm_sub_ps(PROD(h00, y1), PROD(h10, y0)), detrec);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif
+
+// Generic implementation of ZF 2x2 CCD equalizer
+int srslte_predecoding_ccd_2x2_zf_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                      int nof_symbols) {
+  cf_t h00, h01, h10, h11, det;
+  for (int i = 0; i < nof_symbols; i++) {
+
+    // Even precoder
+    h00 = +h[0][0][i] + h[1][0][i];
+    h10 = +h[0][1][i] + h[1][1][i];
+    h01 = +h[0][0][i] - h[1][0][i];
+    h11 = +h[0][1][i] - h[1][1][i];
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * ((float) 2.0 / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+
+    i++;
+
+    // Odd precoder
+    h00 = h[0][0][i] - h[1][0][i];
+    h10 = h[0][1][i] - h[1][1][i];
+    h01 = h[0][0][i] + h[1][0][i];
+    h11 = h[0][1][i] + h[1][1][i];
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * ((float) 2.0 / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+
+  }
+  return SRSLTE_SUCCESS;
+}
+
+int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], 
+                              int nof_rxant, int nof_ports, int nof_layers, int nof_symbols) 
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+#ifdef LV_HAVE_SSE
+      return srslte_predecoding_ccd_2x2_zf_sse(y, h, x, nof_symbols);
+#else
+      return srslte_predecoding_ccd_2x2_zf_gen(y, h, x, nof_symbols);
+#endif
+    } else {
+      fprintf(stderr, "Error predecoding CCD: Invalid number of layers %d\n", nof_layers);
+      return -1;       
+    }          
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
+  } else {
+    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+  }
+  return SRSLTE_ERROR;
+}
+
+
+
 /* 36.211 v10.3.0 Section 6.3.4 */
 int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
     int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
@@ -547,7 +695,13 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
 
   switch (type) {
   case SRSLTE_MIMO_TYPE_CDD:
-    fprintf(stderr, "CCD not supported\n");
+    if (nof_layers >= 2 && nof_layers <= 4) {
+      return srslte_predecoding_ccd_zf(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols);
+    } else {
+      fprintf(stderr,
+          "Invalid number of layers %d\n", nof_layers);
+      return -1;
+    }
     return -1; 
   case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
     if (nof_ports == 1 && nof_layers == 1) {
diff --git a/lib/src/phy/mimo/test/CMakeLists.txt b/lib/src/phy/mimo/test/CMakeLists.txt
index e0e5578d5..42e262550 100644
--- a/lib/src/phy/mimo/test/CMakeLists.txt
+++ b/lib/src/phy/mimo/test/CMakeLists.txt
@@ -42,7 +42,7 @@ add_test(layermap_multiplex_24 layermap_test -n 1000 -m multiplex -c 2 -l 4)
  
  
 ########################################################################
-# LAYER MAPPING TEST  
+# PRECODING MAPPING TEST
 ########################################################################
  
 add_executable(precoding_test precoder_test.c)
@@ -52,6 +52,7 @@ add_test(precoding_single precoding_test -n 1000 -m single)
 add_test(precoding_diversity2 precoding_test -n 1000 -m diversity -l 2 -p 2) 
 add_test(precoding_diversity4 precoding_test -n 1024 -m diversity -l 4 -p 4) 
 
+add_test(precoding_cdd_2x2 precoding_test -m cdd -l 2 -p 2 -r 2)
  
 
 
diff --git a/lib/src/phy/mimo/test/layermap_test.c b/lib/src/phy/mimo/test/layermap_test.c
index 209a9ac73..6fcc72682 100644
--- a/lib/src/phy/mimo/test/layermap_test.c
+++ b/lib/src/phy/mimo/test/layermap_test.c
@@ -40,7 +40,7 @@ int nof_cw = 1, nof_layers = 1;
 char *mimo_type_name = NULL;
 
 void usage(char *prog) {
-  printf("Usage: %s -m [single|diversity|multiplex] -c [nof_cw] -l [nof_layers]\n", prog);
+  printf("Usage: %s -m [single|diversity|multiplex|cdd] -c [nof_cw] -l [nof_layers]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
 }
 
diff --git a/lib/src/phy/mimo/test/precoder_mex.c b/lib/src/phy/mimo/test/precoder_mex.c
index 958f06262..b021f422a 100644
--- a/lib/src/phy/mimo/test/precoder_mex.c
+++ b/lib/src/phy/mimo/test/precoder_mex.c
@@ -78,9 +78,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     x[i] = srslte_vec_malloc(sizeof(cf_t)*nof_symbols/nof_layers);
   }
   
-  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_tx_ports/nof_layers);
+  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_tx_ports);
   for (int i=0;i<nof_tx_ports;i++) {
-    y[i] = &output[i*nof_symbols/nof_layers];
+    y[i] = &output[i*nof_symbols];
   }
   
   char *txscheme = "Port0";
@@ -102,13 +102,30 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
   int symbols_layers[SRSLTE_MAX_LAYERS];
   for (int i=0;i<nof_layers;i++) {
-    symbols_layers[i] = nof_symbols/nof_layers; 
+    symbols_layers[i] = nof_symbols;
   }
   srslte_layermap_type(d, x, nof_codewords, nof_layers, symbols_layers, type);
   srslte_precoding_type(x, y, nof_layers, nof_tx_ports, nof_symbols/nof_layers, type);
   
   if (nlhs >= 1) { 
-    mexutils_write_cf(output, &plhs[0], nof_symbols/nof_layers, nof_tx_ports);  
+    switch (type) {
+      case SRSLTE_MIMO_TYPE_CDD:
+        mexutils_write_cf(output, &plhs[0], nof_symbols/nof_layers, nof_tx_ports);
+        break;
+      case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      default:
+        mexutils_write_cf(output, &plhs[0], (uint32_t) nof_symbols, nof_tx_ports);
+        break;
+    }
+  }
+
+  if (nlhs >= 2) {
+    mexutils_write_cf(x[0], &plhs[1], nof_symbols / nof_layers, 1);
+  }
+  if (nlhs >= 3) {
+    mexutils_write_cf(x[1], &plhs[2], nof_symbols / nof_layers, 1);
   }
   
   if (input) {
diff --git a/lib/src/phy/mimo/test/precoder_test.c b/lib/src/phy/mimo/test/precoder_test.c
index e935a7067..cf8dac89a 100644
--- a/lib/src/phy/mimo/test/precoder_test.c
+++ b/lib/src/phy/mimo/test/precoder_test.c
@@ -35,28 +35,31 @@
 
 #include "srslte/srslte.h"
 
-#define MSE_THRESHOLD	0.00001
+#define MSE_THRESHOLD	0.0005
 
 int nof_symbols = 1000;
-int nof_layers = 1, nof_ports = 1;
+int nof_layers = 1, nof_tx_ports = 1, nof_rx_ports = 1, nof_re = 1;
 char *mimo_type_name = NULL;
 
 void usage(char *prog) {
   printf(
-      "Usage: %s -m [single|diversity|multiplex] -l [nof_layers] -p [nof_ports]\n",
-      prog);
+      "Usage: %s -m [single|diversity|multiplex|cdd] -l [nof_layers] -p [nof_tx_ports]\n"
+          " -r [nof_rx_ports]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "mpln")) != -1) {
+  while ((opt = getopt(argc, argv, "mplnr")) != -1) {
     switch (opt) {
     case 'n':
       nof_symbols = atoi(argv[optind]);
       break;
     case 'p':
-      nof_ports = atoi(argv[optind]);
+      nof_tx_ports = atoi(argv[optind]);
+      break;
+    case 'r':
+      nof_rx_ports = atoi(argv[optind]);
       break;
     case 'l':
       nof_layers = atoi(argv[optind]);
@@ -75,102 +78,175 @@ void parse_args(int argc, char **argv) {
   }
 }
 
+void populate_channel_cdd(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t n) {
+  int i, j, k;
+
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      for (k = 0; k < n; k++) {
+        h[i][j][k] = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+      }
+    }
+  }
+}
+
+void populate_channel_diversity(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t n) {
+  int i, j, k, l;
+
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      for (k = 0; k < n / nof_layers; k++) {
+        cf_t hsymb = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+        for (l = 0; l < nof_layers; l++) {
+          // assume the channel is the same for all symbols
+          h[i][j][k * nof_layers + l] = hsymb;
+        }
+      }
+    }
+  }
+}
+
+void populate_channel_single(cf_t *h) {
+  int i;
+
+  for (i = 0; i < nof_re; i++) {
+    h[i] = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+  }
+}
+
+void populate_channel(srslte_mimo_type_t type, cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]) {
+  switch (type) {
+    case SRSLTE_MIMO_TYPE_CDD:
+      populate_channel_cdd(h, (uint32_t) nof_re);
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      populate_channel_diversity(h, (uint32_t) nof_re);
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      fprintf(stderr, "Error: not implemented channel emulator\n");
+      exit(-1);
+      //break;
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+    default:
+      populate_channel_single(h[0][0]);
+  }
+}
+
 int main(int argc, char **argv) {
-  int i, j;
+  int i, j, k;
   float mse;
-  cf_t *x[SRSLTE_MAX_LAYERS], *r[SRSLTE_MAX_PORTS], *y[SRSLTE_MAX_PORTS], *h[SRSLTE_MAX_PORTS],
+  cf_t *x[SRSLTE_MAX_LAYERS], *r[SRSLTE_MAX_PORTS], *y[SRSLTE_MAX_PORTS], *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
       *xr[SRSLTE_MAX_LAYERS];
   srslte_mimo_type_t type;
   
   parse_args(argc, argv);
 
-  if (nof_ports > SRSLTE_MAX_PORTS || nof_layers > SRSLTE_MAX_LAYERS) {
+  /* Check input ranges */
+  if (nof_tx_ports > SRSLTE_MAX_PORTS || nof_rx_ports > SRSLTE_MAX_PORTS || nof_layers > SRSLTE_MAX_LAYERS) {
     fprintf(stderr, "Invalid number of layers or ports\n");
     exit(-1);
   }
 
+  /* Parse MIMO Type */
   if (srslte_str2mimotype(mimo_type_name, &type)) {
     fprintf(stderr, "Invalid MIMO type %s\n", mimo_type_name);
     exit(-1);
   }
 
+  /* Check scenario conditions are OK */
+  switch (type) {
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      nof_re = nof_layers*nof_symbols;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      nof_re = nof_symbols*nof_tx_ports/nof_layers;
+      if (nof_rx_ports != 2 || nof_tx_ports != 2) {
+        fprintf(stderr, "CDD nof_tx_ports=%d nof_rx_ports=%d is not currently supported\n", nof_tx_ports, nof_rx_ports);
+        exit(-1);
+      }
+      break;
+    default:
+      nof_re = nof_symbols*nof_layers;
+  }
+
+  /* Allocate x and xr (received symbols) in memory for each layer */
   for (i = 0; i < nof_layers; i++) {
+    /* Source data */
     x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!x[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
+
+    /* Sink data */
     xr[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!xr[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
   }
-  for (i = 0; i < nof_ports; i++) {
-    y[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-    // TODO: The number of symbols per port is different in spatial multiplexing.
+
+  /* Allocate y in memory for tx each port */
+  for (i = 0; i < nof_tx_ports; i++) {
+    y[i] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
     if (!y[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
-    h[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-    if (!h[i]) {
-      perror("srslte_vec_malloc");
-      exit(-1);
+  }
+
+  /* Allocate h in memory for each cross channel and layer */
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      h[i][j] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
+      if (!h[i][j]) {
+        perror("srslte_vec_malloc");
+        exit(-1);
+      }
     }
   }
 
-  /* only 1 receiver antenna supported now */
-  r[0] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-  if (!r[0]) {
-    perror("srslte_vec_malloc");
-    exit(-1);
+  /* Allocate r */
+  for (i = 0; i < nof_rx_ports; i++) {
+    r[i] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
+    if (!r[i]) {
+      perror("srslte_vec_malloc");
+      exit(-1);
+    }
   }
 
-  /* generate random data */
+  /* Generate source random data */
   for (i = 0; i < nof_layers; i++) {
     for (j = 0; j < nof_symbols; j++) {
-      x[i][j] = (2*(rand()%2)-1+(2*(rand()%2)-1)*_Complex_I)/sqrt(2);
+      x[i][j] = (2 * (rand() % 2) - 1 + (2 * (rand() % 2) - 1) * _Complex_I) / sqrt(2);
     }
   }
-  
-  /* precoding */
-  if (srslte_precoding_type(x, y, nof_layers, nof_ports, nof_symbols, type) < 0) {
+
+  /* Execute Precoding (Tx) */
+  if (srslte_precoding_type(x, y, nof_layers, nof_tx_ports, nof_symbols, type) < 0) {
     fprintf(stderr, "Error layer mapper encoder\n");
     exit(-1);
   }
 
   /* generate channel */
-  for (i = 0; i < nof_ports; i++) {
-    for (j = 0; j < nof_symbols; j++) {
-      h[i][nof_layers*j] = (float) rand()/RAND_MAX+((float) rand()/RAND_MAX)*_Complex_I;
-      // assume the channel is time-invariant in nlayer consecutive symbols
-      for (int k=0;k<nof_layers;k++) {
-        h[i][nof_layers*j+k] = h[i][nof_layers*j];              
-      }
-    }
-  }
+  populate_channel(type, h);
 
   /* pass signal through channel
    (we are in the frequency domain so it's a multiplication) */
-  /* there's only one receiver antenna, signals from different transmitter
-   * ports are simply combined at the receiver
-   */
-  for (j = 0; j < nof_symbols * nof_layers; j++) {
-    r[0][j] = 0;
-    for (i = 0; i < nof_ports; i++) {
-      r[0][j] += y[i][j] * h[i][j];
+  for (i = 0; i < nof_rx_ports; i++) {
+    for (k = 0; k < nof_re; k++) {
+      r[i][k] = (cf_t) (0.0 + 0.0 * _Complex_I);
+      for (j = 0; j < nof_tx_ports; j++) {
+        r[i][k] += y[j][k] * h[j][i][k];
+      }
     }
   }
-    
+
   /* predecoding / equalization */
   struct timeval t[3];
   gettimeofday(&t[1], NULL);
-  if (srslte_predecoding_type(r[0], h, xr, nof_ports, nof_layers,
-      nof_symbols * nof_layers, type, 0) < 0) {
-    fprintf(stderr, "Error layer mapper encoder\n");
-    exit(-1);
-  }
+  srslte_predecoding_type_multi(r, h, xr, nof_rx_ports, nof_tx_ports, nof_layers,
+                                nof_re, type, 0);
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   printf("Execution Time: %ld us\n", t[0].tv_usec);
@@ -187,17 +263,22 @@ int main(int argc, char **argv) {
     exit(-1);
   } 
 
+  /* Free all data */
   for (i = 0; i < nof_layers; i++) {
     free(x[i]);
     free(xr[i]);
   }
-  for (i = 0; i < nof_ports; i++) {
-    free(y[i]);
-    free(h[i]);
+
+  for (i = 0; i < nof_rx_ports; i++) {
+    free(r[i]);
+  }
+
+  for (i = 0; i < nof_rx_ports; i++) {
+    for (j = 0; j < nof_tx_ports; j++) {
+      free(h[j][i]);
+    }
   }
 
-  free(r[0]);
-  
   printf("Ok\n");
   exit(0); 
 }
diff --git a/lib/src/phy/mimo/test/predecoder_mex.c b/lib/src/phy/mimo/test/predecoder_mex.c
index 629ac0dc6..e04b390d0 100644
--- a/lib/src/phy/mimo/test/predecoder_mex.c
+++ b/lib/src/phy/mimo/test/predecoder_mex.c
@@ -48,118 +48,159 @@ void help()
 /* the gateway function */
 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 {
+  const mwSize *dims = mxGetDimensions(INPUT);
+  mwSize ndims;
   cf_t *input = NULL;
   cf_t *hest = NULL; 
   cf_t *output = NULL; 
   uint32_t nof_symbols = 0; 
+  uint32_t nof_rx_ants  = 1;
+  uint32_t nof_layers;
+  uint32_t nof_tx_ports = 1;
+  uint32_t nof_codewords = 1;
+  float noise_estimate = 0;
+  cf_t *x[SRSLTE_MAX_LAYERS];
+  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  cf_t *y[SRSLTE_MAX_PORTS];
+  int symbols_layers[SRSLTE_MAX_LAYERS];
+  int i, j;
+  srslte_mimo_type_t type;
   
+  /* Print help if number of inputs does not match with expected */
   if (nrhs < NOF_INPUTS) {
     help();
     return;
   }
   
-  // Read input symbols
+  /* Read input symbols */
   if (mexutils_read_cf(INPUT, &input) < 0) {
     mexErrMsgTxt("Error reading input\n");
     return; 
   }
-  uint32_t nof_layers   = mxGetScalar(NLAYERS); 
-  uint32_t nof_tx_ports = 1; 
-  uint32_t nof_codewords = 1; 
 
-  uint32_t nof_rx_ants  = 1; 
-  const mwSize *dims = mxGetDimensions(INPUT);
-  mwSize ndims = mxGetNumberOfDimensions(INPUT);
-  nof_symbols = dims[0];
-  
+  /* Read number of layers */
+  nof_layers = (uint32_t) mxGetScalar(NLAYERS);
+
+  if (nof_layers > SRSLTE_MAX_LAYERS) {
+    mexErrMsgTxt("Too many layers\n");
+    return;
+  }
+
+  /* Read number of symbols and Rx antennas */
+  ndims = mxGetNumberOfDimensions(INPUT);
+  nof_symbols = (uint32_t) dims[0];
+
   if (ndims >= 2) {
-    nof_rx_ants = dims[1];
+    nof_rx_ants = (uint32_t) dims[1];
   }
   
-  // Read channel estimates
+  /* Read channel estimates */
   if (mexutils_read_cf(HEST, &hest) < 0) {
     mexErrMsgTxt("Error reading hest\n");
     return; 
   }
+
+  /* Get number of tx ports */
   dims = mxGetDimensions(HEST);
   ndims = mxGetNumberOfDimensions(HEST);
 
   if (ndims == 3) {
-    nof_tx_ports = dims[2];        
+    nof_tx_ports = (uint32_t) dims[2];
   }
-  
+
+  /* Print parameters trace */
   mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_symbols=%d\n", nof_tx_ports, nof_rx_ants, nof_layers, nof_symbols);
 
-  // Read noise estimate
-  float noise_estimate = 0; 
+  /* Read noise estimate */
   if (nrhs >= NOF_INPUTS) {
-    noise_estimate = mxGetScalar(NEST);
+    noise_estimate = (float) mxGetScalar(NEST);
   }
   
-  cf_t *x[SRSLTE_MAX_LAYERS]; 
-  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  cf_t *y[SRSLTE_MAX_PORTS];
-
-  for (int i=0;i<SRSLTE_MAX_LAYERS;i++) {
+  /* Initialise x, h & y pointers */
+  for (i=0;i<SRSLTE_MAX_LAYERS;i++) {
     x[i] = NULL; 
   }
-  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
-    for (int j=0;j<SRSLTE_MAX_PORTS;j++) {
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j=0;j<SRSLTE_MAX_PORTS;j++) {
       h[i][j] = NULL; 
     }
   }
-  
-  /* Allocate memory */
-  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols);
-  for (int i = 0; i < nof_tx_ports; i++) {
-    x[i] = srslte_vec_malloc(sizeof(cf_t)*nof_symbols);
-    for (int j=0;j<nof_rx_ants;j++) {
-      h[i][j] = &hest[i*nof_symbols*nof_rx_ants + j*nof_symbols];
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    y[i] = NULL;
+  }
+
+  /* Allocate memory for ouput */
+  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_layers);
+
+  /* Allocate memory for intermediate data */
+  for (i = 0; i < nof_tx_ports; i++) {
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
+  }
+
+  /* Allocate memory for channel estimate */
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j=0; j<nof_rx_ants; j++) {
+      h[i][j] = &hest[(i*nof_rx_ants + j)*nof_symbols];
     }
   }
-  
-  for (int j=0;j<nof_rx_ants;j++) {
+
+  /* Allocate memory for input */
+  for (j = 0; j < nof_rx_ants; j++) {
     y[j] = &input[j*nof_symbols];
   }
-    
-  char *txscheme = "Port0";
+
+  /* Parse Tx scheme */
+  char txscheme[32] = "Port0";
   if (nrhs >= NOF_INPUTS) {
-    txscheme = mxArrayToString(TXSCHEME);
-  }  
-  srslte_mimo_type_t type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA; 
+    mxGetString_700(TXSCHEME, txscheme, 32);
+  }
+
   if (!strcmp(txscheme, "Port0")) {
     type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
   } else if (!strcmp(txscheme, "TxDiversity")) {
     type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
   } else if (!strcmp(txscheme, "CDD")) {    
     type = SRSLTE_MIMO_TYPE_CDD;
-  } else if (!strcmp(txscheme, "SpatialMux")) {    
+  } else if (!strcmp(txscheme, "SpatialMux")) {
     type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
   } else {
     mexPrintf("Unsupported TxScheme=%s\n", txscheme);
     return; 
   }
-  int symbols_layers[SRSLTE_MAX_LAYERS];
-  for (int i=0;i<nof_layers;i++) {
+
+  /* Populate symbols in layers */
+  for (i = 0; i < nof_layers; i++) {
     symbols_layers[i] = nof_symbols; 
   }
-  cf_t *d[SRSLTE_MAX_LAYERS]; 
-  d[0] = output; 
-  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, nof_symbols/nof_layers, type, noise_estimate);
-  srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
-  
 
-  if (nlhs >= 1) { 
-    mexutils_write_cf(output, &plhs[0], nof_symbols, 1);  
+  /* Set output pointer */
+  cf_t *d[SRSLTE_MAX_CODEWORDS];
+  for (i = 0; i<nof_codewords; i++) {
+    d[i] = output;
   }
+
+  /* Pre-decode */
+  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, nof_symbols, type, noise_estimate);
+
+  /* Layer de-mapper */
+  srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
   
+  /* Write output */
+  if (nlhs >= 1) {
+    mexutils_write_cf(output, &plhs[0], nof_symbols*nof_layers*nof_rx_ants/nof_tx_ports, 1);
+  }
+
+  /* Free memory */
   if (input) {
     free(input);
   }
+  if (hest) {
+    free(hest);
+  }
   if (output) {
     free(output);
   }
-  for (int i=0;i<SRSLTE_MAX_LAYERS;i++) {
+  for (i=0;i<SRSLTE_MAX_LAYERS;i++) {
     if (x[i]) {
       free(x[i]);      
     }
diff --git a/lib/src/phy/phch/dci.c b/lib/src/phy/phch/dci.c
index 96c446259..2daa7e10d 100644
--- a/lib/src/phy/phch/dci.c
+++ b/lib/src/phy/phch/dci.c
@@ -1051,6 +1051,16 @@ int dci_format2AB_pack(srslte_ra_dl_dci_t *data, srslte_dci_msg_t *msg, uint32_t
   } else {
     *y++ = data->tb_cw_swap;
   }
+
+  /* Force MCS_idx and RV_idx in function of block enable according to 7.1.7 of 36.213 */
+  if (!data->tb_en[0]) {
+    data->mcs_idx = 0;
+    data->rv_idx= 1;
+  }
+  if (!data->tb_en[1]) {
+    data->mcs_idx_1 = 0;
+    data->rv_idx_1 = 1;
+  }
   
   /* pack TB1 */
   srslte_bit_unpack(data->mcs_idx, &y, 5);
diff --git a/lib/src/phy/phch/pdcch.c b/lib/src/phy/phch/pdcch.c
index cfdb19621..f4c59b321 100644
--- a/lib/src/phy/phch/pdcch.c
+++ b/lib/src/phy/phch/pdcch.c
@@ -65,7 +65,19 @@ int srslte_pdcch_init(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell
   return srslte_pdcch_init_multi(q, regs, cell, 1);
 }
 
-int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+int srslte_pdcch_init_tx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
+  return srslte_pdcch_init_txrx(q, regs, cell, 1, false);
+}
+
+int srslte_pdcch_init_rx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
+  return srslte_pdcch_init_txrx(q, regs, cell, nof_rx_antennas, true);
+}
+
+int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
+  return srslte_pdcch_init_txrx(q, regs, cell, nof_rx_antennas, true);
+}
+
+int srslte_pdcch_init_txrx(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas, bool isReceiver)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
@@ -122,19 +134,25 @@ int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_
       goto clean;
     }
 
-    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
-      for (int j=0;j<q->nof_rx_antennas;j++) {
-        q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
-        if (!q->ce[i][j]) {
-          goto clean;
-        }          
+    if (isReceiver) {
+      for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+        for (int j = 0; j < q->nof_rx_antennas; j++) {
+          q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
+          if (!q->ce[i][j]) {
+            goto clean;
+          }
+        }
       }
+    }
+
+    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
       if (!q->x[i]) {
         goto clean;
       }
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
+
+    for (int j = 0; j < ((isReceiver) ? q->nof_rx_antennas : cell.nof_ports); j++) {
       q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
       if (!q->symbols[j]) {
         goto clean;
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 9b6128c64..3d24cf175 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -203,20 +203,15 @@ int srslte_pdsch_get(srslte_pdsch_t *q, cf_t *sf_symbols, cf_t *symbols,
   return srslte_pdsch_cp(q, sf_symbols, symbols, grant, lstart, subframe, false);
 }
 
-int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell) 
-{
-  return srslte_pdsch_init_multi(q, cell, 1);
-}
-
-/** Initializes the PDCCH transmitter and receiver */
-int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+/** Initializes the PDCCH transmitter or receiver */
+int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas, bool is_receiver)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   int i;
 
  if (q != NULL                  &&
      srslte_cell_isvalid(&cell) && 
-     nof_rx_antennas <= SRSLTE_MAX_PORTS) 
+     nof_antennas <= SRSLTE_MAX_PORTS) 
   {   
     
     bzero(q, sizeof(srslte_pdsch_t));
@@ -224,8 +219,10 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
     
     q->cell = cell;
     q->max_re = q->cell.nof_prb * MAX_PDSCH_RE(q->cell.cp);
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
+    if (is_receiver) {
+      q->nof_rx_antennas = nof_antennas;
+    }
+
     INFO("Init PDSCH: %d ports %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
         q->cell.nof_prb, q->max_re);
 
@@ -244,20 +241,37 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       goto clean;
     }
     
+    q->e2 = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
+    if (!q->e) {
+      goto clean;
+    }
+
     q->d = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
     if (!q->d) {
       goto clean;
     }
 
+    q->d2 = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+    if (!q->d) {
+      goto clean;
+    }
+
+    /* Layer mapped symbols memory allocation */
     for (i = 0; i < q->cell.nof_ports; i++) {
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
       if (!q->x[i]) {
         goto clean;
       }
-      for (int j=0;j<q->nof_rx_antennas;j++) {
-        q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-        if (!q->ce[i][j]) {
-          goto clean;
+    }
+
+    /* If it is the receiver side, allocate estimated channel */
+    if (is_receiver) {
+      for (i = 0; i < q->cell.nof_ports; i++) {
+        for (int j = 0; j < q->nof_rx_antennas; j++) {
+          q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+          if (!q->ce[i][j]) {
+            goto clean;
+          }
         }
       }
     }
@@ -268,6 +282,7 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       }              
     }
     
+    /* Allocate User memory (all zeros) */
     q->users = calloc(sizeof(srslte_pdsch_user_t*), 1+SRSLTE_SIRNTI);
     if (!q->users) {
       perror("malloc");
@@ -283,15 +298,33 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
   return ret;
 }
 
+int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell) {
+  return srslte_pdsch_init_multi(q, cell, 1, true);
+}
+
+int srslte_pdsch_init_tx_multi(srslte_pdsch_t *q, srslte_cell_t cell) {
+  return srslte_pdsch_init_multi(q, cell, 0, false);
+}
+
+int srslte_pdsch_init_rx_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas) {
+  return srslte_pdsch_init_multi(q, cell, nof_antennas, true);
+}
+
 void srslte_pdsch_free(srslte_pdsch_t *q) {
   int i;
 
   if (q->e) {
     free(q->e);
   }
+  if (q->e2) {
+    free(q->e2);
+  }
   if (q->d) {
     free(q->d);
   }
+  if (q->d2) {
+    free(q->d2);
+  }
   for (i = 0; i < q->cell.nof_ports; i++) {
     if (q->x[i]) {
       free(q->x[i]);
@@ -302,7 +335,7 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
       }
     }
   }
-  for (int j=0;j<q->nof_rx_antennas;j++) {
+  for (int j=0;j<SRSLTE_MAX_PORTS;j++) {
     if (q->symbols[j]) {
       free(q->symbols[j]);
     }          
@@ -348,6 +381,50 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
   }
 }
 
+/* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg.
+ * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
+ */
+int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
+                           uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2)
+{
+  if (cfg) {
+    if (grant) {
+      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
+    }
+    if (srslte_cbsegm(&cfg->cb_segm, (uint32_t) cfg->grant.mcs.tbs)) {
+      fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cbsegm(&cfg->cb_segm2, (uint32_t) cfg->grant.mcs2.tbs)) {
+      fprintf(stderr, "Error computing Codeblock (2) segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
+      return SRSLTE_ERROR;
+    }
+
+    srslte_ra_dl_grant_to_nbits_multi(&cfg->grant, cfi, cell, sf_idx, &cfg->nbits, &cfg->nbits2);
+    cfg->sf_idx = sf_idx;
+    cfg->rv = rvidx;
+    cfg->rv2 = rvidx2;
+
+    if (cell.nof_ports == 1 && grant->nof_tb == 1) {
+      cfg->mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      cfg->nof_layers = 1;
+    } else if (cell.nof_ports == 2 && grant->nof_tb == 1) {
+      cfg->mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      cfg->nof_layers = 2;
+    } else if (cell.nof_ports == 2 && grant->nof_tb == 2) {
+      cfg->mimo_type = SRSLTE_MIMO_TYPE_CDD;
+      cfg->nof_layers = 2;
+    } else {
+      INFO("nof_ports=%d, nof_tb=%d are not consistent\n", cell.nof_ports, grant->nof_tb);
+      return SRSLTE_ERROR;
+    }
+
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
 
 /* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
@@ -362,6 +439,10 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
             q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
           return SRSLTE_ERROR; 
         }
+        if (srslte_sequence_pdsch(&q->users[rnti]->seq2[i], rnti, 1, 2 * i, q->cell.id,
+                                  q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+          return SRSLTE_ERROR;
+        }
       }
       q->users[rnti]->sequence_generated = true;
     }
@@ -374,6 +455,7 @@ void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
   if (q->users[rnti]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
       srslte_sequence_free(&q->users[rnti]->seq[i]);
+      srslte_sequence_free(&q->users[rnti]->seq2[i]);
     }
     free(q->users[rnti]);
     q->users[rnti] = NULL; 
@@ -392,15 +474,15 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
   for (int i=0;i<q->cell.nof_ports;i++) {
     _ce[i][0] = ce[i]; 
   }
-  return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, data); 
+  return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, &data);
 }
 
 /** Decodes the PDSCH from the received symbols
  */
 int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
                               cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, 
-                              uint16_t rnti, uint8_t *data) 
+                              uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS])
 {
 
   /* Set pointers for layermapping & precoding */
@@ -441,22 +523,38 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       }      
     }
     
-    /* TODO: only diversity is supported */
     if (q->cell.nof_ports == 1) {
       /* no need for layer demapping */
       srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
     } else {
-      srslte_predecoding_diversity_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nbits.nof_re);
-      srslte_layerdemap_diversity(x, q->d, q->cell.nof_ports, cfg->nbits.nof_re / q->cell.nof_ports);
+      int nof_symbols [SRSLTE_MAX_CODEWORDS];
+      nof_symbols[0] = cfg->nbits.nof_re * cfg->grant.nof_tb / q->cell.nof_ports;
+      nof_symbols[1] = cfg->nbits2.nof_re * cfg->grant.nof_tb / q->cell.nof_ports;
+
+      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                    cfg->nbits.nof_re, cfg->mimo_type, 0.0);
+      srslte_layerdemap_type(x, (cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, cfg->nof_layers, cfg->grant.nof_tb,
+                             nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
     
     if (SRSLTE_VERBOSE_ISDEBUG()) {
-      DEBUG("SAVED FILE subframe.dat: received subframe symbols\n",0);
-      srslte_vec_save_file("subframe.dat", sf_symbols[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-      DEBUG("SAVED FILE hest0.dat and hest1.dat: channel estimates for port 0 and port 1\n",0);
-      srslte_vec_save_file("hest0.dat", ce[0][0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-      if (q->cell.nof_ports > 1) {
-        srslte_vec_save_file("hest1.dat", ce[1][0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      char filename[FILENAME_MAX];
+      for (int j = 0; j < q->nof_rx_antennas; j++) {
+        if (snprintf(filename, FILENAME_MAX, "subframe_p%d.dat", j) < 0) {
+          ERROR("Generating file name");
+          break;
+        }
+        DEBUG("SAVED FILE %s: received subframe symbols\n", filename);
+        srslte_vec_save_file(filename, sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+
+        for (i = 0; i < q->cell.nof_ports; i++) {
+          if (snprintf(filename, FILENAME_MAX, "hest_%d%d.dat", i, j) < 0) {
+            ERROR("Generating file name");
+            break;
+          }
+          DEBUG("SAVED FILE %s: channel estimates for Tx %d and Rx %d\n", filename, j, i);
+          srslte_vec_save_file(filename, ce[i][j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+        }
       }
       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));
@@ -466,18 +564,40 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     * 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
     */
-    srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
-    
+    if (cfg->nbits.nof_re) {
+      srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
+    }
+
+    if (cfg->nbits2.nof_re) {
+      srslte_demod_soft_demodulate_s(cfg->grant.mcs2.mod, q->d2, q->e2, cfg->nbits2.nof_re);
+    }
+
     /* descramble */
     if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
+      if (cfg->nbits.nof_bits) {
+        srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
+      }
+
+      if (cfg->nbits2.nof_bits) {
+        srslte_scrambling_s_offset(&q->users[rnti]->seq2[cfg->sf_idx], q->e2, 0, cfg->nbits2.nof_bits);
+      }
     } else {
       srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
+      if (cfg->nbits.nof_bits) {
+        if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
+          return SRSLTE_ERROR;
+        }
+        srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
+        srslte_sequence_free(&seq);
+      }
+
+      if (cfg->nbits2.nof_bits) {
+        if (srslte_sequence_pdsch(&seq, rnti, 1, 2 * cfg->sf_idx, q->cell.id, cfg->nbits2.nof_bits)) {
+          return SRSLTE_ERROR;
+        }
+        srslte_scrambling_s_offset(&seq, q->e2, 0, cfg->nbits2.nof_bits);
+        srslte_sequence_free(&seq);
       }
-      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
-      srslte_sequence_free(&seq);
     }
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
@@ -485,7 +605,8 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       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);      
+    return srslte_dlsch_decode_multi(&q->dl_sch, cfg, softbuffers, (int16_t *[SRSLTE_MAX_CODEWORDS]) {q->e, q->e2},
+                                     data);
     
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
@@ -571,6 +692,112 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
   return ret; 
 }
 
+int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
+                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                              uint8_t *data[SRSLTE_MAX_CODEWORDS], uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) {
+
+  int i;
+  /* Set pointers for layermapping & precoding */
+  cf_t *x[SRSLTE_MAX_LAYERS];
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL &&
+      cfg != NULL) {
+
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      if (sf_symbols[i] == NULL) {
+        return SRSLTE_ERROR_INVALID_INPUTS;
+      }
+    }
+
+    /* If both transport block sizes are zero return error */
+    if (cfg->grant.mcs.tbs == 0 && cfg->grant.mcs2.tbs == 0) {
+      return SRSLTE_ERROR_INVALID_INPUTS;
+    }
+
+    if (cfg->nbits.nof_re > q->max_re) {
+      fprintf(stderr,
+              "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
+              cfg->nbits.nof_re, q->max_re, q->cell.nof_prb);
+      return SRSLTE_ERROR_INVALID_INPUTS;
+    }
+
+    INFO("Encoding PDSCH SF: %d (TB 1), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+         cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs,
+         cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv);
+    if (cfg->grant.nof_tb > 1) {
+      INFO("Encoding PDSCH SF: %d (TB 2), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+           cfg->sf_idx, srslte_mod_string(cfg->grant.mcs2.mod), cfg->grant.mcs2.tbs,
+           cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv2);
+    }
+
+    /* number of layers equals number of ports */
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      x[i] = q->x[i];
+    }
+    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
+
+    if (srslte_dlsch_encode_multi(&q->dl_sch, cfg, softbuffers, data, (uint8_t *[SRSLTE_MAX_CODEWORDS]) {q->e, q->e2})) {
+      fprintf(stderr, "Error encoding TB\n");
+      return SRSLTE_ERROR;
+    }
+
+    /* scramble */
+    if (!q->users[rnti]) {
+      srslte_sequence_t seq;
+
+      if (cfg->nbits.nof_bits) {
+        if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
+          return SRSLTE_ERROR;
+        }
+        srslte_scrambling_bytes(&seq, (uint8_t *) q->e, cfg->nbits.nof_bits);
+        srslte_sequence_free(&seq);
+      }
+
+      if (cfg->nbits2.nof_bits) {
+        if (srslte_sequence_pdsch(&seq, rnti, 1, 2 * cfg->sf_idx, q->cell.id, cfg->nbits2.nof_bits)) {
+          return SRSLTE_ERROR;
+        }
+        srslte_scrambling_bytes(&seq, (uint8_t *) q->e2, cfg->nbits2.nof_bits);
+        srslte_sequence_free(&seq);
+      }
+    } else {
+      if (cfg->nbits.nof_bits) {
+        srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t *) q->e, cfg->nbits.nof_bits);
+      }
+
+      if (cfg->nbits2.nof_bits) {
+        srslte_scrambling_bytes(&q->users[rnti]->seq2[cfg->sf_idx], (uint8_t *) q->e2, cfg->nbits2.nof_bits);
+      }
+    }
+
+    if (cfg->nbits.nof_bits) {
+      srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t *) q->e, q->d, cfg->nbits.nof_bits);
+    }
+
+    if (cfg->nbits2.nof_bits) {
+      srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs2.mod], (uint8_t *) q->e2, q->d2, cfg->nbits2.nof_bits);
+    }
+
+    if (q->cell.nof_ports > 1) {
+      int nof_symbols = srslte_layermap_type((cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, x, cfg->grant.nof_tb, cfg->nof_layers,
+                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits.nof_re, cfg->nbits2.nof_re}, cfg->mimo_type);
+      srslte_precoding_type(x, q->symbols, q->cell.nof_ports, cfg->nof_layers,
+                            nof_symbols, cfg->mimo_type);
+    } else {
+      memcpy(q->symbols[0], q->d, cfg->nbits.nof_re * sizeof(cf_t));
+    }
+
+    /* mapping to resource elements */
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
+    }
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 float srslte_pdsch_average_noi(srslte_pdsch_t *q) 
 {
   return q->dl_sch.average_nof_iterations;
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 6a889b89c..a61f798fb 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -826,4 +826,4 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
 }
 
 
-  
\ No newline at end of file
+  
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 976a669f4..635172762 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -496,7 +496,6 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
     grant->mcs.tbs = (uint32_t) tbs; 
   } else {
     n_prb = grant->nof_prb;
-    grant->nof_tb = 0; 
     if (dci->tb_en[0]) {
       grant->mcs.idx = dci->mcs_idx;
       tbs   = dl_fill_ra_mcs(&grant->mcs, n_prb);
@@ -506,7 +505,6 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
         // For mcs>=29, set last TBS received for this PID
         grant->mcs.tbs = last_dl_tbs[dci->harq_process%8]; 
       }
-      grant->nof_tb++;
     } else {
       grant->mcs.tbs = 0; 
     }
@@ -519,16 +517,18 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
         // For mcs>=29, set last TBS received for this PID
         grant->mcs2.tbs = last_dl_tbs2[dci->harq_process%8]; 
       }
-      grant->nof_tb++;
     } else {
       grant->mcs2.tbs = 0; 
     }
-  }  
+  }
+  grant->nof_tb = 0;
   if (dci->tb_en[0]) {
-    grant->Qm = srslte_mod_bits_x_symbol(grant->mcs.mod);      
+    grant->Qm = srslte_mod_bits_x_symbol(grant->mcs.mod);
+    grant->nof_tb++;
   }
   if (dci->tb_en[1]) {
-    grant->Qm2 = srslte_mod_bits_x_symbol(grant->mcs2.mod);      
+    grant->Qm2 = srslte_mod_bits_x_symbol(grant->mcs2.mod);
+    grant->nof_tb++;
   }
   if (tbs < 0) {
     return SRSLTE_ERROR; 
@@ -546,6 +546,23 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
   nbits->nof_bits = nbits->nof_re * grant->Qm;      
 }
 
+void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx,
+                                       srslte_ra_nbits_t *nbits, srslte_ra_nbits_t *nbits2) {
+  /* Compute number of RE for first transport block */
+  nbits->nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb<10?(cfi+1):cfi);
+  nbits->lstart = cell.nof_prb<10?(cfi+1):cfi;
+  nbits->nof_symb = 2*SRSLTE_CP_NSYMB(cell.cp)-nbits->lstart;
+  nbits->nof_bits = nbits->nof_re * grant->Qm;
+
+  /*/ Compute number of RE for second transport block */
+  if (grant->nof_tb > 1) {
+    nbits2->nof_re = nbits->nof_re;
+    nbits2->lstart = nbits->lstart;
+    nbits2->nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits2->lstart;
+    nbits2->nof_bits = nbits2->nof_re * grant->Qm2;
+  }
+}
+
 /** Obtains a DL grant from a DCI grant for PDSCH */
 int srslte_ra_dl_dci_to_grant(srslte_ra_dl_dci_t *dci, 
                               uint32_t nof_prb, uint16_t msg_rnti, srslte_ra_dl_grant_t *grant) 
@@ -796,18 +813,31 @@ void srslte_ra_pdsch_fprint(FILE *f, srslte_ra_dl_dci_t *dci, uint32_t nof_prb)
     }
     break;
   }
-  fprintf(f, " - Modulation and coding scheme index:\t%d\n", dci->mcs_idx);
   fprintf(f, " - HARQ process:\t\t\t%d\n", dci->harq_process);
-  fprintf(f, " - New data indicator:\t\t\t%s\n", dci->ndi ? "Yes" : "No");
-  fprintf(f, " - Redundancy version:\t\t\t%d\n", dci->rv_idx);
   fprintf(f, " - TPC command for PUCCH:\t\t--\n");
+  fprintf(f, " - Transport blocks swapped:\t\t%s\n", (dci->tb_cw_swap)?"true":"false");
+  fprintf(f, " - Transport block 1 enabled:\t\t%s\n", (dci->tb_en[0])?"true":"false");
+  if (dci->tb_en[0]) {
+    fprintf(f, "   + Modulation and coding scheme index:\t%d\n", dci->mcs_idx);
+    fprintf(f, "   + New data indicator:\t\t\t%s\n", dci->ndi ? "Yes" : "No");
+    fprintf(f, "   + Redundancy version:\t\t\t%d\n", dci->rv_idx);
+  }
+  fprintf(f, " - Transport block 2 enabled:\t\t%s\n", (dci->tb_en[1])?"true":"false");
+  if (dci->tb_en[1]) {
+    fprintf(f, "   + Modulation and coding scheme index:\t%d\n", dci->mcs_idx_1);
+    fprintf(f, "   + New data indicator:\t\t\t%s\n", dci->ndi_1 ? "Yes" : "No");
+    fprintf(f, "   + Redundancy version:\t\t\t%d\n", dci->rv_idx_1);
+  }
 }
 
 void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
+  fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
   fprintf(f, " - Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs.mod));
   fprintf(f, " - Transport block size:\t\t%d\n", grant->mcs.tbs);
+  fprintf(f, " - Modulation type (TB2):\t\t%s\n", srslte_mod_string(grant->mcs2.mod));
+  fprintf(f, " - Transport block size (TB2):\t\t%d\n", grant->mcs2.tbs);
 }
 
 void srslte_ra_prb_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index dceab2506..f0515227e 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -523,6 +523,32 @@ int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
                    e_bits, data);
 }
 
+
+int srslte_dlsch_decode_multi(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                        int16_t *e_bits[SRSLTE_MAX_CODEWORDS], uint8_t *data[SRSLTE_MAX_CODEWORDS])
+{
+  int ret  = SRSLTE_SUCCESS;
+  uint32_t Nl = 1;
+
+  if (cfg->nof_layers != cfg->grant.nof_tb) {
+    Nl = 2;
+  }
+
+  if (cfg->nbits.nof_bits) {
+    ret |= decode_tb(q, &softbuffers[0], &cfg->cb_segm,
+                     cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
+                     e_bits[0], data[0]);
+  }
+
+  if (cfg->nbits2.nof_bits) {
+    ret |= decode_tb(q, &softbuffers[1], &cfg->cb_segm2,
+                     cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
+                     e_bits[1], data[1]);
+  }
+
+  return ret;
+}
+
 /**
  * Encode transport block. Segments into code blocks, adds channel coding, and does rate matching.
  *
@@ -542,6 +568,35 @@ int srslte_dlsch_encode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
                    data, e_bits);
 }
 
+int srslte_dlsch_encode_multi(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                        uint8_t *data[SRSLTE_MAX_CODEWORDS], uint8_t *e_bits[SRSLTE_MAX_CODEWORDS])
+{
+  int ret = 0;
+  uint32_t Nl = 1;
+
+  if (cfg->nof_layers != cfg->grant.nof_tb) {
+    Nl = 2;
+  }
+
+  /* Check if codeword 1 shall be encoded */
+  if(cfg->nbits.nof_bits) {
+    ret |= encode_tb(q,
+                     &softbuffers[0], &cfg->cb_segm,
+                     cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
+                     data[0], e_bits[0]);
+  }
+
+  /* Check if codeword 2 shall be encoded */
+  if(cfg->nbits2.nof_bits) {
+    ret |= encode_tb(q,
+                     &softbuffers[1], &cfg->cb_segm2,
+                     cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
+                     data[1], e_bits[1]);
+  }
+
+  return ret;
+}
+
 /* Compute the interleaving function on-the-fly, because it depends on number of RI bits 
  * Profiling show that the computation of this matrix is neglegible. 
  */
diff --git a/lib/src/phy/phch/test/CMakeLists.txt b/lib/src/phy/phch/test/CMakeLists.txt
index 6170f82fe..1c9846c76 100644
--- a/lib/src/phy/phch/test/CMakeLists.txt
+++ b/lib/src/phy/phch/test/CMakeLists.txt
@@ -87,6 +87,27 @@ add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100)
 add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100 -r 2)
 add_test(pdsch_test_qam64 pdsch_test -m 28 -n 100)
 
+add_test(pdsch_test_sin_6   pdsch_test -p 1 -a 2 -w 1 -n 6)
+add_test(pdsch_test_sin_12  pdsch_test -p 1 -a 2 -w 1 -n 12)
+add_test(pdsch_test_sin_25  pdsch_test -p 1 -a 2 -w 1 -n 25)
+add_test(pdsch_test_sin_50  pdsch_test -p 1 -a 2 -w 1 -n 50)
+add_test(pdsch_test_sin_75  pdsch_test -p 1 -a 2 -w 1 -n 75)
+add_test(pdsch_test_sin_100 pdsch_test -p 1 -a 2 -w 1 -n 100 -m 28)
+
+add_test(pdsch_test_div_6   pdsch_test -p 2 -a 2 -w 1 -n 6)
+add_test(pdsch_test_div_12  pdsch_test -p 2 -a 2 -w 1 -n 12)
+add_test(pdsch_test_div_25  pdsch_test -p 2 -a 2 -w 1 -n 25)
+add_test(pdsch_test_div_50  pdsch_test -p 2 -a 2 -w 1 -n 50)
+add_test(pdsch_test_div_75  pdsch_test -p 2 -a 2 -w 1 -n 75)
+add_test(pdsch_test_div_100 pdsch_test -p 2 -a 2 -w 1 -n 100 -m 28)
+
+add_test(pdsch_test_cdd_6   pdsch_test -p 2 -a 2 -w 2 -n 6)
+add_test(pdsch_test_cdd_12  pdsch_test -p 2 -a 2 -w 2 -n 12)
+add_test(pdsch_test_cdd_25  pdsch_test -p 2 -a 2 -w 2 -n 25)
+add_test(pdsch_test_cdd_50  pdsch_test -p 2 -a 2 -w 2 -n 50)
+add_test(pdsch_test_cdd_75  pdsch_test -p 2 -a 2 -w 2 -n 75)
+add_test(pdsch_test_cdd_100 pdsch_test -p 2 -a 2 -w 2 -n 100 -m 28 -M 28)
+
 ########################################################################
 # FILE TEST  
 ########################################################################
diff --git a/lib/src/phy/phch/test/dlsch_encode_test_mex.c b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
index 8c2318583..f391ab111 100644
--- a/lib/src/phy/phch/test/dlsch_encode_test_mex.c
+++ b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
@@ -43,9 +43,14 @@ void help()
 /* the gateway function */
 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 {
+  int i;
   srslte_sch_t dlsch;
   srslte_pdsch_cfg_t cfg; 
-  srslte_softbuffer_tx_t softbuffer; 
+  srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
+  uint32_t nof_codewords = 1;
+
+  memset(&dlsch, 0, sizeof(srslte_sch_t));
+  memset(&cfg, 0, sizeof(srslte_pdsch_cfg_t));
 
   if (nrhs < NOF_INPUTS) {
     help();
@@ -62,6 +67,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   srslte_verbose = SRSLTE_VERBOSE_NONE; 
   
   uint8_t *trblkin_bits = NULL;
+  cfg.grant.nof_tb = 1;
   cfg.grant.mcs.tbs = mexutils_read_uint8(TRBLKIN, &trblkin_bits);
   if (cfg.grant.mcs.tbs == 0) {
     mexErrMsgTxt("Error trblklen is zero\n");
@@ -76,6 +82,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     return;
   }
 
+  if (mexutils_read_uint32_struct(PUSCHCFG, "NLayers", &cfg.nof_layers)) {
+    mexErrMsgTxt("Field NLayers not found in dlsch config\n");
+    return;
+  }
+
   char *mod_str = mexutils_get_char_struct(PUSCHCFG, "Modulation");
   
   if (!strcmp(mod_str, "QPSK")) {
@@ -94,9 +105,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 
   mxFree(mod_str);
 
-  if (srslte_softbuffer_tx_init(&softbuffer, cell.nof_prb)) {
-    mexErrMsgTxt("Error initiating DL-SCH soft buffer\n");
-    return; 
+  /* Initialise buffers */
+  for (i = 0; i < nof_codewords; i++) {
+    softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_tx_t));
+    if (srslte_softbuffer_tx_init(softbuffers[i], cell.nof_prb)) {
+      mexErrMsgTxt("Error initiating DL-SCH soft buffer\n");
+      return;
+    }
   }
 
   cfg.nbits.nof_bits = mxGetScalar(OUTLEN); 
@@ -111,13 +126,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   uint32_t tmp_rv=cfg.rv;
   if (tmp_rv) {
     cfg.rv = 0; 
-    if (srslte_dlsch_encode(&dlsch, &cfg, &softbuffer, trblkin, e_bits)) {
+    if (srslte_dlsch_encode_multi(&dlsch, &cfg, softbuffers, &trblkin, &e_bits)) {
       mexErrMsgTxt("Error encoding TB\n");
       return;
     }
     cfg.rv = tmp_rv;
   }
-  if (srslte_dlsch_encode(&dlsch, &cfg, &softbuffer, trblkin, e_bits)) {
+  if (srslte_dlsch_encode_multi(&dlsch, &cfg, softbuffers, &trblkin, &e_bits)) {
     mexErrMsgTxt("Error encoding TB\n");
     return;
   }
@@ -135,7 +150,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   free(trblkin);
   free(e_bits);
   free(e_bits_unpacked);
-  
+  for (i = 0; i < nof_codewords; i++) {
+    srslte_softbuffer_tx_free(softbuffers[i]);
+    free(softbuffers[i]);
+  }
   return;
 }
 
diff --git a/lib/src/phy/phch/test/pdcch_test.c b/lib/src/phy/phch/test/pdcch_test.c
index 5d4fb0a7c..9495c497b 100644
--- a/lib/src/phy/phch/test/pdcch_test.c
+++ b/lib/src/phy/phch/test/pdcch_test.c
@@ -27,22 +27,26 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/phy/phch/ra.h>
+#include <srslte/phy/phch/dci.h>
+#include <srslte/phy/phch/pdcch.h>
 
 #include "srslte/srslte.h"
 
 srslte_cell_t cell = {
-  6,            // nof_prb
-  1,            // nof_ports
-  1,            // cell_id
-  SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+  .nof_prb = 6,
+  .nof_ports = 1,
+  .id = 1,
+  .cp = SRSLTE_CP_NORM,
+  .phich_resources = SRSLTE_PHICH_R_1,
+  .phich_length = SRSLTE_PHICH_NORM
 };
 
 uint32_t cfi = 1;
-bool print_dci_table; 
+uint32_t nof_rx_ant = 1;
+bool print_dci_table;
 
 void usage(char *prog) {
   printf("Usage: %s [cfpndv]\n", prog);
@@ -50,25 +54,29 @@ void usage(char *prog) {
   printf("\t-f cfi [Default %d]\n", cfi);
   printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
   printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-A nof_rx_ant [Default %d]\n", nof_rx_ant);
   printf("\t-d Print DCI table [Default %s]\n", print_dci_table?"yes":"no");
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "cfpndv")) != -1) {
+  while ((opt = getopt(argc, argv, "cfpndvA")) != -1) {
     switch (opt) {
     case 'p':
-      cell.nof_ports = atoi(argv[optind]);
+      cell.nof_ports = (uint32_t) atoi(argv[optind]);
       break;
     case 'f':
-      cfi = atoi(argv[optind]);
+      cfi = (uint32_t) atoi(argv[optind]);
       break;
     case 'n':
-      cell.nof_prb = atoi(argv[optind]);
+      cell.nof_prb = (uint32_t) atoi(argv[optind]);
       break;
     case 'c':
-      cell.id = atoi(argv[optind]);
+      cell.id = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'A':
+      nof_rx_ant = (uint32_t) atoi(argv[optind]);
       break;
     case 'd':
       print_dci_table = true;
@@ -85,25 +93,26 @@ void parse_args(int argc, char **argv) {
 
 int test_dci_payload_size() {
   int i, j;
-  int x[4];
-  const srslte_dci_format_t formats[4] = { SRSLTE_DCI_FORMAT0, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1C };
+  int x[5];
+  const srslte_dci_format_t formats[] = { SRSLTE_DCI_FORMAT0, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1C, SRSLTE_DCI_FORMAT2A };
   const int prb[6] = { 6, 15, 25, 50, 75, 100 };
-  const int dci_sz[6][5] = { { 21, 19, 21, 8 }, { 22, 23, 22, 10 }, { 25, 27,
-      25, 12 }, { 27, 31, 27, 13 }, { 27, 33, 27, 14 }, { 28, 39, 28, 15 } };
+  const int dci_sz[6][5] = { { 21, 19, 21, 8, 28 }, { 22, 23, 22, 10 , 31}, { 25, 27,
+      25, 12 , 36}, { 27, 31, 27, 13 , 41}, { 27, 33, 27, 14 , 42}, { 28, 39, 28, 15, 48 }};
+
 
   printf("Testing DCI payload sizes...\n");
-  printf("  PRB\t0\t1\t1A\t1C\n");
+  printf("  PRB\t0\t1\t1A\t1C\t2A\n");
   for (i = 0; i < 6; i++) {
     int n = prb[i];
-    for (j = 0; j < 4; j++) {
-      x[j] = srslte_dci_format_sizeof(formats[j], n, 1);
+    for (j = 0; j < 5; j++) {
+      x[j] = srslte_dci_format_sizeof(formats[j], (uint32_t) n, 1);
       if (x[j] != dci_sz[i][j]) {
         fprintf(stderr, "Invalid DCI payload size for %s\n",
             srslte_dci_format_string(formats[j]));
         return -1;
       }
     }
-    printf("  %2d:\t%2d\t%2d\t%2d\t%2d\n", n, x[0], x[1], x[2], x[3]);
+    printf("  %2d:\t%2d\t%2d\t%2d\t%2d\t%2d\n", n, x[0], x[1], x[2], x[3], x[4]);
   }
   printf("Ok\n");
   
@@ -111,8 +120,8 @@ int test_dci_payload_size() {
     printf("dci_sz_table[101][4] = {\n");
     for (i=0;i<=100;i++) {
       printf("  {");
-      for (int j=0;j<4;j++) {
-        printf("%d",srslte_dci_format_sizeof(formats[j], i, 1));
+      for (j=0;j<4;j++) {
+        printf("%d",srslte_dci_format_sizeof(formats[j], (uint32_t) i, 1));
         if (j<3) {
           printf(", ");
         }
@@ -128,16 +137,23 @@ int test_dci_payload_size() {
   return 0;
 }
 
+typedef struct {
+  srslte_dci_msg_t dci_tx, dci_rx;
+  srslte_dci_location_t dci_location;
+  srslte_dci_format_t dci_format;
+  srslte_ra_dl_dci_t ra_dl_tx;
+  srslte_ra_dl_dci_t ra_dl_rx;
+} testcase_dci_t;
+
 int main(int argc, char **argv) {
-  srslte_pdcch_t pdcch;
-  srslte_dci_msg_t dci_tx[2], dci_rx[2], dci_tmp;
-  srslte_dci_location_t dci_locations[2];
+  srslte_pdcch_t pdcch_tx, pdcch_rx;
+  testcase_dci_t testcases[10] = {0};
   srslte_ra_dl_dci_t ra_dl;
   srslte_regs_t regs;
-  int i, j;
-  cf_t *ce[SRSLTE_MAX_PORTS];
+  int i, j, k;
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   int nof_re;
-  cf_t *slot_symbols[SRSLTE_MAX_PORTS];
+  cf_t *tx_slot_symbols[SRSLTE_MAX_PORTS], *rx_slot_symbols[SRSLTE_MAX_PORTS];
   int nof_dcis; 
 
   int ret = -1;
@@ -152,19 +168,30 @@ int main(int argc, char **argv) {
 
   /* init memory */
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    ce[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!ce[i]) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      ce[i][j] = malloc(sizeof(cf_t) * nof_re);
+      if (!ce[i][j]) {
+        perror("malloc");
+        exit(-1);
+      }
+      for (k = 0; k < nof_re; k++) {
+        //ce[i][j][k] = (i == j) ? 1 : 0;
+        ce[i][j][k] = ((float)rand()/(float)RAND_MAX) + _Complex_I*((float)rand()/(float)RAND_MAX);
+      }
+    }
+    tx_slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
+    if (!tx_slot_symbols[i]) {
       perror("malloc");
       exit(-1);
     }
-    for (j = 0; j < nof_re; j++) {
-      ce[i][j] = 1;
-    }
-    slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!slot_symbols[i]) {
+    bzero(tx_slot_symbols[i], sizeof(cf_t) * nof_re);
+
+    rx_slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
+    if (!rx_slot_symbols[i]) {
       perror("malloc");
       exit(-1);
     }
+    bzero(rx_slot_symbols[i], sizeof(cf_t) * nof_re);
   }
 
   if (srslte_regs_init(&regs, cell)) {
@@ -177,12 +204,18 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init_tx(&pdcch_tx, &regs, cell)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+
+  if (srslte_pdcch_init_rx(&pdcch_rx, &regs, cell, nof_rx_ant)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
 
-  nof_dcis = 2;
+  /* Resource allocate init */
+  nof_dcis = 0;
   bzero(&ra_dl, sizeof(srslte_ra_dl_dci_t));
   ra_dl.harq_process = 0;
   ra_dl.mcs_idx = 5;
@@ -190,62 +223,131 @@ int main(int argc, char **argv) {
   ra_dl.rv_idx = 0;
   ra_dl.alloc_type = SRSLTE_RA_ALLOC_TYPE0;
   ra_dl.type0_alloc.rbg_bitmask = 0x5;
+  ra_dl.tb_en[0] = true;
 
-  srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_tx[0], cell.nof_prb, cell.nof_ports, false);
-  srslte_dci_location_set(&dci_locations[0], 0, 0);
+  /* Format 1 Test case */
+  testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT1;
+  testcases[nof_dcis].ra_dl_tx = ra_dl;
+  nof_dcis++;
 
+  /* Format 1 Test case */
   ra_dl.mcs_idx = 15;
-  srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_tx[1], cell.nof_prb, cell.nof_ports, false);
-  srslte_dci_location_set(&dci_locations[1], 0, 1);
-  
+  testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT1;
+  testcases[nof_dcis].ra_dl_tx = ra_dl;
+  nof_dcis++;
+
+  /* Tx Diversity Test case */
+  if (cell.nof_ports > 1) {
+    ra_dl.mcs_idx_1 = 0;
+    ra_dl.rv_idx_1 = 0;
+    ra_dl.ndi_1 = false;
+    ra_dl.tb_en[1] = false;
+    testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT2A;
+    testcases[nof_dcis].ra_dl_tx = ra_dl;
+    nof_dcis++;
+  }
+
+  /* CDD Spatial Multiplexing Test case */
+  if (cell.nof_ports > 1) {
+    ra_dl.mcs_idx_1 = 28;
+    ra_dl.rv_idx_1 = 1;
+    ra_dl.ndi_1 = false;
+    ra_dl.tb_en[1] = true;
+    testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT2A;
+    testcases[nof_dcis].ra_dl_tx = ra_dl;
+    nof_dcis++;
+  }
+
+  /* Execute Rx */
   for (i=0;i<nof_dcis;i++) {
-    if (srslte_pdcch_encode(&pdcch, &dci_tx[i], dci_locations[i], 1234+i, slot_symbols, 0, cfi)) {
+    srslte_dci_msg_pack_pdsch(&testcases[i].ra_dl_tx, testcases[i].dci_format, &testcases[i].dci_tx,
+                              cell.nof_prb, cell.nof_ports, false);
+    srslte_dci_location_set(&testcases[i].dci_location, 0, (uint32_t) i);
+
+    if (srslte_pdcch_encode(&pdcch_tx, &testcases[i].dci_tx, testcases[i].dci_location, (uint16_t) (1234 + i),
+                            tx_slot_symbols, 0, cfi)) {
       fprintf(stderr, "Error encoding DCI message\n");
       goto quit;
     }
   }
 
-  srslte_vec_fprint_b(stdout, dci_tx[0].data, dci_tx[0].nof_bits);
-  /* combine outputs */
-  for (i = 1; i < cell.nof_ports; i++) {
-    for (j = 0; j < nof_re; j++) {
-      slot_symbols[0][j] += slot_symbols[i][j];
+  /* Apply channel */
+  for (j = 0; j < nof_rx_ant; j++) {
+    for (k = 0; k < nof_re; k++) {
+      for (i = 0; i < cell.nof_ports; i++) {
+        rx_slot_symbols[j][k] += tx_slot_symbols[i][k]*ce[i][j][k];
+      }
     }
   }
 
-  for (i=0;i<2;i++) {
-    if (srslte_pdcch_extract_llr(&pdcch, slot_symbols[0], ce, 0, 0, cfi)) {
-      fprintf(stderr, "Error extracting LLRs\n");
+  /* Execute 'Rx' */
+  if (srslte_pdcch_extract_llr_multi(&pdcch_rx, rx_slot_symbols, ce, 0, 0, cfi)) {
+    fprintf(stderr, "Error extracting LLRs\n");
+    goto quit;
+  }
+
+  /* Decode DCIs */
+  for (i=0;i<nof_dcis;i++) {
+    uint16_t crc_rem;
+    if (srslte_pdcch_decode_msg(&pdcch_rx, &testcases[i].dci_rx, &testcases[i].dci_location, testcases[i].dci_format, &crc_rem)) {
+      fprintf(stderr, "Error decoding DCI message\n");
       goto quit;
     }
-    uint16_t crc_rem; 
-    if (srslte_pdcch_decode_msg(&pdcch, &dci_tmp, &dci_locations[i], SRSLTE_DCI_FORMAT1, &crc_rem)) {
-      fprintf(stderr, "Error decoding DCI message\n");
+    if (srslte_dci_msg_unpack_pdsch(&testcases[i].dci_rx, &testcases[i].ra_dl_rx, cell.nof_prb, cell.nof_ports, false)) {
+      fprintf(stderr, "Error unpacking DCI message\n");
       goto quit;
-    }      
+    }
     if (crc_rem >= 1234 && crc_rem < 1234 + nof_dcis) {
       crc_rem -= 1234;
-        memcpy(&dci_rx[crc_rem], &dci_tmp, sizeof(srslte_dci_msg_t));
     } else {
       printf("Received invalid DCI CRC 0x%x\n", crc_rem);
       goto quit;
     }
   }
+
+  /* Compare Tx and Rx */
   for (i = 0; i < nof_dcis; i++) {
-    if (memcmp(dci_tx[i].data, dci_rx[i].data, dci_tx[i].nof_bits)) {
+    if (memcmp(testcases[i].dci_tx.data, testcases[i].dci_rx.data, testcases[i].dci_tx.nof_bits)) {
       printf("Error in DCI %d: Received data does not match\n", i);
       goto quit;
     }
+    if (memcmp(&testcases[i].ra_dl_tx, &testcases[i].ra_dl_rx, sizeof(srslte_ra_dl_dci_t))) {
+      printf("Error in RA %d: Received data does not match\n", i);
+      printf("     Field    |    Tx    |    Rx    \n");
+      printf("--------------+----------+----------\n");
+      printf(" harq_process | %8d | %8d\n", testcases[i].ra_dl_tx.harq_process, testcases[i].ra_dl_rx.harq_process);
+      printf("      mcs_idx | %8d | %8d\n", testcases[i].ra_dl_tx.mcs_idx, testcases[i].ra_dl_rx.mcs_idx);
+      printf("       rv_idx | %8d | %8d\n", testcases[i].ra_dl_tx.rv_idx, testcases[i].ra_dl_rx.rv_idx);
+      printf("          ndi | %8d | %8d\n", testcases[i].ra_dl_tx.ndi, testcases[i].ra_dl_rx.ndi);
+      printf("    mcs_idx_1 | %8d | %8d\n", testcases[i].ra_dl_tx.mcs_idx_1, testcases[i].ra_dl_rx.mcs_idx_1);
+      printf("     rv_idx_1 | %8d | %8d\n", testcases[i].ra_dl_tx.rv_idx_1, testcases[i].ra_dl_rx.rv_idx_1);
+      printf("        ndi_1 | %8d | %8d\n", testcases[i].ra_dl_tx.ndi_1, testcases[i].ra_dl_rx.ndi_1);
+      printf("   tb_cw_swap | %8d | %8d\n", testcases[i].ra_dl_tx.tb_cw_swap, testcases[i].ra_dl_rx.tb_cw_swap);
+      printf("      sram_id | %8d | %8d\n", testcases[i].ra_dl_tx.sram_id, testcases[i].ra_dl_rx.sram_id);
+      printf("        pinfo | %8d | %8d\n", testcases[i].ra_dl_tx.pinfo, testcases[i].ra_dl_rx.pinfo);
+      printf("        pconf | %8d | %8d\n", testcases[i].ra_dl_tx.pconf, testcases[i].ra_dl_rx.pconf);
+      printf(" power_offset | %8d | %8d\n", testcases[i].ra_dl_tx.power_offset, testcases[i].ra_dl_rx.power_offset);
+      printf("    tpc_pucch | %8d | %8d\n", testcases[i].ra_dl_tx.tpc_pucch, testcases[i].ra_dl_rx.tpc_pucch);
+      printf("     tb_en[0] | %8d | %8d\n", testcases[i].ra_dl_tx.tb_en[0], testcases[i].ra_dl_rx.tb_en[0]);
+      printf("     tb_en[1] | %8d | %8d\n", testcases[i].ra_dl_tx.tb_en[1], testcases[i].ra_dl_rx.tb_en[1]);
+      printf("    dci_is_1a | %8d | %8d\n", testcases[i].ra_dl_tx.dci_is_1a, testcases[i].ra_dl_rx.dci_is_1a);
+      printf("    dci_is_1c | %8d | %8d\n", testcases[i].ra_dl_tx.dci_is_1c, testcases[i].ra_dl_rx.dci_is_1c);
+      goto quit;
+    }
   }
   ret = 0;
 
 quit: 
-  srslte_pdcch_free(&pdcch);
+  srslte_pdcch_free(&pdcch_tx);
+  srslte_pdcch_free(&pdcch_rx);
   srslte_regs_free(&regs);
 
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    free(ce[i]);
-    free(slot_symbols[i]);
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      free(ce[i][j]);
+    }
+    free(tx_slot_symbols[i]);
+    free(rx_slot_symbols[i]);
   }
   if (ret) {
     printf("Error\n");
diff --git a/lib/src/phy/phch/test/pdcch_test_mex.c b/lib/src/phy/phch/test/pdcch_test_mex.c
index e30ef900f..76b0bb021 100644
--- a/lib/src/phy/phch/test/pdcch_test_mex.c
+++ b/lib/src/phy/phch/test/pdcch_test_mex.c
@@ -33,13 +33,14 @@
 
 #define ENBCFG  prhs[0]
 #define RNTI    prhs[1]
-#define INPUT   prhs[2]
-#define NOF_INPUTS 3
+#define AMP     prhs[2]
+#define INPUT   prhs[3]
+#define NOF_INPUTS 4
 
 
 
-srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1}; // SRSLTE_DCI_FORMAT1B should go here also
-const uint32_t nof_ue_formats = 2; 
+srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1,SRSLTE_DCI_FORMAT2B}; // SRSLTE_DCI_FORMAT1B should go here also
+const uint32_t nof_ue_formats = 3; 
 
 srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
@@ -162,7 +163,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   } else {
     noise_power = srslte_chest_dl_get_noise_estimate(&chest);
   }
-  mexPrintf("noise power=%f, RNTI=0x%x, cfi=%d\n", noise_power, rnti, cfi);
+  
+  float amplitude = mxGetScalar(AMP);
+  
+  srslte_viterbi_set_gain_quant(&pdcch.decoder, amplitude);
   
   srslte_pdcch_extract_llr(&pdcch, input_fft, ce, noise_power, sf_idx, cfi);
   
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index d4e5163d3..2dc21bab4 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -30,45 +30,60 @@
 #include <strings.h>
 #include <unistd.h>
 #include <sys/time.h>
+#include <srslte/phy/phch/ra.h>
 
 #include "srslte/srslte.h"
 
 // Enable to measure execution time
 //#define DO_OFDM
 
+#ifdef DO_OFDM
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_PRB(cell.nof_prb)
+#else
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp)
+#endif
+
 srslte_cell_t cell = {
   6,            // nof_prb
   1,            // nof_ports
   0,            // cell_id
   SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1_6,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+  SRSLTE_PHICH_NORM,    // PHICH length
+  SRSLTE_PHICH_R_1_6    // PHICH resources
 };
 
 uint32_t cfi = 2;
 uint32_t mcs = 0;
+uint32_t mcs2 = 0;
 uint32_t subframe = 1;
 uint32_t rv_idx = 0;
-uint16_t rnti = 1234; 
+uint32_t rv_idx2 = 0;
+uint16_t rnti = 1234;
+uint32_t nof_tb = 1;
+uint32_t nof_rx_antennas = 1;
 char *input_file = NULL; 
 
 void usage(char *prog) {
-  printf("Usage: %s [fmcsrRFpnv] \n", prog);
+  printf("Usage: %s [fmMcsrtRFpnwav] \n", prog);
   printf("\t-f read signal from file [Default generate it with pdsch_encode()]\n");
   printf("\t-m MCS [Default %d]\n", mcs);
+  printf("\t-M MCS2 [Default %d]\n", mcs2);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-s subframe [Default %d]\n", subframe);
   printf("\t-r rv_idx [Default %d]\n", rv_idx);
+  printf("\t-t rv_idx2 [Default %d]\n", rv_idx2);
   printf("\t-R rnti [Default %d]\n", rnti);
   printf("\t-F cfi [Default %d]\n", cfi);
   printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
   printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-w nof_tb [Default %d]\n", nof_tb);
+  printf("\t-a nof_rx_antennas [Default %d]\n", nof_rx_antennas);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "fmcsrRFpnv")) != -1) {
+  while ((opt = getopt(argc, argv, "fmMcsrtRFpnwav")) != -1) {
     switch(opt) {
     case 'f':
       input_file = argv[optind];
@@ -76,12 +91,18 @@ void parse_args(int argc, char **argv) {
     case 'm':
       mcs = atoi(argv[optind]);
       break;
+    case 'M':
+      mcs2 = (uint32_t) atoi(argv[optind]);
+      break;
     case 's':
       subframe = atoi(argv[optind]);
       break;
     case 'r':
       rv_idx = atoi(argv[optind]);
       break;
+    case 't':
+      rv_idx2 = (uint32_t) atoi(argv[optind]);
+      break;
     case 'R':
       rnti = atoi(argv[optind]);
       break;
@@ -97,6 +118,12 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
+    case 'w':
+      nof_tb = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'a':
+      nof_rx_antennas = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -107,30 +134,37 @@ void parse_args(int argc, char **argv) {
   }
 }
 
-uint8_t *data = NULL;
-cf_t *ce[SRSLTE_MAX_PORTS];
-srslte_softbuffer_rx_t softbuffer_rx;
+uint8_t *data[SRSLTE_MAX_CODEWORDS] = {NULL};
+cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+srslte_softbuffer_rx_t softbuffers_rx[SRSLTE_MAX_CODEWORDS];
 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;
+#ifdef DO_OFDM
+cf_t *tx_sf_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_sf_symbols[SRSLTE_MAX_PORTS];
+#endif /* DO_OFDM */
+cf_t *tx_slot_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_slot_symbols[SRSLTE_MAX_PORTS];
+srslte_pdsch_t pdsch_tx, pdsch_rx;
 srslte_ofdm_t ofdm_tx, ofdm_rx; 
 
 int main(int argc, char **argv) {
-  uint32_t i, j;
+  uint32_t i, j, k;
   int ret = -1;
   struct timeval t[3];
-  srslte_softbuffer_tx_t softbuffer_tx;
+  srslte_softbuffer_tx_t softbuffers_tx[SRSLTE_MAX_CODEWORDS];
   
   parse_args(argc,argv);
 
-  bzero(&pdsch, sizeof(srslte_pdsch_t));
+  /* Initialise to zeros */
+  bzero(&pdsch_tx, sizeof(srslte_pdsch_t));
+  bzero(&pdsch_rx, sizeof(srslte_pdsch_t));
   bzero(&pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  bzero(slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  bzero(&softbuffer_rx, sizeof(srslte_softbuffer_rx_t));
-  bzero(&softbuffer_tx, sizeof(srslte_softbuffer_tx_t));
+  bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(softbuffers_tx, sizeof(srslte_softbuffer_tx_t)*SRSLTE_MAX_CODEWORDS);
+  bzero(softbuffers_rx, sizeof(srslte_softbuffer_rx_t)*SRSLTE_MAX_CODEWORDS);
   
   srslte_ra_dl_dci_t dci;
   bzero(&dci, sizeof(srslte_ra_dl_dci_t));
@@ -138,57 +172,136 @@ int main(int argc, char **argv) {
   dci.rv_idx = rv_idx;
   dci.type0_alloc.rbg_bitmask = 0xffffffff;
   dci.tb_en[0] = true; 
+  if (nof_tb > 1) {
+    dci.mcs_idx_1 = mcs2;
+    dci.rv_idx_1 = rv_idx2;
+    dci.tb_en[1] = true;
+  }
+
+  /* Generate grant from DCI */
   if (srslte_ra_dl_dci_to_grant(&dci, cell.nof_prb, rnti, &grant)) {
     fprintf(stderr, "Error computing resource allocation\n");
     return ret;
   }
   
+#ifdef DO_OFDM
   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));
-  
+  srslte_ofdm_set_normalize(&ofdm_tx, true);
+  srslte_ofdm_set_normalize(&ofdm_rx, true);
+
+  for (i = 0; i < cell.nof_ports; i++) {
+    tx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+
+  for (i = 0; i < nof_rx_antennas; i++) {
+    rx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+#endif /* DO_OFDM */
+
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx)) {
+  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, rv_idx2)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     exit(-1);
   }
 
+  /* Select MIMO mode */
+  if (cell.nof_ports == 1 && nof_tb == 1) {
+    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+    pdsch_cfg.nof_layers = 1;
+  } else if (cell.nof_ports == 2 && nof_tb == 1) {
+    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+    pdsch_cfg.nof_layers = 2;
+  } else if (cell.nof_ports == 2 && nof_tb == 2) {
+    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_CDD;
+    pdsch_cfg.nof_layers = 2;
+  } else {
+    fprintf(stderr, "nof_ports=%d, nof_tb=%d are not consistent\n", cell.nof_ports, nof_tb);
+    exit(-1);
+  }
+
   /* init memory */
   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]) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * NOF_CE_SYMBOLS);
+      if (!ce[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+        ce[i][j][k] = (i == j) ? 1.0f : 0.0f;
+      }
+    }
+    rx_slot_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
+    if (!rx_slot_symbols[i]) {
       perror("srslte_vec_malloc");
       goto quit;
     }
-    for (j=0;j<SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);j++) {
-      ce[i][j] = 1;
-    }
-    slot_symbols[i] = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
-    if (!slot_symbols[i]) {
+  }
+
+
+  if (grant.mcs.tbs) {
+    data[0] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs);
+    if (!data[0]) {
       perror("srslte_vec_malloc");
       goto quit;
     }
   }
-  
-  data = srslte_vec_malloc(sizeof(uint8_t) * (grant.mcs.tbs/8)+24);
-  if (!data) {
-    perror("srslte_vec_malloc");
-    goto quit;
+
+  if (grant.mcs2.tbs) {
+    data[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs);
+    if (!data[1]) {
+      perror("srslte_vec_malloc");
+      goto quit;
+    }
   }
 
-  if (srslte_pdsch_init(&pdsch, cell)) {
+  if (srslte_pdsch_init_rx_multi(&pdsch_rx, cell, nof_rx_antennas)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
-  
-  srslte_pdsch_set_rnti(&pdsch, rnti);
-  
-  if (srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb)) {
-    fprintf(stderr, "Error initiating RX soft buffer\n");
-    goto quit;
+
+  srslte_pdsch_set_rnti(&pdsch_rx, rnti);
+
+  for (i = 0; i < nof_tb; i++) {
+    if (srslte_softbuffer_rx_init(&softbuffers_rx[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating RX soft buffer\n");
+      goto quit;
+    }
   }
 
+  INFO("         nof_prb=%d\n", cell.nof_prb);
+  INFO("       nof_ports=%d\n", cell.nof_ports);
+  INFO("              id=%d\n", cell.id);
+  INFO("              cp=%s\n", srslte_cp_string(cell.cp));
+  INFO("    phich_length=%d\n", (int) cell.phich_length);
+  INFO(" phich_resources=%d\n", (int) cell.phich_resources);
+  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
+  INFO("              Qm=%d\n", pdsch_cfg.grant.Qm);
+  INFO("             Qm2=%d\n", pdsch_cfg.grant.Qm2);
+  INFO("         mcs.idx=0x%X\n", pdsch_cfg.grant.mcs.idx);
+  INFO("         mcs.tbs=%d\n", pdsch_cfg.grant.mcs.tbs);
+  INFO("         mcs.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs.mod));
+  INFO("        mcs2.idx=0x%X\n", pdsch_cfg.grant.mcs2.idx);
+  INFO("        mcs2.tbs=%d\n", pdsch_cfg.grant.mcs2.tbs);
+  INFO("        mcs2.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs2.mod));
+  INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
+  INFO("              rv=%d\n", pdsch_cfg.rv);
+  INFO("             rv2=%d\n", pdsch_cfg.rv2);
+  INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
+  INFO("       mimo_type=%d\n", (int) pdsch_cfg.mimo_type);
+  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("          lstart=%d\n", pdsch_cfg.nbits.lstart);
+  INFO("        nof_bits=%d\n", pdsch_cfg.nbits.nof_bits);
+  INFO("          nof_re=%d\n", pdsch_cfg.nbits.nof_re);
+  INFO("        nof_symb=%d\n", pdsch_cfg.nbits.nof_symb);
+  INFO("          lstart=%d\n", pdsch_cfg.nbits2.lstart);
+  INFO("        nof_bits=%d\n", pdsch_cfg.nbits2.nof_bits);
+  INFO("          nof_re=%d\n", pdsch_cfg.nbits2.nof_re);
+  INFO("        nof_symb=%d\n", pdsch_cfg.nbits2.nof_symb);
+
   if (input_file) {
     srslte_filesource_t fsrc;
     if (srslte_filesource_init(&fsrc, input_file, SRSLTE_COMPLEX_FLOAT_BIN)) {
@@ -196,102 +309,156 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 #ifdef DO_OFDM
-    srslte_filesource_read(&fsrc, sf_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
+    srslte_filesource_read(&fsrc, rx_slot_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));
+    srslte_filesource_read(&fsrc, rx_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");
+      fprintf(stderr, "Error initializing equalizer\n");
       exit(-1);
     }
-    srslte_chest_dl_estimate(&chest, slot_symbols[0], ce, subframe);    
+    srslte_chest_dl_estimate(&chest, rx_slot_symbols[0], ce[0], subframe);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
-  } else { 
-    if (srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb)) {
-      fprintf(stderr, "Error initiating TX soft buffer\n");
+  } else {
+
+    if (srslte_pdsch_init_tx_multi(&pdsch_tx, cell)) {
+      fprintf(stderr, "Error creating PDSCH object\n");
       goto quit;
     }
 
-    for (i=0;i<grant.mcs.tbs/8;i++) {
-      data[i] = rand()%256;
+    srslte_pdsch_set_rnti(&pdsch_tx, rnti);
+
+    for (i = 0; i < nof_tb; i++) {
+      if (srslte_softbuffer_tx_init(&softbuffers_tx[i], cell.nof_prb)) {
+        fprintf(stderr, "Error initiating TX soft buffer\n");
+        goto quit;
+      }
     }
-    
-    uint8_t databit[100000];
+
+    for (i = 0; i < cell.nof_ports; i++) {
+      tx_slot_symbols[i] = calloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp), sizeof(cf_t));
+      if (!tx_slot_symbols[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+    }
+
+    for (i = 0; i < grant.mcs.tbs / 8; i++) {
+      data[0][i] = (uint8_t) (rand() % 256);
+    }
+
+    for (i = 0; i < grant.mcs2.tbs / 8; i++) {
+      data[1][i] = (uint8_t) (rand() % 256);
+    }
+
+    /*uint8_t databit[100000];
     srslte_bit_unpack_vector(data, databit, grant.mcs.tbs);
-    srslte_vec_save_file("data_in", databit, grant.mcs.tbs);
+    srslte_vec_save_file("data_in", databit, grant.mcs.tbs);*/
     
-    if (!input_file) {
-      
-      if (rv_idx) {
-        /* Do 1st transmission for rv_idx!=0 */
-        pdsch_cfg.rv = 0;
-        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, rnti, slot_symbols)) {
-          fprintf(stderr, "Error encoding PDSCH\n");
-          goto quit;
-        }
-      }
-      pdsch_cfg.rv = rv_idx; 
-      
-      if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, rnti, slot_symbols)) {
+    if (rv_idx) {
+      /* Do 1st transmission for rv_idx!=0 */
+      pdsch_cfg.rv = 0;
+      pdsch_cfg.rv2 = 0;
+      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
         fprintf(stderr, "Error encoding PDSCH\n");
         goto quit;
       }
     }
-    
+    pdsch_cfg.rv = rv_idx;
+    pdsch_cfg.rv2 = rv_idx2;
+    if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
+      fprintf(stderr, "Error encoding PDSCH\n");
+      goto quit;
+    }
+
+  #ifdef DO_OFDM
+    for (i = 0; i < cell.nof_ports; i++) {
+      /* For each Tx antenna modulate OFDM */
+      srslte_ofdm_tx_sf(&ofdm_tx, tx_slot_symbols[i], tx_sf_symbols[i]);
+    }
+
     /* 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];
+    for (j = 0; j < nof_rx_antennas; j++) {
+      for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+        rx_sf_symbols[j][k] = 0.0f;
+        for (i = 0; i < cell.nof_ports; i++) {
+          rx_sf_symbols[j][k] += tx_sf_symbols[i][k] * ce[i][j][k];
+        }
+      }
+    }
+  #else
+    /* combine outputs */
+    for (j = 0; j < nof_rx_antennas; j++) {
+      for (k = 0; k < SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp); k++) {
+        rx_slot_symbols[j][k] = 0.0f;
+        for (i = 0; i < cell.nof_ports; i++) {
+          rx_slot_symbols[j][k] += tx_slot_symbols[i][k] * ce[i][j][k];
         }
-        ce[i][j] = 1;
       }
     }
-    
-  #ifdef DO_OFDM
-    srslte_ofdm_tx_sf(&ofdm_tx, slot_symbols[0], sf_symbols);
   #endif
-  } 
-  int M=1;
+
+
+  }
+  int M=10;
   int r=0; 
-  srslte_sch_set_max_noi(&pdsch.dl_sch, 10);
+  srslte_sch_set_max_noi(&pdsch_rx.dl_sch, 10);
   gettimeofday(&t[1], NULL);
-  for (i=0;i<M;i++) {
-  #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);    
-    r = srslte_pdsch_decode(&pdsch, &pdsch_cfg, &softbuffer_rx, slot_symbols[0], ce, 0, rnti, data);
+  for (k = 0; k < M; k++) {
+#ifdef DO_OFDM
+    /* For each Rx antenna demodulate OFDM */
+    for (i = 0; i < nof_rx_antennas; i++) {
+      srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
+    }
+#endif
+    if (grant.mcs.tbs) {
+      srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[0], (uint32_t) grant.mcs.tbs);
+    }
+    if (grant.mcs2.tbs) {
+      srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[1], (uint32_t) grant.mcs2.tbs);
+    }
+    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data);
   }
   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);                
+  INFO("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 + grant.mcs2.tbs)/1000.0f,
+         (float) (grant.mcs.tbs + grant.mcs2.tbs)*M/t[0].tv_usec);
   if (r) {
     ret = -1;
     goto quit;
   } 
 
   ret = 0;
+
 quit:
-  srslte_pdsch_free(&pdsch);
-  srslte_softbuffer_tx_free(&softbuffer_tx);
-  srslte_softbuffer_rx_free(&softbuffer_rx);
-  
-  for (i=0;i<cell.nof_ports;i++) {
-    if (ce[i]) {
-      free(ce[i]);
-    }
-    if (slot_symbols[i]) {
-      free(slot_symbols[i]);
+  srslte_pdsch_free(&pdsch_tx);
+  srslte_pdsch_free(&pdsch_rx);
+  for (i = 0; i < nof_tb; i++) {
+    srslte_softbuffer_tx_free(&softbuffers_tx[i]);
+    srslte_softbuffer_rx_free(&softbuffers_rx[i]);
+
+    if (data[i]) {
+      free(data[i]);
     }
   }
-  if (data) {
-    free(data);
+
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      if (ce[i][j]) {
+        free(ce[i][j]);
+      }
+    }
+    if (tx_slot_symbols[i]) {
+      free(tx_slot_symbols[i]);
+    }
+    if (rx_slot_symbols[i]) {
+      free(rx_slot_symbols[i]);
+    }
   }
   if (ret) {
     printf("Error\n");
diff --git a/lib/src/phy/phch/test/pdsch_test_mex.c b/lib/src/phy/phch/test/pdsch_test_mex.c
index c2273916a..204154130 100644
--- a/lib/src/phy/phch/test/pdsch_test_mex.c
+++ b/lib/src/phy/phch/test/pdsch_test_mex.c
@@ -101,7 +101,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     nof_antennas = dims[2];
   }
   
-  if (srslte_pdsch_init_multi(&pdsch, cell, nof_antennas)) {
+  if (srslte_pdsch_init_rx_multi(&pdsch, cell, nof_antennas)) {
     mexErrMsgTxt("Error initiating PDSCH\n");
     return;
   }
diff --git a/lib/src/phy/phch/test/prach_detect_test_mex.c b/lib/src/phy/phch/test/prach_detect_test_mex.c
index bfec8c23a..8db26eaff 100644
--- a/lib/src/phy/phch/test/prach_detect_test_mex.c
+++ b/lib/src/phy/phch/test/prach_detect_test_mex.c
@@ -104,6 +104,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     srslte_prach_set_detect_factor(&prach, factor);
   }
 
+  mexPrintf("format=%d config=%d, N_cp=%d, ifft=%d\n", prach.f, prach.config_idx, prach.N_cp, prach.N_ifft_ul);
+
   if (srslte_prach_detect_offset(&prach, frequency_offset, &input_signal[prach.N_cp], nof_samples, preambles, offsets, NULL, &nof_detected)) {    
     mexErrMsgTxt("Error detecting PRACH\n");
     return; 
diff --git a/lib/src/phy/phch/test/pucch_encode_test_mex.c b/lib/src/phy/phch/test/pucch_encode_test_mex.c
index ba8d41980..a7e5a94cb 100644
--- a/lib/src/phy/phch/test/pucch_encode_test_mex.c
+++ b/lib/src/phy/phch/test/pucch_encode_test_mex.c
@@ -172,7 +172,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   
   srslte_pucch_set_cfg(&pucch, &pucch_cfg, group_hopping_en);
   
-  if (srslte_pucch_encode(&pucch, format, n_pucch, sf_idx, bits, sf_symbols)) {
+  if (srslte_pucch_encode(&pucch, format, n_pucch, sf_idx, (uint16_t) rnti, bits, sf_symbols)) {
     mexErrMsgTxt("Error encoding PUCCH\n");
     return; 
   }
diff --git a/lib/src/phy/phch/test/pucch_test_mex.c b/lib/src/phy/phch/test/pucch_test_mex.c
index baf6c84db..9672230b2 100644
--- a/lib/src/phy/phch/test/pucch_test_mex.c
+++ b/lib/src/phy/phch/test/pucch_test_mex.c
@@ -187,12 +187,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   
   srslte_pucch_set_cfg(&pucch, &pucch_cfg, group_hopping_en);
   
-  if (srslte_chest_ul_estimate_pucch(&chest_ul, sf_symbols, ce, format, n_pucch, sf_idx)) {
+  uint8_t pucch2_ack_bits[2] = {0};
+
+  if (srslte_chest_ul_estimate_pucch(&chest_ul, sf_symbols, ce, format, n_pucch, sf_idx, &pucch2_ack_bits)) {
     mexErrMsgTxt("Error estimating PUCCH DMRS\n");
     return;
   }
   
-  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, sf_symbols, ce, 0, bits)<0) {
+  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, (uint16_t) rnti, sf_symbols, ce, 0, bits)<0) {
     mexErrMsgTxt("Error decoding PUCCH\n");
     return; 
   }
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index f3dd471f6..c48a2ad5b 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -178,7 +178,7 @@ float rf_blade_get_rssi(void *h)
   return 0;
 }
 
-int rf_blade_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
+int rf_blade_open_multi(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
 {
   return rf_blade_open(args, h); 
 }
@@ -469,6 +469,20 @@ int rf_blade_recv_with_time(void *h,
   return nsamples;
 }
                    
+int rf_blade_send_timed_multi(void *h,
+                     void *data[4],
+                     int nsamples,
+                     time_t secs,
+                     double frac_secs,                      
+                     bool has_time_spec,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst)
+{
+  return rf_blade_send_timed(h, data[0], nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst,
+                             is_end_of_burst);
+}
+
 int rf_blade_send_timed(void *h,
                      void *data,
                      int nsamples,
diff --git a/lib/src/phy/rf/rf_blade_imp.h b/lib/src/phy/rf/rf_blade_imp.h
index 07e9bb7cf..2876a5404 100644
--- a/lib/src/phy/rf/rf_blade_imp.h
+++ b/lib/src/phy/rf/rf_blade_imp.h
@@ -34,7 +34,7 @@ SRSLTE_API int rf_blade_open(char *args,
                         void **handler);
 
 SRSLTE_API int rf_blade_open_multi(char *args, 
-                        void **handler, uint32_t nof_rx_antennas);
+                        void **handler, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas);
 
 SRSLTE_API char* rf_blade_devname(void *h);
 
@@ -111,6 +111,16 @@ SRSLTE_API void rf_blade_get_time(void *h,
                               time_t *secs, 
                               double *frac_secs); 
 
+SRSLTE_API int rf_blade_send_timed_multi(void *h,
+                              void *data[4],
+                              int nsamples,
+                              time_t secs,
+                              double frac_secs,
+                              bool has_time_spec,
+                              bool blocking,
+                              bool is_start_of_burst,
+                              bool is_end_of_burst);
+
 SRSLTE_API int  rf_blade_send_timed(void *h, 
                                   void *data, 
                                   int nsamples,
diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index e1b761419..4e3f2681b 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -38,7 +38,7 @@ typedef struct {
   void   (*srslte_rf_suppress_stdout)(void *h);
   void   (*srslte_rf_register_error_handler)(void *h, srslte_rf_error_handler_t error_handler);
   int    (*srslte_rf_open)(char *args, void **h);
-  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_rx_antennas);
+  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas);
   int    (*srslte_rf_close)(void *h);
   void   (*srslte_rf_set_master_clock_rate)(void *h, double rate);
   bool   (*srslte_rf_is_master_clock_dynamic)(void *h);
@@ -58,6 +58,9 @@ typedef struct {
   int    (*srslte_rf_send_timed)(void *h, void *data, int nsamples,
                      time_t secs, double frac_secs, bool has_time_spec,
                      bool blocking, bool is_start_of_burst, bool is_end_of_burst);
+  int    (*srslte_rf_send_timed_multi)(void *h, void *data[4], int nsamples,
+                     time_t secs, double frac_secs, bool has_time_spec,
+                     bool blocking, bool is_start_of_burst, bool is_end_of_burst);
   void   (*srslte_rf_set_tx_cal)(void *h, srslte_rf_cal_t *cal);
 
   void   (*srslte_rf_set_rx_cal)(void *h, srslte_rf_cal_t *cal);
@@ -81,7 +84,7 @@ static rf_dev_t dev_uhd = {
   rf_uhd_suppress_stdout,
   rf_uhd_register_error_handler,
   rf_uhd_open,
-  rf_uhd_open_multi,
+  .srslte_rf_open_multi = rf_uhd_open_multi,
   rf_uhd_close,
   rf_uhd_set_master_clock_rate,
   rf_uhd_is_master_clock_dynamic,
@@ -97,6 +100,7 @@ static rf_dev_t dev_uhd = {
   rf_uhd_recv_with_time,
   rf_uhd_recv_with_time_multi,
   rf_uhd_send_timed,
+  .srslte_rf_send_timed_multi = rf_uhd_send_timed_multi,
   rf_uhd_set_tx_cal,
   rf_uhd_set_rx_cal
 };
@@ -119,7 +123,7 @@ static rf_dev_t dev_blade = {
   rf_blade_suppress_stdout,
   rf_blade_register_error_handler,
   rf_blade_open,
-  rf_blade_open_multi,
+  .srslte_rf_open_multi = rf_blade_open_multi,
   rf_blade_close,
   rf_blade_set_master_clock_rate,
   rf_blade_is_master_clock_dynamic,
@@ -135,6 +139,7 @@ static rf_dev_t dev_blade = {
   rf_blade_recv_with_time,
   rf_blade_recv_with_time_multi,
   rf_blade_send_timed,
+  .srslte_rf_send_timed_multi = rf_blade_send_timed_multi,
   rf_blade_set_tx_cal,
   rf_blade_set_rx_cal
 };
@@ -172,6 +177,7 @@ static rf_dev_t dev_soapy = {
   rf_soapy_recv_with_time,
   rf_soapy_recv_with_time_multi,
   rf_soapy_send_timed,
+  .srslte_rf_send_timed_multi = /* FIXME: Implement srslte_rf_send_timed_multi for Soapy SDR */ NULL,
   rf_soapy_set_tx_cal,
   rf_soapy_set_rx_cal
 };
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 92b8143da..0ae771043 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -103,6 +103,10 @@ int srslte_rf_open_devname(srslte_rf_t *rf, char *devname, char *args) {
 }
 
 int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_rx_antennas) {
+  return srslte_rf_open_devname_multi2(rf, devname, args, 1, nof_rx_antennas);
+}
+
+int srslte_rf_open_devname_multi2(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas) {
   /* Try to open the device if name is provided */
   if (devname) {
     if (devname[0] != '\0') {
@@ -110,7 +114,7 @@ int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uin
       while(available_devices[i] != NULL) {
         if (!strcmp(available_devices[i]->name, devname)) {
           rf->dev = available_devices[i];
-          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_rx_antennas);
+          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_tx_antennas, nof_rx_antennas);
         }
         i++;
       }    
@@ -121,7 +125,7 @@ int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uin
   /* If in auto mode or provided device not found, try to open in order of apperance in available_devices[] array */
   int i=0;
   while(available_devices[i] != NULL) {
-    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_rx_antennas)) {
+    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_tx_antennas, nof_rx_antennas)) {
       rf->dev = available_devices[i];
       return 0; 
     }
@@ -194,6 +198,11 @@ int srslte_rf_open_multi(srslte_rf_t *h, char *args, uint32_t nof_rx_antennas)
   return srslte_rf_open_devname_multi(h, NULL, args, nof_rx_antennas);
 }
 
+int srslte_rf_open_multi2(srslte_rf_t *h, char *args, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
+{
+  return srslte_rf_open_devname_multi2(h, NULL, args, nof_tx_antennas, nof_rx_antennas);
+}
+
 int srslte_rf_close(srslte_rf_t *rf)
 {
   return ((rf_dev_t*) rf->dev)->srslte_rf_close(rf->handler);  
@@ -301,6 +310,18 @@ int srslte_rf_send_timed3(srslte_rf_t *rf,
                                  has_time_spec, blocking, is_start_of_burst, is_end_of_burst);  
 }
 
+int srslte_rf_send_multi(srslte_rf_t *rf,
+                     void *data[4],
+                     int nsamples,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst)
+{
+
+  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, 0, 0,
+                                 false, blocking, is_start_of_burst, is_end_of_burst);
+}
+
 int srslte_rf_send(srslte_rf_t *rf, void *data, uint32_t nsamples, bool blocking)
 {
   return srslte_rf_send2(rf, data, nsamples, blocking, true, true); 
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 319093994..e0f3ac573 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -286,10 +286,10 @@ float rf_uhd_get_rssi(void *h) {
 
 int rf_uhd_open(char *args, void **h)
 {
-  return rf_uhd_open_multi(args, h, 1);
+  return rf_uhd_open_multi(args, h, 1, 1);
 }
 
-int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
+int rf_uhd_open_multi(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
 {
   if (h) {
     *h = NULL; 
@@ -395,12 +395,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
           .otw_format = "sc16",
           .args = "",
           .channel_list = channel,
-          .n_channels = 1
+          .n_channels = (nof_tx_antennas > nof_rx_antennas)?nof_tx_antennas:nof_rx_antennas,
       };
       
     handler->nof_rx_channels = nof_rx_antennas; 
-    handler->nof_tx_channels = 1; 
-    
+    handler->nof_tx_channels = nof_tx_antennas;
     /* Initialize rx and tx stremers */
     uhd_rx_streamer_make(&handler->rx_stream);
     error = uhd_usrp_get_rx_stream(handler->usrp, &stream_args, handler->rx_stream);
@@ -648,8 +647,28 @@ int rf_uhd_send_timed(void *h,
                      bool is_start_of_burst,
                      bool is_end_of_burst) 
 {
+  void *_data[SRSLTE_MAX_PORTS]= {data, zero_mem, zero_mem, zero_mem};
+
+  return rf_uhd_send_timed_multi(h, _data, nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
+}
+
+int rf_uhd_send_timed_multi(void *h,
+                            void *data[4],
+                            int nsamples,
+                            time_t secs,
+                            double frac_secs,
+                            bool has_time_spec,
+                            bool blocking,
+                            bool is_start_of_burst,
+                            bool is_end_of_burst) {
   rf_uhd_handler_t* handler = (rf_uhd_handler_t*) h;
   
+  /* Resets the USRP time FIXME: this might cause problems for burst transmissions */
+  if (is_start_of_burst && handler->nof_tx_channels > 1) {
+    uhd_usrp_set_time_now(handler->usrp, 0, 0, 0);
+    uhd_tx_metadata_set_time_spec(&handler->tx_md, 0, 0.1);
+  }
+
   size_t txd_samples;
   if (has_time_spec) {
     uhd_tx_metadata_set_time_spec(&handler->tx_md, secs, frac_secs);
@@ -657,7 +676,10 @@ int rf_uhd_send_timed(void *h,
   int trials = 0; 
   if (blocking) {
     int n = 0;
-    cf_t *data_c = (cf_t*) data;
+    cf_t *data_c[4];
+    for (int i = 0; i < 4; i++) {
+      data_c[i] = data[i];
+    }
     do {
       size_t tx_samples = handler->tx_nof_samples;
       
@@ -675,9 +697,12 @@ int rf_uhd_send_timed(void *h,
         tx_samples = nsamples - n; 
         uhd_tx_metadata_set_end(&handler->tx_md, is_end_of_burst);
       }
-      
-      void *buff = (void*) &data_c[n];
-      const void *buffs_ptr[4] = {buff, zero_mem, zero_mem, zero_mem};
+
+      const void *buffs_ptr[4];
+      for (int i = 0; i < 4; i++) {
+        void *buff = (void*) &data_c[i][n];
+        buffs_ptr[i] = buff;
+      }
       uhd_error error = uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, 
                                              tx_samples, &handler->tx_md, 3.0, &txd_samples);
       if (error) {
@@ -691,7 +716,10 @@ int rf_uhd_send_timed(void *h,
     } while (n < nsamples && trials < 100);
     return nsamples;
   } else {
-    const void *buffs_ptr[4] = {data, zero_mem, zero_mem, zero_mem};
+    const void *buffs_ptr[4];
+    for (int i = 0; i < 4; i++) {
+     buffs_ptr[i] = data[i];
+    }
     uhd_tx_metadata_set_start(&handler->tx_md, is_start_of_burst);
     uhd_tx_metadata_set_end(&handler->tx_md, is_end_of_burst);
     return uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, nsamples, &handler->tx_md, 0.0, &txd_samples);
diff --git a/lib/src/phy/rf/rf_uhd_imp.h b/lib/src/phy/rf/rf_uhd_imp.h
index 7c26f015c..6c0ab7dc7 100644
--- a/lib/src/phy/rf/rf_uhd_imp.h
+++ b/lib/src/phy/rf/rf_uhd_imp.h
@@ -39,6 +39,7 @@ SRSLTE_API int rf_uhd_open(char *args,
 
 SRSLTE_API int rf_uhd_open_multi(char *args, 
                                  void **handler, 
+                                 uint32_t nof_tx_antennas,
                                  uint32_t nof_rx_antennas);
 
 SRSLTE_API char* rf_uhd_devname(void *h);
@@ -123,3 +124,13 @@ SRSLTE_API int  rf_uhd_send_timed(void *h,
                                   bool is_start_of_burst, 
                                   bool is_end_of_burst);
 
+SRSLTE_API int rf_uhd_send_timed_multi(void *h,
+                                       void *data[SRSLTE_MAX_PORTS],
+                                       int nsamples,
+                                       time_t secs,
+                                       double frac_secs,
+                                       bool has_time_spec,
+                                       bool blocking,
+                                       bool is_start_of_burst,
+                                       bool is_end_of_burst);
+
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 7201f96c2..3b1a83466 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -38,8 +38,8 @@
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
 
-static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1}; // Only TM1 and TM2 are currently supported 
-const uint32_t nof_ue_formats = 2; 
+static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT2A}; // Only TM1 and TM2 are currently supported
+const uint32_t nof_ue_formats = 3;
 
 static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
@@ -97,13 +97,16 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_multi(&q->pdsch, q->cell, nof_rx_antennas)) {
+    if (srslte_pdsch_init_rx_multi(&q->pdsch, q->cell, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
-    if (srslte_softbuffer_rx_init(&q->softbuffer, q->cell.nof_prb)) {
-      fprintf(stderr, "Error initiating soft buffer\n");
-      goto clean_exit;
+
+    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (srslte_softbuffer_rx_init(&q->softbuffers[i], q->cell.nof_prb)) {
+        fprintf(stderr, "Error initiating soft buffer\n");
+        goto clean_exit;
+      }
     }
     if (srslte_cfo_init(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
       fprintf(stderr, "Error initiating SFO correct\n");
@@ -154,7 +157,9 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     srslte_cfo_free(&q->sfo_correct);
-    srslte_softbuffer_rx_free(&q->softbuffer);
+    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      srslte_softbuffer_rx_free(&q->softbuffers[i]);
+    }
     for (int j=0;j<q->nof_rx_antennas;j++) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
@@ -188,7 +193,9 @@ void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
 }
 
 void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
-  srslte_softbuffer_rx_reset(&q->softbuffer);
+  for(int i = 0; i < SRSLTE_MAX_CODEWORDS; i++){
+    srslte_softbuffer_rx_reset(&q->softbuffers[i]);
+  }
   bzero(&q->pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
 }
 
@@ -204,12 +211,14 @@ void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
  *    - PDSCH decoding: Decode TB scrambling with RNTI given by srslte_ue_dl_set_rnti()
  */
 int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti) {
+  uint8_t *_data[SRSLTE_MAX_CODEWORDS];
   cf_t *_input[SRSLTE_MAX_PORTS]; 
+  _data[0] = data;
   _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, data, tti, q->current_rnti);
+  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, q->current_rnti);
 }
 
-int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data, uint32_t tti) {
+int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti) {
   return srslte_ue_dl_decode_rnti_multi(q, input, data, tti, q->current_rnti);
 }
 
@@ -275,17 +284,24 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
 {
-  return srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx);
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, 0);
 }
 
-int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti) 
+int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2)
 {
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, rvidx2);
+}
+
+int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti)
+{
+  uint8_t *_data[SRSLTE_MAX_CODEWORDS];
   cf_t *_input[SRSLTE_MAX_PORTS];
   _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, data, tti, rnti);
+  _data[0] = data;
+  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, rnti);
 }
 
-int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data, uint32_t tti, uint16_t rnti) 
+int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti, uint16_t rnti)
 {
   srslte_dci_msg_t dci_msg;
   srslte_ra_dl_dci_t dci_unpacked;
@@ -320,17 +336,26 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
 
     
     uint32_t rvidx = 0; 
+    uint32_t rvidx2 = 0;
     if (dci_unpacked.rv_idx < 0) {
       uint32_t sfn = tti/10; 
       uint32_t k   = (sfn/2)%4; 
       rvidx        = ((uint32_t) ceilf((float)1.5*k))%4;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffer, grant.mcs.tbs);      
+      srslte_softbuffer_rx_reset_tbs(&q->softbuffers[0], (uint32_t) grant.mcs.tbs);
+      if (grant.nof_tb > 1) {
+        rvidx2     = ((uint32_t) ceilf((float)1.5*k))%4;
+        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[1], (uint32_t) grant.mcs2.tbs);
+      }
     } else {
-      rvidx = dci_unpacked.rv_idx;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffer, grant.mcs.tbs);      
+      rvidx = (uint32_t) dci_unpacked.rv_idx;
+      srslte_softbuffer_rx_reset_tbs(&q->softbuffers[0], (uint32_t) grant.mcs.tbs);
+      if (grant.nof_tb > 1) {
+        rvidx2 = (uint32_t) dci_unpacked.rv_idx_1;
+        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[1], (uint32_t) grant.mcs2.tbs);
+      }
     }
 
-    if (srslte_ue_dl_cfg_grant(q, &grant, cfi, sf_idx, rvidx)) {
+    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, rvidx2)) {
       return SRSLTE_ERROR; 
     }
     
@@ -340,7 +365,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   
     
     if (q->pdsch_cfg.grant.mcs.mod > 0 && q->pdsch_cfg.grant.mcs.tbs >= 0) {
-      ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, &q->softbuffer, 
+      ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
                                     q->sf_symbols_m, q->ce_m, 
                                     noise_estimate, 
                                     rnti, data);
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index b5b2be3d0..e011789ca 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -53,6 +53,11 @@ cf_t dummy_buffer1[15*2048/2];
 cf_t *dummy_offset_buffer[SRSLTE_MAX_PORTS] = {dummy_buffer0, dummy_buffer1};
 
 int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset_time, float offset_freq) {
+  return srslte_ue_sync_init_file_multi(q, nof_prb, file_name, offset_time, offset_freq, 1);
+}
+
+int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset_time,
+                                   float offset_freq, uint32_t nof_rx_ant) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q                   != NULL && 
@@ -66,6 +71,7 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     q->file_cfo = -offset_freq; 
     q->correct_cfo = true; 
     q->fft_size = srslte_symbol_sz(nof_prb);
+    q->nof_rx_antennas = nof_rx_ant;
     
     if (srslte_cfo_init(&q->file_cfo_correct, 2*q->sf_len)) {
       fprintf(stderr, "Error initiating CFO\n");
@@ -80,12 +86,12 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     INFO("Offseting input file by %d samples and %.1f kHz\n", offset_time, offset_freq/1000);
 
     if (offset_time) {
-      cf_t *file_offset_buffer = srslte_vec_malloc(offset_time * sizeof(cf_t));
+      cf_t *file_offset_buffer = srslte_vec_malloc(offset_time * nof_rx_ant * sizeof(cf_t));
       if (!file_offset_buffer) {
         perror("malloc");
         goto clean_exit;
       }
-      srslte_filesource_read(&q->file_source, file_offset_buffer, offset_time);
+      srslte_filesource_read(&q->file_source, file_offset_buffer, offset_time * nof_rx_ant);
       free(file_offset_buffer);
     }
 
@@ -518,26 +524,27 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
   {
     
     if (q->file_mode) {
-      int n = srslte_filesource_read(&q->file_source, input_buffer[0], q->sf_len);
+      int n = srslte_filesource_read_multi(&q->file_source, (void **) input_buffer, q->sf_len, q->nof_rx_antennas);
       if (n < 0) {
         fprintf(stderr, "Error reading input file\n");
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
       if (n == 0) {
         srslte_filesource_seek(&q->file_source, 0);
-        q->sf_idx = 9; 
-        int n = srslte_filesource_read(&q->file_source, input_buffer[0], q->sf_len);
+        q->sf_idx = 9;
+        n = srslte_filesource_read_multi(&q->file_source, (void **) input_buffer, q->sf_len, q->nof_rx_antennas);
         if (n < 0) {
           fprintf(stderr, "Error reading input file\n");
-          return SRSLTE_ERROR; 
+          return SRSLTE_ERROR;
         }
       }
       if (q->correct_cfo) {
-        srslte_cfo_correct(&q->file_cfo_correct, 
-                    input_buffer[0], 
-                    input_buffer[0], 
-                    q->file_cfo / 15000 / q->fft_size);               
-                    
+        for (int i = 0; i < q->nof_rx_antennas; i++) {
+          srslte_cfo_correct(&q->file_cfo_correct,
+                             input_buffer[i],
+                             input_buffer[i],
+                             q->file_cfo / 15000 / q->fft_size);
+        }
       }
       q->sf_idx++;
       if (q->sf_idx == 10) {
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 669422981..8cdec22c2 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -78,6 +78,7 @@ private:
   bool decode_pdcch_dl(mac_interface_phy::mac_grant_t *grant);
   bool decode_phich(bool *ack); 
   bool decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload, srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t pid);
+  bool decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS], srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS], int rv, uint16_t rnti, uint32_t pid);
 
   /* ... for UL */
   void encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, srslte_softbuffer_tx_t *softbuffer, 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index c2796e2ad..ef3640a40 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -405,8 +405,14 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
   }
 }
 
-bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload, 
+bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
                                srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t harq_pid)
+{
+  return decode_pdsch_multi(grant, &payload, softbuffer, rv, rnti, harq_pid);
+}
+
+bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                               srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS], int rv, uint16_t rnti, uint32_t harq_pid)
 {
   char timestr[64];
   timestr[0]='\0';
@@ -435,7 +441,7 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
         gettimeofday(&t[1], NULL);
   #endif
         
-        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffer, ue_dl.sf_symbols_m, 
+        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
                                       ue_dl.ce_m, noise_estimate, rnti, payload) == 0;
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);

From 19bc98081a1c505679e3626f807985d023e0a33d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 25 Jul 2017 15:35:25 +0200
Subject: [PATCH 061/269] PDSCH decoder multi has been modified for future
 parallel decoding of codewords.

---
 lib/include/srslte/phy/phch/sch.h |  9 ++++---
 lib/src/phy/phch/pdsch.c          | 45 ++++++++++++++++---------------
 lib/src/phy/phch/sch.c            | 19 +++++++------
 3 files changed, 38 insertions(+), 35 deletions(-)

diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index 0e59ecbc9..128cfea6f 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -108,11 +108,12 @@ SRSLTE_API int srslte_dlsch_decode(srslte_sch_t *q,
                                    int16_t *e_bits, 
                                    uint8_t *data);
 
-SRSLTE_API int srslte_dlsch_decode_multi(srslte_sch_t *q,
+SRSLTE_API int srslte_dlsch_decode2(srslte_sch_t *q,
                                    srslte_pdsch_cfg_t *cfg,
-                                   srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                                   int16_t *e_bits[SRSLTE_MAX_CODEWORDS],
-                                   uint8_t *data[SRSLTE_MAX_CODEWORDS]);
+                                   srslte_softbuffer_rx_t *softbuffer,
+                                   int16_t *e_bits,
+                                   uint8_t *data,
+                                   int codeword_idx);
 
 SRSLTE_API int srslte_ulsch_encode(srslte_sch_t *q, 
                                    srslte_pusch_cfg_t *cfg,
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 3d24cf175..5d85a3ec2 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -33,6 +33,8 @@
 #include <assert.h>
 #include <math.h>
 #include <srslte/phy/phch/pdsch.h>
+#include <srslte/phy/phch/ra.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
@@ -488,7 +490,8 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
   /* Set pointers for layermapping & precoding */
   uint32_t i, n;
   cf_t *x[SRSLTE_MAX_LAYERS];
-  
+  int ret = 0;
+
   if (q            != NULL &&
       sf_symbols   != NULL &&
       data         != NULL && 
@@ -564,26 +567,14 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     * 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
     */
-    if (cfg->nbits.nof_re) {
+    if (cfg->nbits.nof_bits) {
+      INFO("Decoding CW 0 (%d bits)\n", cfg->nbits.nof_bits);
       srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
-    }
-
-    if (cfg->nbits2.nof_re) {
-      srslte_demod_soft_demodulate_s(cfg->grant.mcs2.mod, q->d2, q->e2, cfg->nbits2.nof_re);
-    }
 
-    /* descramble */
-    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      if (cfg->nbits.nof_bits) {
+      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
         srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
-      }
-
-      if (cfg->nbits2.nof_bits) {
-        srslte_scrambling_s_offset(&q->users[rnti]->seq2[cfg->sf_idx], q->e2, 0, cfg->nbits2.nof_bits);
-      }
-    } else {
-      srslte_sequence_t seq;
-      if (cfg->nbits.nof_bits) {
+      } else {
+        srslte_sequence_t seq;
         if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
           return SRSLTE_ERROR;
         }
@@ -591,22 +582,34 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
         srslte_sequence_free(&seq);
       }
 
-      if (cfg->nbits2.nof_bits) {
+      ret |= srslte_dlsch_decode2(&q->dl_sch, cfg, &softbuffers[0], q->e, data[0], 0);
+    }
+
+    if (cfg->nbits2.nof_bits) {
+      INFO("Decoding CW 1 (%d bits)\n", cfg->nbits2.nof_bits);
+      srslte_demod_soft_demodulate_s(cfg->grant.mcs2.mod, q->d2, q->e2, cfg->nbits2.nof_re);
+
+      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
+        srslte_scrambling_s_offset(&q->users[rnti]->seq2[cfg->sf_idx], q->e2, 0, cfg->nbits2.nof_bits);
+      } else {
+        srslte_sequence_t seq;
         if (srslte_sequence_pdsch(&seq, rnti, 1, 2 * cfg->sf_idx, q->cell.id, cfg->nbits2.nof_bits)) {
           return SRSLTE_ERROR;
         }
         srslte_scrambling_s_offset(&seq, q->e2, 0, cfg->nbits2.nof_bits);
         srslte_sequence_free(&seq);
       }
+
+      ret |= srslte_dlsch_decode2(&q->dl_sch, cfg, &softbuffers[1], q->e2, data[1], 1);
     }
 
+
     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_multi(&q->dl_sch, cfg, softbuffers, (int16_t *[SRSLTE_MAX_CODEWORDS]) {q->e, q->e2},
-                                     data);
+    return ret;
     
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index f0515227e..1caacacc9 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -524,29 +524,28 @@ int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
 }
 
 
-int srslte_dlsch_decode_multi(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                        int16_t *e_bits[SRSLTE_MAX_CODEWORDS], uint8_t *data[SRSLTE_MAX_CODEWORDS])
+int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                        int16_t *e_bits, uint8_t *data, int codeword_idx)
 {
-  int ret  = SRSLTE_SUCCESS;
   uint32_t Nl = 1;
 
   if (cfg->nof_layers != cfg->grant.nof_tb) {
     Nl = 2;
   }
 
-  if (cfg->nbits.nof_bits) {
-    ret |= decode_tb(q, &softbuffers[0], &cfg->cb_segm,
+  if (codeword_idx == 0) {
+    return decode_tb(q, softbuffer, &cfg->cb_segm,
                      cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
-                     e_bits[0], data[0]);
+                     e_bits, data);
   }
 
-  if (cfg->nbits2.nof_bits) {
-    ret |= decode_tb(q, &softbuffers[1], &cfg->cb_segm2,
+  if (codeword_idx == 1) {
+    return decode_tb(q, softbuffer, &cfg->cb_segm2,
                      cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
-                     e_bits[1], data[1]);
+                     e_bits, data);
   }
 
-  return ret;
+  return SRSLTE_ERROR;
 }
 
 /**

From 6a45147f4598b6d2b37a2a196d40c03a27c92cd0 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 26 Jul 2017 11:31:02 +0200
Subject: [PATCH 062/269] Added different data sources and sinks

---
 lib/examples/pdsch_enodeb.c | 48 ++++++++++++++++++++++++++++---------
 lib/examples/pdsch_ue.c     | 22 ++++++++++++-----
 2 files changed, 53 insertions(+), 17 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 01e06c4c3..b1304958c 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -99,6 +99,9 @@ srslte_netsink_t net_sink;
 int prbset_num = 1, last_prbset_num = 1; 
 int prbset_orig = 0; 
 
+#define DATA_BUFF_SZ    1024*1024
+uint8_t *data[2], data2[DATA_BUFF_SZ];
+uint8_t data_tmp[DATA_BUFF_SZ];
 
 void usage(char *prog) {
   printf("Usage: %s [agmfoncvpuM]\n", prog);
@@ -204,6 +207,16 @@ void base_init() {
       exit(-1);
   }
 
+  /* Allocate memory */
+  for(i = 0; i < nof_tb; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t) * SOFTBUFFER_SIZE);
+    if (!data[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+    bzero(data[i], sizeof(uint8_t) * SOFTBUFFER_SIZE);
+  }
+
   /* init memory */
   for (i = 0; i < cell.nof_ports; i++) {
     sf_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
@@ -322,6 +335,12 @@ void base_free() {
 
   srslte_ofdm_tx_free(&ifft);
 
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
+  }
+
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (sf_buffer[i]) {
       free(sf_buffer[i]);
@@ -468,10 +487,6 @@ int update_control() {
   }
 }
 
-#define DATA_BUFF_SZ    1024*128
-uint8_t data[8*DATA_BUFF_SZ], data2[DATA_BUFF_SZ];
-uint8_t data_tmp[DATA_BUFF_SZ];
-
 /** Function run in a separate thread to receive UDP data */
 void *net_thread_fnc(void *arg) {
   int n; 
@@ -480,14 +495,16 @@ void *net_thread_fnc(void *arg) {
   do {
     n = srslte_netsource_read(&net_source, &data2[rpm], DATA_BUFF_SZ-rpm);
     if (n > 0) {
-      int nbytes = 1+(pdsch_cfg.grant.mcs.tbs-1)/8;
+      // FIXME: I assume that both transport blocks have same size in case of 2 tb are active
+      int nbytes = 1 + (pdsch_cfg.grant.mcs.tbs + pdsch_cfg.grant.mcs2.tbs - 1) / 8;
       rpm += n; 
       INFO("received %d bytes. rpm=%d/%d\n",n,rpm,nbytes);
       wpm = 0; 
       while (rpm >= nbytes) {
         // wait for packet to be transmitted
         sem_wait(&net_sem);
-        memcpy(data, &data2[wpm], nbytes);          
+        memcpy(data[0], &data2[wpm], nbytes / (size_t) 2);
+        memcpy(data[1], &data2[wpm], nbytes / (size_t) 2);
         INFO("Sent %d/%d bytes ready\n", nbytes, rpm);
         rpm -= nbytes;          
         wpm += nbytes; 
@@ -663,8 +680,11 @@ int main(int argc, char **argv) {
         }
       } else {
         INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs.tbs);
-        for (i=0;i<pdsch_cfg.grant.mcs.tbs/8;i++) {
-          data[i] = rand()%256;
+        for (i = 0; i < pdsch_cfg.grant.mcs.tbs / 8; i++) {
+          data[0][i] = rand() % 256;
+        }
+        for (i = 0; i < pdsch_cfg.grant.mcs2.tbs / 8; i++) {
+          data[1][i] = rand() % 256;
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
         if (sf_idx != 0 && sf_idx != 5) {
@@ -706,17 +726,23 @@ int main(int argc, char **argv) {
         }
        
         /* Encode PDSCH */
-        if (srslte_pdsch_encode_multi(&pdsch, &pdsch_cfg, softbuffers, (uint8_t*[2]){data, data}, UE_CRNTI,
+        if (srslte_pdsch_encode_multi(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI,
                                       sf_symbols)) {
           fprintf(stderr, "Error encoding PDSCH\n");
           exit(-1);
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            srslte_bit_pack_vector(data, data_tmp, pdsch_cfg.grant.mcs.tbs);
+            srslte_bit_pack_vector(data[0], data_tmp, pdsch_cfg.grant.mcs.tbs);
             if (srslte_netsink_write(&net_sink, data_tmp, 1+(pdsch_cfg.grant.mcs.tbs-1)/8) < 0) {
               fprintf(stderr, "Error sending data through UDP socket\n");
-            }            
+            }
+            if (nof_tb > 1) {
+              srslte_bit_pack_vector(data[1], data_tmp, pdsch_cfg.grant.mcs2.tbs);
+              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs2.tbs - 1) / 8) < 0) {
+                fprintf(stderr, "Error sending data through UDP socket\n");
+              }
+            }
           }
           net_packet_ready = false; 
           sem_post(&net_sem);
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index bab9d7219..80f960508 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -36,6 +36,9 @@
 #include <signal.h>
 #include <pthread.h>
 #include <semaphore.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/phch/ra.h>
 
 #include "srslte/srslte.h"
 
@@ -508,7 +511,7 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0;
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0;
   bool decode_pdsch = false; 
 
 #ifndef DISABLE_RF
@@ -584,7 +587,9 @@ int main(int argc, char **argv) {
               
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
-                srslte_netsink_write(&net_sink, data, 1+(n-1)/8);
+                // FIXME: UDP Data transmission does not work
+                srslte_netsink_write(&net_sink, data[0], 1 + (ue_dl.pdsch_cfg.grant.mcs.tbs - 1) / 8);
+                srslte_netsink_write(&net_sink, data[1], 1 + (ue_dl.pdsch_cfg.grant.mcs2.tbs - 1) / 8);
               }
               
               #ifdef PRINT_CHANGE_SCHEDULIGN
@@ -609,6 +614,8 @@ int main(int argc, char **argv) {
             rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05);
             rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05);
             noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);
+            enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0, enodebrate, 0.05);
+            uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0:0.0, uerate, 0.01);
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -640,14 +647,17 @@ int main(int argc, char **argv) {
                      100 * (1 - (float) ue_dl.nof_detected / nof_trials),
                      (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
             } else {
-              printf("CFO: %+6.2f kHz, "
-                         "SNR port 0: %4.1f dB, "
-                         "SNR port 1: %4.1f dB, "
-                         "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
+              printf("CFO: %+5.2f kHz, "
+                         "SNR: %+5.1f dB | %+5.1f dB, "
+                         "Rb: %6.2f / %6.2f Mbps, "
+                         "PDCCH-Miss: %5.2f%%, "
+                         "PDSCH-BLER: %5.2f%%\r",
 
                      srslte_ue_sync_get_cfo(&ue_sync) / 1000,
                      10 * log10(rsrp0 / noise),
                      10 * log10(rsrp1 / noise),
+                     uerate,
+                     enodebrate,
                      100 * (1 - (float) ue_dl.nof_detected / nof_trials),
                      (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
             }

From 1d00e1acaa2168888f3ae250f3c925523b82590f Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 3 Aug 2017 13:56:57 +0200
Subject: [PATCH 063/269] Merge with next_with_matlab

---
 lib/examples/pdsch_enodeb.c                |  36 +-
 lib/examples/pdsch_ue.c                    |  21 +-
 lib/include/srslte/phy/common/phy_common.h |   2 +
 lib/include/srslte/phy/mimo/precoding.h    |  13 +-
 lib/include/srslte/phy/phch/pdsch.h        |   4 +-
 lib/include/srslte/phy/phch/pdsch_cfg.h    |   1 +
 lib/include/srslte/phy/phch/ra.h           |   7 +
 lib/include/srslte/phy/rf/rf.h             |   5 +
 lib/include/srslte/phy/ue/ue_dl.h          |   4 +-
 lib/include/srslte/phy/utils/algebra.h     |  70 ++
 lib/src/phy/common/phy_common.c            |   2 +-
 lib/src/phy/mimo/precoding.c               | 710 +++++++++++++++++++--
 lib/src/phy/mimo/test/CMakeLists.txt       |  11 +-
 lib/src/phy/mimo/test/precoder_test.c      |  19 +-
 lib/src/phy/phch/pdsch.c                   | 139 +++-
 lib/src/phy/phch/test/CMakeLists.txt       |  95 ++-
 lib/src/phy/phch/test/pdsch_test.c         |  92 +--
 lib/src/phy/phch/test/pdsch_test_mex.c     |  19 +-
 lib/src/phy/ue/ue_dl.c                     |  78 ++-
 lib/src/phy/utils/algebra.c                |  72 +++
 20 files changed, 1240 insertions(+), 160 deletions(-)
 create mode 100644 lib/include/srslte/phy/utils/algebra.h
 create mode 100644 lib/src/phy/utils/algebra.c

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index b1304958c..1275360a4 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -70,6 +70,8 @@ uint32_t mcs_idx = 1, last_mcs_idx = 1;
 int nof_frames = -1;
 char mimo_type_str[32] = "single";
 uint32_t nof_tb = 1;
+uint32_t multiplex_pmi = 0;
+uint32_t multiplex_nof_layers = 1;
 
 char *rf_args = "";
 float rf_amp = 0.8, rf_gain = 70.0, rf_freq = 2400000000;
@@ -104,7 +106,7 @@ uint8_t *data[2], data2[DATA_BUFF_SZ];
 uint8_t data_tmp[DATA_BUFF_SZ];
 
 void usage(char *prog) {
-  printf("Usage: %s [agmfoncvpuM]\n", prog);
+  printf("Usage: %s [agmfoncvpuxb]\n", prog);
 #ifndef DISABLE_RF
   printf("\t-a RF args [Default %s]\n", rf_args);
   printf("\t-l RF amplitude [Default %.2f]\n", rf_amp);
@@ -118,14 +120,18 @@ void usage(char *prog) {
   printf("\t-n number of frames [Default %d]\n", nof_frames);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-p nof_prb [Default %d]\n", cell.nof_prb);
-  printf("\t-M Transmission mode[single|diversity|cdd] [Default %s]\n", mimo_type_str);
+  printf("\t-x Transmission mode[single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
+  printf("\t-b Precoding Matrix Index (multiplex mode only)* [Default %d]\n", multiplex_pmi);
+  printf("\t-w Number of codewords/layers (multiplex mode only)* [Default %d]\n", multiplex_nof_layers);
   printf("\t-u listen TCP port for input data (-1 is random) [Default %d]\n", net_port);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
+  printf("\n");
+  printf("\t*: See 3GPP 36.212 Table  5.3.3.1.5-4 for more information\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "aglfmoncpvutM")) != -1) {
+  while ((opt = getopt(argc, argv, "aglfmoncpvutxbw")) != -1) {
     switch (opt) {
     case 'a':
       rf_args = argv[optind];
@@ -157,9 +163,15 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
-    case 'M':
+    case 'x':
       strncpy(mimo_type_str, argv[optind], 32);
       break;
+    case 'b':
+      multiplex_pmi = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'w':
+      multiplex_nof_layers = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -202,6 +214,11 @@ void base_init() {
       pdsch_cfg.nof_layers = 2;
       nof_tb = 2;
       break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      cell.nof_ports = 2;
+      pdsch_cfg.nof_layers = multiplex_nof_layers;
+      nof_tb = multiplex_nof_layers;
+      break;
     default:
       ERROR("Transmission mode not implemented.");
       exit(-1);
@@ -695,7 +712,7 @@ int main(int argc, char **argv) {
       }        
       
       if (send_data) {
-        srslte_dci_format_t dci_format = SRSLTE_DCI_FORMAT1;
+        srslte_dci_format_t dci_format;
         switch(pdsch_cfg.mimo_type) {
           case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
             dci_format = SRSLTE_DCI_FORMAT1;
@@ -705,6 +722,13 @@ int main(int argc, char **argv) {
             dci_format = SRSLTE_DCI_FORMAT2A;
             break;
           case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+            dci_format = SRSLTE_DCI_FORMAT2;
+            if (multiplex_nof_layers == 1) {
+              ra_dl.pinfo = (uint8_t) (multiplex_pmi + 1);
+            } else {
+              ra_dl.pinfo = (uint8_t) multiplex_pmi;
+            }
+            break;
           default:
             fprintf(stderr, "Wrong MIMO configuration\n");
             exit(SRSLTE_ERROR);
@@ -720,7 +744,7 @@ int main(int argc, char **argv) {
         /* Configure pdsch_cfg parameters */
         srslte_ra_dl_grant_t grant; 
         srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0, 0)) {
+        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0, 0, pdsch_cfg.mimo_type, multiplex_pmi)) {
           fprintf(stderr, "Error configuring PDSCH\n");
           exit(-1);
         }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 80f960508..1c6059d9f 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -511,7 +511,8 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0;
+  uint32_t ri = 0, pmi = 0;
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0, sinr = 0.0;
   bool decode_pdsch = false; 
 
 #ifndef DISABLE_RF
@@ -616,6 +617,16 @@ int main(int argc, char **argv) {
             noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);
             enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0, enodebrate, 0.05);
             uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0:0.0, uerate, 0.01);
+
+            if (ue_dl.cell.nof_ports == 2 && ue_dl.pdsch.nof_rx_antennas == 2) {
+              float _sinr;
+              srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, &_sinr);
+
+              if (!isinff(_sinr) && !isnanf(_sinr)) {
+                sinr = SRSLTE_VEC_EMA(_sinr, sinr, 0.05f);
+              }
+            }
+
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -651,7 +662,8 @@ int main(int argc, char **argv) {
                          "SNR: %+5.1f dB | %+5.1f dB, "
                          "Rb: %6.2f / %6.2f Mbps, "
                          "PDCCH-Miss: %5.2f%%, "
-                         "PDSCH-BLER: %5.2f%%\r",
+                         "PDSCH-BLER: %5.2f%%, "
+                         "SINR: %3.1f dB RI: %d PMI: %d    \r",
 
                      srslte_ue_sync_get_cfo(&ue_sync) / 1000,
                      10 * log10(rsrp0 / noise),
@@ -659,7 +671,10 @@ int main(int argc, char **argv) {
                      uerate,
                      enodebrate,
                      100 * (1 - (float) ue_dl.nof_detected / nof_trials),
-                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total,
+                     10 * log10(sinr),
+                     ri,
+                     pmi);
             }
           }
           break;
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index b891b7330..e1c6f1b0d 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -55,6 +55,8 @@
 
 #define SRSLTE_MAX_CODEBLOCKS 32
 
+#define SRSLTE_MAX_CODEBOOKS 4
+
 #define SRSLTE_LTE_CRC24A  0x1864CFB
 #define SRSLTE_LTE_CRC24B  0X1800063
 #define SRSLTE_LTE_CRC16   0x11021
diff --git a/lib/include/srslte/phy/mimo/precoding.h b/lib/include/srslte/phy/mimo/precoding.h
index 62fad08ef..222c085b6 100644
--- a/lib/include/srslte/phy/mimo/precoding.h
+++ b/lib/include/srslte/phy/mimo/precoding.h
@@ -65,8 +65,9 @@ SRSLTE_API int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS],
 SRSLTE_API int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], 
                                      cf_t *y[SRSLTE_MAX_PORTS], 
                                      int nof_layers,
-                                     int nof_ports, 
-                                     int nof_symbols, 
+                                     int nof_ports,
+                                     int codebook_idx,
+                                     int nof_symbols,
                                      srslte_mimo_type_t type);
 
 /* Estimates the vector "x" based on the received signal "y" and the channel estimates "h"
@@ -112,8 +113,16 @@ SRSLTE_API int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS],
                                              int nof_rxant,
                                              int nof_ports, 
                                              int nof_layers, 
+                                             int codebook_idx,
                                              int nof_symbols, 
                                              srslte_mimo_type_t type, 
                                              float noise_estimate);
 
+int srslte_precoding_pmi_select (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                 uint32_t nof_symbols,
+                                 float noise_estimate,
+                                 int nof_layers,
+                                 uint32_t *pmi,
+                                 float sinr[SRSLTE_MAX_CODEBOOKS]);
+
 #endif /* PRECODING_H_ */
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7bdaaef9c..fc250a7d5 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -115,7 +115,9 @@ SRSLTE_API int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg,
                                       uint32_t cfi,
                                       uint32_t sf_idx,
                                       uint32_t rvidx,
-                                      uint32_t rvidx2);
+                                      uint32_t rvidx2,
+                                      srslte_mimo_type_t mimo_type,
+                                      uint32_t pmi);
 
 SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
                                    srslte_pdsch_cfg_t *cfg,
diff --git a/lib/include/srslte/phy/phch/pdsch_cfg.h b/lib/include/srslte/phy/phch/pdsch_cfg.h
index bec326338..9664d826d 100644
--- a/lib/include/srslte/phy/phch/pdsch_cfg.h
+++ b/lib/include/srslte/phy/phch/pdsch_cfg.h
@@ -49,6 +49,7 @@ typedef struct SRSLTE_API {
   uint32_t rv2;
   uint32_t sf_idx;  
   uint32_t nof_layers;
+  uint32_t codebook_idx;
   srslte_mimo_type_t mimo_type;
 } srslte_pdsch_cfg_t;
 
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 3c7c3d93c..5fab0f10b 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -216,6 +216,13 @@ SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
                                                   srslte_ra_nbits_t *nbits,
                                                   srslte_ra_nbits_t *nbits2);
 
+SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
+                                                  uint32_t cfi,
+                                                  srslte_cell_t cell,
+                                                  uint32_t sf_idx,
+                                                  srslte_ra_nbits_t *nbits,
+                                                  srslte_ra_nbits_t *nbits2);
+
 SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, 
                                                uint32_t nof_prb, 
                                                uint32_t nof_ctrl_symbols); 
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index d37750749..bc74d17e7 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -80,6 +80,11 @@ SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
                                      uint32_t nof_tx_antennas,
                                      uint32_t nof_rx_antennas);
 
+SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
+                                     char *args,
+                                     uint32_t nof_tx_antennas,
+                                     uint32_t nof_rx_antennas);
+
 SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h, 
                                       char *devname, 
                                       char *args);
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 408ba0c65..0c77ba911 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -147,7 +147,9 @@ SRSLTE_API int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q,
                                             uint32_t cfi,
                                             uint32_t sf_idx,
                                             uint32_t rvidx,
-                                            uint32_t rvidx2);
+                                            uint32_t rvidx2,
+                                            srslte_mimo_type_t mimo_type,
+                                            uint32_t pinfo);
 
 SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, 
                                         uint32_t cfi, 
diff --git a/lib/include/srslte/phy/utils/algebra.h b/lib/include/srslte/phy/utils/algebra.h
new file mode 100644
index 000000000..41d5e0025
--- /dev/null
+++ b/lib/include/srslte/phy/utils/algebra.h
@@ -0,0 +1,70 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef SRSLTE_ALGEBRA_H
+#define SRSLTE_ALGEBRA_H
+
+#include "srslte/config.h"
+
+#ifdef LV_HAVE_SSE
+
+#define _MM_MULJ_PS(X) _mm_permute_ps(_MM_CONJ_PS(X), 0b10110001)
+#define _MM_CONJ_PS(X) (_mm_xor_ps(X, (__m128){0.0f, -0.0f, 0.0f, -0.0f}))
+#define _MM_PROD_PS(a, b) _mm_addsub_ps(_mm_mul_ps(a,_mm_moveldup_ps(b)),_mm_mul_ps(\
+                            _mm_shuffle_ps(a,a,0xB1),_mm_movehdup_ps(b)))
+
+SRSLTE_API void srslte_algebra_2x2_zf_sse(__m128 y0,
+                                          __m128 y1,
+                                          __m128 h00,
+                                          __m128 h01,
+                                          __m128 h10,
+                                          __m128 h11,
+                                          __m128 *x0,
+                                          __m128 *x1,
+                                          float norm);
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+#define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
+#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, (__m256){0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f}))
+#define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+
+SRSLTE_API void srslte_algebra_2x2_zf_avx(__m256 y0,
+                                          __m256 y1,
+                                          __m256 h00,
+                                          __m256 h01,
+                                          __m256 h10,
+                                          __m256 h11,
+                                          __m256 *x0,
+                                          __m256 *x1,
+                                          float norm);
+
+#endif /* LV_HAVE_AVX */
+
+#endif //SRSLTE_ALGEBRA_H
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 69e39b49a..7be57c63a 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -436,7 +436,7 @@ int srslte_str2mimotype(char *mimo_type_str, srslte_mimo_type_t *type) {
     *type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
   } else if (!strcmp(mimo_type_str, "diversity") || !strcmp(mimo_type_str, "TxDiversity")) {
     *type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-  } else if (!strcmp(mimo_type_str, "multiplex")) {
+  } else if (!strcmp(mimo_type_str, "multiplex") || !strcmp(mimo_type_str, "SpatialMux")) {
     *type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
   } else if (!strcmp(mimo_type_str, "cdd") || !strcmp(mimo_type_str, "CDD")) {
     *type = SRSLTE_MIMO_TYPE_CDD;
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index f93c63479..ba950f196 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -33,6 +33,7 @@
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/precoding.h"
 #include "srslte/phy/utils/vector.h"
+#include "srslte/phy/utils/debug.h"
 
 #ifdef LV_HAVE_SSE
 #include <xmmintrin.h>
@@ -43,6 +44,9 @@ int srslte_predecoding_diversity2_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_
 
 #ifdef LV_HAVE_AVX
 #include <immintrin.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/utils/algebra.h>
+
 int srslte_predecoding_single_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 #endif
 
@@ -527,7 +531,7 @@ int srslte_predecoding_type(cf_t *y_, cf_t *h_[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE
     h[i][0] = h_[i];
   }
   y[0] = y_; 
-  return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, type, noise_estimate);  
+  return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, 0, nof_symbols, type, noise_estimate);
 }
 
 
@@ -565,11 +569,46 @@ int srslte_precoding_mimo_2x2_gen(cf_t W[2][2], cf_t *y[SRSLTE_MAX_PORTS], cf_t
   return SRSLTE_SUCCESS; 
 }
 
+// SSE implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_AVX
 
+int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols) {
+  uint32_t i = 0;
 
+  for (i = 0; i < nof_symbols; i += 4) {
+    /* Load channel */
+    __m256 h00i = _mm256_load_ps((float *) &h[0][0][i]);
+    __m256 h01i = _mm256_load_ps((float *) &h[0][1][i]);
+    __m256 h10i = _mm256_load_ps((float *) &h[1][0][i]);
+    __m256 h11i = _mm256_load_ps((float *) &h[1][1][i]);
 
+    /* Apply precoding */
+    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i,
+                                                   (__m256) {+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f}));
+    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i,
+                                                   (__m256) {+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f}));
+    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i,
+                                                   (__m256) {-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f}));
+    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i,
+                                                   (__m256) {-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f}));
 
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
 
+    __m256 x0, x1;
+
+    srslte_algebra_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif /* LV_HAVE_AVX */
 
 // SSE implementation of ZF 2x2 CCD equalizer
 #ifdef LV_HAVE_SSE
@@ -580,9 +619,6 @@ int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
                                       uint32_t nof_symbols) {
   uint32_t i = 0;
 
-  /* Conjugate mask */
-  __m128 conj_mask = (__m128) {+0.0f, -0.0f, +0.0f, -0.0f};
-
   for (i = 0; i < nof_symbols; i += 2) {
     /* Load channel */
     __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
@@ -596,21 +632,12 @@ int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
     __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
     __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
 
-    __m128 detmult1 = PROD(h00, h11);
-    __m128 detmult2 = PROD(h01, h10);
-
-    __m128 det = _mm_sub_ps(detmult1, detmult2);
-    __m128 detconj = _mm_xor_ps(det, conj_mask);
-    __m128 detabs2 = PROD(det, detconj);
-    __m128 detabs2rec = _mm_rcp_ps(detabs2);
-    detabs2rec = _mm_shuffle_ps(detabs2rec, detabs2rec, _MM_SHUFFLE(2, 2, 0, 0));
-    __m128 detrec = _mm_mul_ps(_mm_mul_ps(detconj, detabs2rec), (__m128) {2.0f, 2.0f, 2.0f, 2.0f});
-
     __m128 y0 = _mm_load_ps((float *) &y[0][i]);
     __m128 y1 = _mm_load_ps((float *) &y[1][i]);
 
-    __m128 x0 = PROD(_mm_sub_ps(PROD(h11, y0), PROD(h01, y1)), detrec);
-    __m128 x1 = PROD(_mm_sub_ps(PROD(h00, y1), PROD(h10, y0)), detrec);
+    __m128 x0, x1;
+
+    srslte_algebra_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
 
     _mm_store_ps((float *) &x[0][i], x0);
     _mm_store_ps((float *) &x[1][i], x1);
@@ -659,11 +686,15 @@ int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORT
 {
   if (nof_ports == 2 && nof_rxant == 2) {
     if (nof_layers == 2) {
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_ccd_2x2_zf_avx(y, h, x, nof_symbols);
+#else
 #ifdef LV_HAVE_SSE
       return srslte_predecoding_ccd_2x2_zf_sse(y, h, x, nof_symbols);
 #else
       return srslte_predecoding_ccd_2x2_zf_gen(y, h, x, nof_symbols);
-#endif
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
     } else {
       fprintf(stderr, "Error predecoding CCD: Invalid number of layers %d\n", nof_layers);
       return -1;       
@@ -676,11 +707,535 @@ int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORT
   return SRSLTE_ERROR;
 }
 
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                    float noise_estimate, uint32_t *pmi,
+                                    float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+#define SQRT1_2 ((float)M_SQRT1_2);
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += 100) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a0, a1;
+      switch(i) {
+        case 0:
+          a0 = conjf(h00) + conjf(h01);
+          a1 = conjf(h10) + conjf(h11);
+          break;
+        case 1:
+          a0 = conjf(h00) - conjf(h01);
+          a1 = conjf(h10) - conjf(h11);
+          break;
+        case 2:
+          a0 = conjf(h00) - _Complex_I * conjf(h01);
+          a1 = conjf(h10) - _Complex_I * conjf(h11);
+          break;
+        case 3:
+          a0 = conjf(h00) + _Complex_I * conjf(h01);
+          a1 = conjf(h10) + _Complex_I * conjf(h11);
+          break;
+      }
+      a0 *= SQRT1_2;
+      a1 *= SQRT1_2;
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b0 = a0*h00 + a1*h10;
+      cf_t b1 = a0*h01 + a1*h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c;
+      switch(i) {
+        case 0:
+          c = b0 + b1;
+          break;
+        case 1:
+          c = b0 - b1;
+          break;
+        case 2:
+          c = b0 + _Complex_I*b1;
+          break;
+        case 3:
+          c = b0 - _Complex_I*b1;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c *= SQRT1_2;
+
+      /* Add for averaging */
+      sinr_list[i] += crealf(c);
+
+      count ++;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate*count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  INFO("Precoder PMI Select for 1 layer SINR=[%.1fdB; %.1fdB; %.1fdB; %.1fdB] PMI=%d\n", 10*log10(sinr_list[0]), 10*log10(sinr_list[1]),
+       10*log10(sinr_list[2]), 10*log10(sinr_list[3]), *pmi);
+
+  return i;
+}
+
+/* PMI Select for 2 layers */
+int srslte_precoding_pmi_select_2l (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                    float noise_estimate, uint32_t *pmi,
+                                    float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += 100) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a00, a01, a10, a11;
+      switch(i) {
+        case 0:
+          a00 = conjf(h00) + conjf(h01);
+          a01 = conjf(h10) + conjf(h11);
+          a10 = conjf(h00) - conjf(h01);
+          a11 = conjf(h10) - conjf(h11);
+          break;
+        case 1:
+          a00 = conjf(h00) - _Complex_I*conjf(h01);
+          a01 = conjf(h10) - _Complex_I*conjf(h11);
+          a10 = conjf(h00) + _Complex_I*conjf(h01);
+          a11 = conjf(h10) + _Complex_I*conjf(h11);
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      a00 *= 0.5f;
+      a01 *= 0.5f;
+      a10 *= 0.5f;
+      a11 *= 0.5f;
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b00 = a00*h00 + a01*h10;
+      cf_t b01 = a00*h01 + a01*h11;
+      cf_t b10 = a10*h00 + a11*h10;
+      cf_t b11 = a10*h01 + a11*h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c00, c01, c10, c11;
+      switch(i) {
+        case 0:
+          c00 = b00 + b01;
+          c01 = b00 - b01;
+          c10 = b10 + b11;
+          c11 = b10 - b11;
+          break;
+        case 1:
+          c00 = b00 + _Complex_I*b01;
+          c01 = b00 - _Complex_I*b01;
+          c10 = b10 + _Complex_I*b11;
+          c11 = b10 - _Complex_I*b11;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 *= 0.5;
+      c01 *= 0.5;
+      c10 *= 0.5;
+      c11 *= 0.5;
+
+      /* 4. C += noise * I */
+      c00 += noise_estimate;
+      c11 += noise_estimate;
+
+      /* 5. detC */
+      cf_t detC = c00*c11 - c01*c10;
+      cf_t inv_detC = conjf(detC)/(crealf(detC)*crealf(detC) + cimagf(detC)*cimagf(detC));
+
+      cf_t den0 = noise_estimate*c00*inv_detC;
+      cf_t den1 = noise_estimate*c11*inv_detC;
+
+      float gamma0 = crealf((conjf(den0)/(crealf(den0)*crealf(den0) + cimagf(den0)*cimagf(den0))) - 1);
+      float gamma1 = crealf((conjf(den1)/(crealf(den1)*crealf(den1) + cimagf(den1)*cimagf(den1))) - 1);
+
+      /* Add for averaging */
+      sinr_list[i] += (gamma0 + gamma1);
+
+      count ++;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= (2*count);
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  INFO("Precoder PMI Select for 2 layers SINR=[%.1fdB; %.1fdB] PMI=%d\n", 10*log10(sinr_list[0]), 10*log10(sinr_list[1]), *pmi);
+
+  return i;
+}
+
+int srslte_precoding_pmi_select (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                 float noise_estimate, int nof_layers, uint32_t *pmi,
+                                 float sinr[SRSLTE_MAX_CODEBOOKS]) {
+  int ret;
+
+  if (sinr == NULL || pmi == NULL) {
+    ERROR("Null pointer");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  } else if (nof_layers == 1) {
+    ret = srslte_precoding_pmi_select_1l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else if (nof_layers == 2) {
+    ret = srslte_precoding_pmi_select_2l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else {
+    ERROR("Wrong number of layers");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return ret;
+}
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  int i = 0;
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+#ifdef LV_HAVE_AVX
+  for (/* i = 0*/; i < nof_symbols; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm256_add_ps(_h00, _h10);
+        h01 = _mm256_sub_ps(_h00, _h10);
+        h10 = _mm256_add_ps(_h01, _h11);
+        h11 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h01 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h10 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
+        h11 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_algebra_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+
+  }
+  if (i > nof_symbols) {
+    i -= 4;
+  }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+  for (/* i = 0*/; i < nof_symbols; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm_add_ps(_h00, _h10);
+        h01 = _mm_sub_ps(_h00, _h10);
+        h10 = _mm_add_ps(_h01, _h11);
+        h11 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h01 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h10 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
+        h11 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_algebra_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+
+  }
+  if (i > nof_symbols) {
+    i -= 2;
+  }
+#endif /* LV_HAVE_SSE */
+
+  for (/*int i = 0*/; i < nof_symbols; i++) {
+    cf_t h00, h01, h10, h11, det;
+
+    switch(codebook_idx) {
+      case 0:
+        h00 = h[0][0][i];
+        h01 = h[1][0][i];
+        h10 = h[0][1][i];
+        h11 = h[1][1][i];
+        break;
+      case 1:
+        h00 = h[0][0][i] + h[1][0][i];
+        h01 = h[0][0][i] - h[1][0][i];
+        h10 = h[0][1][i] + h[1][1][i];
+        h11 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h00 = h[0][0][i] + _Complex_I*h[1][0][i];
+        h01 = h[0][0][i] - _Complex_I*h[1][0][i];
+        h10 = h[0][1][i] + _Complex_I*h[1][1][i];
+        h11 = h[0][1][i] - _Complex_I*h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * (norm / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+  }
+  return SRSLTE_SUCCESS;
+}
+
+// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  int i = 0;
+
+#ifdef LV_HAVE_AVX
+  for (/* i = 0*/; i < nof_symbols; i += 4) {
+      __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+      __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+      __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+      __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h0, h1;
+    switch (codebook_idx) {
+      case 0:
+        h0 = _mm256_add_ps(_h00, _h10);
+        h1 = _mm256_add_ps(_h01, _h11);
+        break;
+      case 1:
+        h0 = _mm256_sub_ps(_h00, _h10);
+        h1 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h0 = _mm256_add_ps(_h00, _mm256_permute_ps(_MM256_CONJ_PS(_h10), 0b10110001));
+        h1 = _mm256_add_ps(_h01, _mm256_permute_ps(_MM256_CONJ_PS(_h11), 0b10110001));
+        break;
+      case 3:
+        h0 = _mm256_sub_ps(_h00, _mm256_permute_ps(_MM256_CONJ_PS(_h10), 0b10110001));
+        h1 = _mm256_sub_ps(_h01, _mm256_permute_ps(_MM256_CONJ_PS(_h11), 0b10110001));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 h0_2 = _mm256_mul_ps(h0, h0);
+    __m256 h1_2 = _mm256_mul_ps(h1, h1);
+    __m256 hh0 = _mm256_add_ps(_mm256_movehdup_ps(h0_2), _mm256_moveldup_ps(h0_2));
+    __m256 hh1 = _mm256_add_ps(_mm256_movehdup_ps(h1_2), _mm256_moveldup_ps(h1_2));
+    __m256 hh = _mm256_add_ps(hh0, hh1);
+    __m256 hhrec = _mm256_rcp_ps(hh);
+
+    hhrec = _mm256_mul_ps(hhrec, (__m256){(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2,
+                                          (float) M_SQRT2,(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2});
+    __m256 y0 = _mm256_load_ps((float*)&y[0][i]);
+    __m256 y1 = _mm256_load_ps((float*)&y[1][i]);
+
+    __m256 x0 = _mm256_add_ps(_MM256_PROD_PS(_MM256_CONJ_PS(h0), y0), _MM256_PROD_PS(_MM256_CONJ_PS(h1), y1));
+    x0 = _mm256_mul_ps(hhrec, x0);
+
+    _mm256_store_ps((float*)&x[0][i], x0);
+
+  }
+  if (i > nof_symbols) {
+    i -= 4;
+  }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+  for (/* i = 0*/; i < nof_symbols; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h0, h1;
+    switch (codebook_idx) {
+      case 0:
+        h0 = _mm_add_ps(_h00, _h10);
+        h1 = _mm_add_ps(_h01, _h11);
+        break;
+      case 1:
+        h0 = _mm_sub_ps(_h00, _h10);
+        h1 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h0 = _mm_add_ps(_h00, _mm_permute_ps(_MM_CONJ_PS(_h10), 0b10110001));
+        h1 = _mm_add_ps(_h01, _mm_permute_ps(_MM_CONJ_PS(_h11), 0b10110001));
+        break;
+      case 3:
+        h0 = _mm_sub_ps(_h00, _mm_permute_ps(_MM_CONJ_PS(_h10), 0b10110001));
+        h1 = _mm_sub_ps(_h01, _mm_permute_ps(_MM_CONJ_PS(_h11), 0b10110001));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 h0_2 = _mm_mul_ps(h0, h0);
+    __m128 h1_2 = _mm_mul_ps(h1, h1);
+    __m128 hh0 = _mm_add_ps(_mm_movehdup_ps(h0_2), _mm_moveldup_ps(h0_2));
+    __m128 hh1 = _mm_add_ps(_mm_movehdup_ps(h1_2), _mm_moveldup_ps(h1_2));
+    __m128 hh = _mm_add_ps(hh0, hh1);
+    __m128 hhrec = _mm_rcp_ps(hh);
+
+    hhrec = _mm_mul_ps(hhrec, (__m128){(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2});
 
+    __m128 y0 = _mm_load_ps((float*)&y[0][i]);
+    __m128 y1 = _mm_load_ps((float*)&y[1][i]);
+
+    __m128 x0 = _mm_add_ps(_MM_PROD_PS(_MM_CONJ_PS(h0), y0), _MM_PROD_PS(_MM_CONJ_PS(h1), y1));
+    x0 = _mm_mul_ps(hhrec, x0);
+
+    _mm_store_ps((float*)&x[0][i], x0);
+
+  }
+  if (i > nof_symbols) {
+    i -= 2;
+  }
+#endif /* LV_HAVE_SSE */
+
+  for (/*i = 0*/; i < nof_symbols; i += 1) {
+    cf_t h0, h1;
+    float hh;
+
+    switch(codebook_idx) {
+      case 0:
+        h0 = h[0][0][i] + h[1][0][i];
+        h1 = h[0][1][i] + h[1][1][i];
+        break;
+      case 1:
+        h0 = h[0][0][i] - h[1][0][i];
+        h1 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h0 = h[0][0][i] + _Complex_I * h[1][0][i];
+        h1 = h[0][1][i] + _Complex_I * h[1][1][i];
+        break;
+      case 3:
+        h0 = h[0][0][i] - _Complex_I * h[1][0][i];
+        h1 = h[0][1][i] - _Complex_I * h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    hh = (float) M_SQRT2/(crealf(h0)*crealf(h0) + cimagf(h0)*cimagf(h0) + crealf(h1)*crealf(h1) + cimagf(h1)*cimagf(h1));
+
+    x[0][i] = (conjf(h0) * y[0][i] + conjf(h1) * y[1][i]) * hh;
+  }
+  return SRSLTE_SUCCESS;
+}
+
+int srslte_predecoding_multiplex_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                              int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols)
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+      return srslte_predecoding_multiplex_2x2_zf(y, h, x, codebook_idx, nof_symbols);
+    } else {
+      return srslte_predecoding_multiplex_2x1_mrc(y, h, x, codebook_idx, nof_symbols);
+    }
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
+  } else {
+    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+  }
+  return SRSLTE_ERROR;
+}
 
 /* 36.211 v10.3.0 Section 6.3.4 */
-int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
-    int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
+int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                  cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_ports, int nof_layers,
+                                  int codebook_idx, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
 
   if (nof_ports > SRSLTE_MAX_PORTS) {
     fprintf(stderr, "Maximum number of ports is %d (nof_ports=%d)\n", SRSLTE_MAX_PORTS,
@@ -722,10 +1277,11 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
     }
     break;
   case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-    fprintf(stderr, "Spatial multiplexing not supported\n");
-    return -1;
+    return srslte_predecoding_multiplex_zf(y, h, x, nof_rxant, nof_ports, nof_layers, codebook_idx, nof_symbols);
+    default:
+      return SRSLTE_ERROR;
   }
-  return 0;
+  return SRSLTE_ERROR;
 }
 
 
@@ -813,9 +1369,70 @@ int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS],
   }
 }
 
+int srslte_precoding_multiplex(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports,
+                               int codebook_idx, uint32_t nof_symbols)
+{
+  int i;
+  if (nof_ports == 2) {
+    if (nof_layers == 1) {
+      switch(codebook_idx) {
+        case 0:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 1:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[0], -1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 2:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_ccc(x[0], _Complex_I/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 3:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_ccc(x[0], -_Complex_I/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        default:
+          fprintf(stderr, "Invalid multiplex combination: codebook_idx=%d, nof_layers=%d, nof_ports=%d\n",
+                  codebook_idx, nof_layers, nof_ports);
+          return SRSLTE_ERROR;
+      }
+    } else if (nof_layers == 2) {
+      switch(codebook_idx) {
+        case 0:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[1], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 1:
+          for (i = 0; i < nof_symbols; i++) {
+            y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
+            y[1][i] = 0.5f*x[0][i] - 0.5f*x[1][i];
+          }
+          break;
+        case 2:
+          for (i = 0; i < nof_symbols; i++) {
+            y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
+            y[1][i] = 0.5f*_Complex_I*x[0][i] - 0.5f*_Complex_I*x[1][i];
+          }
+          break;
+        case 3:
+        default:
+          fprintf(stderr, "Invalid multiplex combination: codebook_idx=%d, nof_layers=%d, nof_ports=%d\n",
+                  codebook_idx, nof_layers, nof_ports);
+          return SRSLTE_ERROR;
+      }
+    } else {
+      ERROR("Not implemented");
+    }
+  } else {
+    ERROR("Not implemented");
+  }
+  return SRSLTE_SUCCESS;
+}
+
 /* 36.211 v10.3.0 Section 6.3.4 */
 int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers,
-    int nof_ports, int nof_symbols, srslte_mimo_type_t type) {
+    int nof_ports, int codebook_idx, int nof_symbols, srslte_mimo_type_t type) {
 
   if (nof_ports > SRSLTE_MAX_PORTS) {
     fprintf(stderr, "Maximum number of ports is %d (nof_ports=%d)\n", SRSLTE_MAX_PORTS,
@@ -829,29 +1446,30 @@ int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS],
   }
 
   switch (type) {
-  case SRSLTE_MIMO_TYPE_CDD:
-    return srslte_precoding_cdd(x, y, nof_layers, nof_ports, nof_symbols); 
-  case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-    if (nof_ports == 1 && nof_layers == 1) {
-      return srslte_precoding_single(x[0], y[0], nof_symbols);
-    } else {
-      fprintf(stderr,
-          "Number of ports and layers must be 1 for transmission on single antenna ports\n");
-      return -1;
-    }
-    break;
-  case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-    if (nof_ports == nof_layers) {
-      return srslte_precoding_diversity(x, y, nof_ports, nof_symbols);
-    } else {
-      fprintf(stderr,
-          "Error number of layers must equal number of ports in transmit diversity\n");
-      return -1;
-    }
-  case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-    fprintf(stderr, "Spatial multiplexing not supported\n");
-    return -1;
+    case SRSLTE_MIMO_TYPE_CDD:
+      return srslte_precoding_cdd(x, y, nof_layers, nof_ports, nof_symbols);
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      if (nof_ports == 1 && nof_layers == 1) {
+        return srslte_precoding_single(x[0], y[0], nof_symbols);
+      } else {
+        fprintf(stderr,
+                "Number of ports and layers must be 1 for transmission on single antenna ports\n");
+        return -1;
+      }
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      if (nof_ports == nof_layers) {
+        return srslte_precoding_diversity(x, y, nof_ports, nof_symbols);
+      } else {
+        fprintf(stderr,
+                "Error number of layers must equal number of ports in transmit diversity\n");
+        return -1;
+      }
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      return srslte_precoding_multiplex(x, y, nof_layers, nof_ports, codebook_idx, nof_symbols);
+    default:
+      return SRSLTE_ERROR;
   }
-  return 0;
+  return SRSLTE_ERROR;
 }
 
diff --git a/lib/src/phy/mimo/test/CMakeLists.txt b/lib/src/phy/mimo/test/CMakeLists.txt
index 42e262550..e0894a24d 100644
--- a/lib/src/phy/mimo/test/CMakeLists.txt
+++ b/lib/src/phy/mimo/test/CMakeLists.txt
@@ -52,7 +52,14 @@ add_test(precoding_single precoding_test -n 1000 -m single)
 add_test(precoding_diversity2 precoding_test -n 1000 -m diversity -l 2 -p 2) 
 add_test(precoding_diversity4 precoding_test -n 1024 -m diversity -l 4 -p 4) 
 
-add_test(precoding_cdd_2x2 precoding_test -m cdd -l 2 -p 2 -r 2)
- 
+add_test(precoding_cdd_2x2 precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000)
+
+add_test(precoding_multiplex_1l_cb0 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 0)
+add_test(precoding_multiplex_1l_cb1 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 1)
+add_test(precoding_multiplex_1l_cb2 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 2)
+add_test(precoding_multiplex_1l_cb3 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 3)
 
+add_test(precoding_multiplex_2l_cb0 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0)
+add_test(precoding_multiplex_2l_cb1 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1)
+add_test(precoding_multiplex_2l_cb2 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2)
 
diff --git a/lib/src/phy/mimo/test/precoder_test.c b/lib/src/phy/mimo/test/precoder_test.c
index cf8dac89a..bc23004f1 100644
--- a/lib/src/phy/mimo/test/precoder_test.c
+++ b/lib/src/phy/mimo/test/precoder_test.c
@@ -38,6 +38,7 @@
 #define MSE_THRESHOLD	0.0005
 
 int nof_symbols = 1000;
+uint32_t codebook_idx = 0;
 int nof_layers = 1, nof_tx_ports = 1, nof_rx_ports = 1, nof_re = 1;
 char *mimo_type_name = NULL;
 
@@ -46,11 +47,12 @@ void usage(char *prog) {
       "Usage: %s -m [single|diversity|multiplex|cdd] -l [nof_layers] -p [nof_tx_ports]\n"
           " -r [nof_rx_ports]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
+  printf("\t-c codebook_idx [Default %d]\n\n", codebook_idx);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "mplnr")) != -1) {
+  while ((opt = getopt(argc, argv, "mplnrc")) != -1) {
     switch (opt) {
     case 'n':
       nof_symbols = atoi(argv[optind]);
@@ -67,6 +69,9 @@ void parse_args(int argc, char **argv) {
     case 'm':
       mimo_type_name = argv[optind];
       break;
+    case 'c':
+      codebook_idx = (uint32_t) atoi(argv[optind]);
+      break;
     default:
       usage(argv[0]);
       exit(-1);
@@ -116,16 +121,13 @@ void populate_channel_single(cf_t *h) {
 
 void populate_channel(srslte_mimo_type_t type, cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]) {
   switch (type) {
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
     case SRSLTE_MIMO_TYPE_CDD:
       populate_channel_cdd(h, (uint32_t) nof_re);
       break;
     case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
       populate_channel_diversity(h, (uint32_t) nof_re);
       break;
-    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-      fprintf(stderr, "Error: not implemented channel emulator\n");
-      exit(-1);
-      //break;
     case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
     default:
       populate_channel_single(h[0][0]);
@@ -158,6 +160,9 @@ int main(int argc, char **argv) {
     case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
       nof_re = nof_layers*nof_symbols;
       break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      nof_re = nof_symbols;
+      break;
     case SRSLTE_MIMO_TYPE_CDD:
       nof_re = nof_symbols*nof_tx_ports/nof_layers;
       if (nof_rx_ports != 2 || nof_tx_ports != 2) {
@@ -223,7 +228,7 @@ int main(int argc, char **argv) {
   }
 
   /* Execute Precoding (Tx) */
-  if (srslte_precoding_type(x, y, nof_layers, nof_tx_ports, nof_symbols, type) < 0) {
+  if (srslte_precoding_type(x, y, nof_layers, nof_tx_ports, codebook_idx, nof_symbols, type) < 0) {
     fprintf(stderr, "Error layer mapper encoder\n");
     exit(-1);
   }
@@ -246,7 +251,7 @@ int main(int argc, char **argv) {
   struct timeval t[3];
   gettimeofday(&t[1], NULL);
   srslte_predecoding_type_multi(r, h, xr, nof_rx_ports, nof_tx_ports, nof_layers,
-                                nof_re, type, 0);
+                                codebook_idx, nof_re, type, 0);
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   printf("Execution Time: %ld us\n", t[0].tv_usec);
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 5d85a3ec2..8cdfd0884 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -387,7 +387,7 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
 int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
-                           uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2)
+                           uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2, srslte_mimo_type_t mimo_type, uint32_t pmi)
 {
   if (cfg) {
     if (grant) {
@@ -399,7 +399,7 @@ int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_r
     }
 
     if (srslte_cbsegm(&cfg->cb_segm2, (uint32_t) cfg->grant.mcs2.tbs)) {
-      fprintf(stderr, "Error computing Codeblock (2) segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
+      fprintf(stderr, "Error computing Codeblock (2) segmentation for TBS=%d\n", cfg->grant.mcs2.tbs);
       return SRSLTE_ERROR;
     }
 
@@ -407,19 +407,41 @@ int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_r
     cfg->sf_idx = sf_idx;
     cfg->rv = rvidx;
     cfg->rv2 = rvidx2;
+    cfg->mimo_type = mimo_type;
 
-    if (cell.nof_ports == 1 && grant->nof_tb == 1) {
-      cfg->mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-      cfg->nof_layers = 1;
-    } else if (cell.nof_ports == 2 && grant->nof_tb == 1) {
-      cfg->mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      cfg->nof_layers = 2;
-    } else if (cell.nof_ports == 2 && grant->nof_tb == 2) {
-      cfg->mimo_type = SRSLTE_MIMO_TYPE_CDD;
-      cfg->nof_layers = 2;
-    } else {
-      INFO("nof_ports=%d, nof_tb=%d are not consistent\n", cell.nof_ports, grant->nof_tb);
-      return SRSLTE_ERROR;
+    /* Check and configure PDSCH transmission modes */
+    switch(mimo_type) {
+      case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+        if (grant->nof_tb != 1) {
+          ERROR("Number of transport blocks is not supported for single transmission mode.");
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 1;
+        break;
+      case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+        if (grant->nof_tb != 1) {
+          ERROR("Number of transport blocks is not supported for transmit diversity mode.");
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 2;
+        break;
+      case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+        if (grant->nof_tb == 1) {
+          cfg->codebook_idx = pmi;
+          cfg->nof_layers = 1;
+        } else {
+          cfg->codebook_idx = pmi + 1;
+          cfg->nof_layers = 2;
+        }
+        INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n", grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+        break;
+      case SRSLTE_MIMO_TYPE_CDD:
+        if (grant->nof_tb != 2) {
+          ERROR("Number of transport blocks is not supported for CDD transmission mode.");
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 2;
+        break;
     }
 
     return SRSLTE_SUCCESS;
@@ -498,9 +520,9 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       cfg          != NULL)
   {
     
-    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d, C_prb=%d\n",
+    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: [%d %d], C_prb=%d\n",
         cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re, 
-         cfg->nbits.nof_bits, cfg->rv, cfg->grant.nof_prb);
+         cfg->nbits.nof_bits, cfg->rv, cfg->rv2, cfg->grant.nof_prb);
 
     /* number of layers equals number of ports */
     for (i = 0; i < q->cell.nof_ports; i++) {
@@ -525,17 +547,19 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
         }
       }      
     }
-    
+
+    INFO("PDSCH Layer demapper and predecode: mimo_type=%d, nof_layers=%d, nof_tb=%d\n", cfg->mimo_type,
+         cfg->nof_layers, cfg->grant.nof_tb);
     if (q->cell.nof_ports == 1) {
       /* no need for layer demapping */
       srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
     } else {
       int nof_symbols [SRSLTE_MAX_CODEWORDS];
-      nof_symbols[0] = cfg->nbits.nof_re * cfg->grant.nof_tb / q->cell.nof_ports;
-      nof_symbols[1] = cfg->nbits2.nof_re * cfg->grant.nof_tb / q->cell.nof_ports;
+      nof_symbols[0] = cfg->nbits.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+      nof_symbols[1] = cfg->nbits2.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
 
       srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
-                                    cfg->nbits.nof_re, cfg->mimo_type, 0.0);
+                                    cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, 0.0);
       srslte_layerdemap_type(x, (cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, cfg->nof_layers, cfg->grant.nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
@@ -616,6 +640,79 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
   }
 }
 
+int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
+                               srslte_pdsch_cfg_t *cfg,
+                               cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
+                               uint32_t *ri, uint32_t *pmi,
+                               float *current_sinr) {
+  uint32_t best_pmi_1l;
+  uint32_t best_pmi_2l;
+  float sinr_1l[SRSLTE_MAX_CODEBOOKS];
+  float sinr_2l[SRSLTE_MAX_CODEBOOKS];
+  float best_sinr_1l = 0.0;
+  float best_sinr_2l = 0.0;
+  int n1, n2;
+
+  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+    n1 = srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, 1, &best_pmi_1l, sinr_1l);
+    if (n1 < 0) {
+      ERROR("PMI Select for 1 layer");
+      return SRSLTE_ERROR;
+    }
+
+    n2 = srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, 2, &best_pmi_2l, sinr_2l);
+    if (n2 < 0) {
+      ERROR("PMI Select for 2 layer");
+      return SRSLTE_ERROR;
+    }
+
+    for (int i = 0; i < n1; i++) {
+      if (sinr_1l[i] > best_sinr_1l) {
+        best_sinr_1l = sinr_1l[i];
+      }
+    }
+
+    for (int i = 0; i < n2; i++) {
+      if (sinr_2l[i] > best_sinr_2l) {
+        best_sinr_2l = sinr_2l[i];
+      }
+    }
+
+    /* Set RI */
+    if (ri != NULL) {
+      *ri = (best_sinr_1l > best_sinr_2l) ? 1 : 2;
+    }
+
+    /* Set PMI */
+    if (pmi != NULL) {
+      *pmi = (best_sinr_1l > best_sinr_2l) ? best_pmi_1l : best_pmi_2l;
+    }
+
+    /* Set current condition number */
+    if (current_sinr != NULL) {
+      if (cfg->nof_layers == 1) {
+        *current_sinr = sinr_1l[cfg->codebook_idx];
+      } else if (cfg->nof_layers == 2) {
+        *current_sinr = sinr_2l[cfg->codebook_idx - 1];
+      }else {
+        ERROR("Not implemented number of layers");
+        return SRSLTE_ERROR;
+      }
+    }
+
+    /* Print Trace */
+    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
+      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
+           10*log10(*current_sinr), cfg->nof_layers, cfg->codebook_idx);
+    }
+  } else {
+    ERROR("Not implemented configuration");
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
 int srslte_pdsch_encode(srslte_pdsch_t *q, 
                         srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
                         uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) 
@@ -785,7 +882,7 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
     if (q->cell.nof_ports > 1) {
       int nof_symbols = srslte_layermap_type((cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, x, cfg->grant.nof_tb, cfg->nof_layers,
                            (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits.nof_re, cfg->nbits2.nof_re}, cfg->mimo_type);
-      srslte_precoding_type(x, q->symbols, q->cell.nof_ports, cfg->nof_layers,
+      srslte_precoding_type(x, q->symbols, cfg->nof_layers, q->cell.nof_ports, cfg->codebook_idx,
                             nof_symbols, cfg->mimo_type);
     } else {
       memcpy(q->symbols[0], q->d, cfg->nbits.nof_re * sizeof(cf_t));
diff --git a/lib/src/phy/phch/test/CMakeLists.txt b/lib/src/phy/phch/test/CMakeLists.txt
index 1c9846c76..832f18d1f 100644
--- a/lib/src/phy/phch/test/CMakeLists.txt
+++ b/lib/src/phy/phch/test/CMakeLists.txt
@@ -85,28 +85,79 @@ target_link_libraries(pdsch_test srslte_phy)
 add_test(pdsch_test_qpsk pdsch_test -m 10 -n 50 -r 1)
 add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100)
 add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100 -r 2)
-add_test(pdsch_test_qam64 pdsch_test -m 28 -n 100)
-
-add_test(pdsch_test_sin_6   pdsch_test -p 1 -a 2 -w 1 -n 6)
-add_test(pdsch_test_sin_12  pdsch_test -p 1 -a 2 -w 1 -n 12)
-add_test(pdsch_test_sin_25  pdsch_test -p 1 -a 2 -w 1 -n 25)
-add_test(pdsch_test_sin_50  pdsch_test -p 1 -a 2 -w 1 -n 50)
-add_test(pdsch_test_sin_75  pdsch_test -p 1 -a 2 -w 1 -n 75)
-add_test(pdsch_test_sin_100 pdsch_test -p 1 -a 2 -w 1 -n 100 -m 28)
-
-add_test(pdsch_test_div_6   pdsch_test -p 2 -a 2 -w 1 -n 6)
-add_test(pdsch_test_div_12  pdsch_test -p 2 -a 2 -w 1 -n 12)
-add_test(pdsch_test_div_25  pdsch_test -p 2 -a 2 -w 1 -n 25)
-add_test(pdsch_test_div_50  pdsch_test -p 2 -a 2 -w 1 -n 50)
-add_test(pdsch_test_div_75  pdsch_test -p 2 -a 2 -w 1 -n 75)
-add_test(pdsch_test_div_100 pdsch_test -p 2 -a 2 -w 1 -n 100 -m 28)
-
-add_test(pdsch_test_cdd_6   pdsch_test -p 2 -a 2 -w 2 -n 6)
-add_test(pdsch_test_cdd_12  pdsch_test -p 2 -a 2 -w 2 -n 12)
-add_test(pdsch_test_cdd_25  pdsch_test -p 2 -a 2 -w 2 -n 25)
-add_test(pdsch_test_cdd_50  pdsch_test -p 2 -a 2 -w 2 -n 50)
-add_test(pdsch_test_cdd_75  pdsch_test -p 2 -a 2 -w 2 -n 75)
-add_test(pdsch_test_cdd_100 pdsch_test -p 2 -a 2 -w 2 -n 100 -m 28 -M 28)
+add_test(pdsch_test_qam64 pdsch_test -n 100)
+
+# PDSCH test for single transmision mode and 2 Rx antennas
+add_test(pdsch_test_sin_6   pdsch_test -x single -a 2 -n 6)
+add_test(pdsch_test_sin_12  pdsch_test -x single -a 2 -n 12)
+add_test(pdsch_test_sin_25  pdsch_test -x single -a 2 -n 25)
+add_test(pdsch_test_sin_50  pdsch_test -x single -a 2 -n 50)
+add_test(pdsch_test_sin_75  pdsch_test -x single -a 2 -n 75)
+add_test(pdsch_test_sin_100 pdsch_test -x single -a 2 -n 100)
+
+# PDSCH test for transmit diversity transmision mode (1 codeword)
+add_test(pdsch_test_div_6   pdsch_test -x diversity -a 2 -n 6)
+add_test(pdsch_test_div_12  pdsch_test -x diversity -a 2 -n 12)
+add_test(pdsch_test_div_25  pdsch_test -x diversity -a 2 -n 25)
+add_test(pdsch_test_div_50  pdsch_test -x diversity -a 2 -n 50)
+add_test(pdsch_test_div_75  pdsch_test -x diversity -a 2 -n 75)
+add_test(pdsch_test_div_100 pdsch_test -x diversity -a 2 -n 100)
+
+# PDSCH test for CDD transmision mode (2 codeword)
+add_test(pdsch_test_cdd_6   pdsch_test -x cdd -a 2 -t 0 -n 6)
+add_test(pdsch_test_cdd_12  pdsch_test -x cdd -a 2 -t 0 -n 12)
+add_test(pdsch_test_cdd_25  pdsch_test -x cdd -a 2 -t 0 -n 25)
+add_test(pdsch_test_cdd_50  pdsch_test -x cdd -a 2 -t 0 -n 50)
+add_test(pdsch_test_cdd_75  pdsch_test -x cdd -a 2 -t 0 -n 75)
+add_test(pdsch_test_cdd_100 pdsch_test -x cdd -a 2 -t 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 0 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p0_6   pdsch_test -x multiplex -a 2 -p 0 -n 6)
+add_test(pdsch_test_multiplex1cw_p0_12  pdsch_test -x multiplex -a 2 -p 0 -n 12)
+add_test(pdsch_test_multiplex1cw_p0_25  pdsch_test -x multiplex -a 2 -p 0 -n 25)
+add_test(pdsch_test_multiplex1cw_p0_50  pdsch_test -x multiplex -a 2 -p 0 -n 50)
+add_test(pdsch_test_multiplex1cw_p0_75  pdsch_test -x multiplex -a 2 -p 0 -n 75)
+add_test(pdsch_test_multiplex1cw_p0_100 pdsch_test -x multiplex -a 2 -p 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 1 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p1_6   pdsch_test -x multiplex -a 2 -p 1 -n 6)
+add_test(pdsch_test_multiplex1cw_p1_12  pdsch_test -x multiplex -a 2 -p 1 -n 12)
+add_test(pdsch_test_multiplex1cw_p1_25  pdsch_test -x multiplex -a 2 -p 1 -n 25)
+add_test(pdsch_test_multiplex1cw_p1_50  pdsch_test -x multiplex -a 2 -p 1 -n 50)
+add_test(pdsch_test_multiplex1cw_p1_75  pdsch_test -x multiplex -a 2 -p 1 -n 75)
+add_test(pdsch_test_multiplex1cw_p1_100 pdsch_test -x multiplex -a 2 -p 1 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 2 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p2_6   pdsch_test -x multiplex -a 2 -p 2 -n 6)
+add_test(pdsch_test_multiplex1cw_p2_12  pdsch_test -x multiplex -a 2 -p 2 -n 12)
+add_test(pdsch_test_multiplex1cw_p2_25  pdsch_test -x multiplex -a 2 -p 2 -n 25)
+add_test(pdsch_test_multiplex1cw_p2_50  pdsch_test -x multiplex -a 2 -p 2 -n 50)
+add_test(pdsch_test_multiplex1cw_p2_75  pdsch_test -x multiplex -a 2 -p 2 -n 75)
+add_test(pdsch_test_multiplex1cw_p2_100 pdsch_test -x multiplex -a 2 -p 2 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 3 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p3_6   pdsch_test -x multiplex -a 2 -p 3 -n 6)
+add_test(pdsch_test_multiplex1cw_p3_12  pdsch_test -x multiplex -a 2 -p 3 -n 12)
+add_test(pdsch_test_multiplex1cw_p3_25  pdsch_test -x multiplex -a 2 -p 3 -n 25)
+add_test(pdsch_test_multiplex1cw_p3_50  pdsch_test -x multiplex -a 2 -p 3 -n 50)
+add_test(pdsch_test_multiplex1cw_p3_75  pdsch_test -x multiplex -a 2 -p 3 -n 75)
+add_test(pdsch_test_multiplex1cw_p3_100 pdsch_test -x multiplex -a 2 -p 3 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 0 (2 codeword)
+add_test(pdsch_test_multiplex2cw_p0_6   pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 6)
+add_test(pdsch_test_multiplex2cw_p0_12  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 12)
+add_test(pdsch_test_multiplex2cw_p0_25  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 25)
+add_test(pdsch_test_multiplex2cw_p0_50  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 50)
+add_test(pdsch_test_multiplex2cw_p0_75  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 75)
+add_test(pdsch_test_multiplex2cw_p0_100 pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 1 (2 codeword)
+add_test(pdsch_test_multiplex2cw_p1_6   pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 6)
+add_test(pdsch_test_multiplex2cw_p1_12  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 12)
+add_test(pdsch_test_multiplex2cw_p1_25  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 25)
+add_test(pdsch_test_multiplex2cw_p1_50  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 50)
+add_test(pdsch_test_multiplex2cw_p1_75  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 75)
+add_test(pdsch_test_multiplex2cw_p1_100 pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 100)
 
 ########################################################################
 # FILE TEST  
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 2dc21bab4..8c79cc836 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -52,15 +52,17 @@ srslte_cell_t cell = {
   SRSLTE_PHICH_R_1_6    // PHICH resources
 };
 
+char mimo_type_str [32] = "single";
+srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
 uint32_t cfi = 2;
 uint32_t mcs = 0;
 uint32_t mcs2 = 0;
 uint32_t subframe = 1;
 uint32_t rv_idx = 0;
-uint32_t rv_idx2 = 0;
+uint32_t rv_idx2 = 1;
 uint16_t rnti = 1234;
-uint32_t nof_tb = 1;
 uint32_t nof_rx_antennas = 1;
+uint32_t pmi = 0;
 char *input_file = NULL; 
 
 void usage(char *prog) {
@@ -74,16 +76,16 @@ void usage(char *prog) {
   printf("\t-t rv_idx2 [Default %d]\n", rv_idx2);
   printf("\t-R rnti [Default %d]\n", rnti);
   printf("\t-F cfi [Default %d]\n", cfi);
-  printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
+  printf("\t-x Transmission mode [single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
   printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
-  printf("\t-w nof_tb [Default %d]\n", nof_tb);
   printf("\t-a nof_rx_antennas [Default %d]\n", nof_rx_antennas);
+  printf("\t-p pmi (multiplex only)  [Default %d]\n", pmi);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "fmMcsrtRFpnwav")) != -1) {
+  while ((opt = getopt(argc, argv, "fmMcsrtRFpnavx")) != -1) {
     switch(opt) {
     case 'f':
       input_file = argv[optind];
@@ -109,8 +111,11 @@ void parse_args(int argc, char **argv) {
     case 'F':
       cfi = atoi(argv[optind]);
       break;
+    case 'x':
+      strncpy(mimo_type_str, argv[optind], 32);
+      break;
     case 'p':
-      cell.nof_ports = atoi(argv[optind]);
+      pmi = (uint32_t) atoi(argv[optind]);
       break;
     case 'n':
       cell.nof_prb = atoi(argv[optind]);
@@ -118,9 +123,6 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
-    case 'w':
-      nof_tb = (uint32_t) atoi(argv[optind]);
-      break;
     case 'a':
       nof_rx_antennas = (uint32_t) atoi(argv[optind]);
       break;
@@ -165,14 +167,43 @@ int main(int argc, char **argv) {
   bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(softbuffers_tx, sizeof(srslte_softbuffer_tx_t)*SRSLTE_MAX_CODEWORDS);
   bzero(softbuffers_rx, sizeof(srslte_softbuffer_rx_t)*SRSLTE_MAX_CODEWORDS);
-  
+
+  /* Parse transmission mode */
+  if (srslte_str2mimotype(mimo_type_str, &mimo_type)) {
+    ERROR("Wrong transmission mode.");
+    goto quit;
+  }
+
+  switch(mimo_type) {
+
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      cell.nof_ports = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+    case SRSLTE_MIMO_TYPE_CDD:
+      if (nof_rx_antennas < 2) {
+        ERROR("At least two receiving antennas are required");
+        goto quit;
+      }
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+    default:
+      cell.nof_ports = 2;
+      break;
+  }
+
   srslte_ra_dl_dci_t dci;
   bzero(&dci, sizeof(srslte_ra_dl_dci_t));
-  dci.mcs_idx = mcs;
-  dci.rv_idx = rv_idx;
   dci.type0_alloc.rbg_bitmask = 0xffffffff;
-  dci.tb_en[0] = true; 
-  if (nof_tb > 1) {
+
+  /* If transport block 0 is enabled */
+  if (mcs != 0 || rv_idx != 1) {
+    dci.mcs_idx = mcs;
+    dci.rv_idx = rv_idx;
+    dci.tb_en[0] = true;
+  }
+
+  /* If transport block 0 is disabled */
+  if (mcs2 != 0 || rv_idx2 != 1) {
     dci.mcs_idx_1 = mcs2;
     dci.rv_idx_1 = rv_idx2;
     dci.tb_en[1] = true;
@@ -183,7 +214,9 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error computing resource allocation\n");
     return ret;
   }
-  
+
+
+
 #ifdef DO_OFDM
   srslte_ofdm_tx_init(&ofdm_tx, cell.cp, cell.nof_prb);
   srslte_ofdm_rx_init(&ofdm_rx, cell.cp, cell.nof_prb);
@@ -201,24 +234,9 @@ int main(int argc, char **argv) {
 #endif /* DO_OFDM */
 
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, rv_idx2)) {
+  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, rv_idx2, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
-    exit(-1);
-  }
-
-  /* Select MIMO mode */
-  if (cell.nof_ports == 1 && nof_tb == 1) {
-    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-    pdsch_cfg.nof_layers = 1;
-  } else if (cell.nof_ports == 2 && nof_tb == 1) {
-    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-    pdsch_cfg.nof_layers = 2;
-  } else if (cell.nof_ports == 2 && nof_tb == 2) {
-    pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_CDD;
-    pdsch_cfg.nof_layers = 2;
-  } else {
-    fprintf(stderr, "nof_ports=%d, nof_tb=%d are not consistent\n", cell.nof_ports, nof_tb);
-    exit(-1);
+    goto quit;
   }
 
   /* init memory */
@@ -250,7 +268,7 @@ int main(int argc, char **argv) {
   }
 
   if (grant.mcs2.tbs) {
-    data[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs);
+    data[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs2.tbs);
     if (!data[1]) {
       perror("srslte_vec_malloc");
       goto quit;
@@ -264,7 +282,7 @@ int main(int argc, char **argv) {
 
   srslte_pdsch_set_rnti(&pdsch_rx, rnti);
 
-  for (i = 0; i < nof_tb; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     if (srslte_softbuffer_rx_init(&softbuffers_rx[i], cell.nof_prb)) {
       fprintf(stderr, "Error initiating RX soft buffer\n");
       goto quit;
@@ -306,7 +324,7 @@ int main(int argc, char **argv) {
     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);
+      goto quit;
     }
 #ifdef DO_OFDM
     srslte_filesource_read(&fsrc, rx_slot_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
@@ -332,7 +350,7 @@ int main(int argc, char **argv) {
 
     srslte_pdsch_set_rnti(&pdsch_tx, rnti);
 
-    for (i = 0; i < nof_tb; i++) {
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
       if (srslte_softbuffer_tx_init(&softbuffers_tx[i], cell.nof_prb)) {
         fprintf(stderr, "Error initiating TX soft buffer\n");
         goto quit;
@@ -438,7 +456,7 @@ int main(int argc, char **argv) {
 quit:
   srslte_pdsch_free(&pdsch_tx);
   srslte_pdsch_free(&pdsch_rx);
-  for (i = 0; i < nof_tb; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     srslte_softbuffer_tx_free(&softbuffers_tx[i]);
     srslte_softbuffer_rx_free(&softbuffers_rx[i]);
 
diff --git a/lib/src/phy/phch/test/pdsch_test_mex.c b/lib/src/phy/phch/test/pdsch_test_mex.c
index 204154130..5093c326d 100644
--- a/lib/src/phy/phch/test/pdsch_test_mex.c
+++ b/lib/src/phy/phch/test/pdsch_test_mex.c
@@ -202,10 +202,17 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       ce[i][j] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
     }
   }
-  uint8_t *data_bytes = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs/8);
-  if (!data_bytes) {
+  uint8_t *data_bytes[SRSLTE_MAX_CODEWORDS];
+  data_bytes[0] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs/8);
+  if (!data_bytes[0]) {
     return;
   }
+
+  data_bytes[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs2.tbs/8);
+  if (!data_bytes[1]) {
+    return;
+  }
+
   srslte_sch_set_max_noi(&pdsch.dl_sch, max_iterations);
 
   bool input_fft_allocated = false; 
@@ -272,7 +279,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
   
   uint8_t *data = malloc(grant.mcs.tbs);
-  srslte_bit_unpack_vector(data_bytes, data, grant.mcs.tbs);
+  srslte_bit_unpack_vector(data_bytes[0], data, grant.mcs.tbs);
   
   if (nlhs >= 1) { 
     plhs[0] = mxCreateLogicalScalar(r == 0);
@@ -323,7 +330,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       }
     }
   }
-  free(data_bytes);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data_bytes[i]) {
+      free(data_bytes[i]);
+    }
+  }
   free(data);
   
   return;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 3b1a83466..c068f5899 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -38,8 +38,8 @@
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
 
-static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT2A}; // Only TM1 and TM2 are currently supported
-const uint32_t nof_ue_formats = 3;
+static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT2A, SRSLTE_DCI_FORMAT2}; // Only TM1, TM2, TM3 and TM4 are currently supported
+const uint32_t nof_ue_formats = 4;
 
 static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
@@ -284,12 +284,33 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
 {
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, 0);
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, 0, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
 }
 
-int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2)
+int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
+                                 uint32_t rvidx, uint32_t rvidx2, srslte_mimo_type_t mimo_type, uint32_t pinfo)
 {
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, rvidx2);
+  uint32_t pmi = 0;
+
+  /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
+  if (q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+    if (q->pdsch_cfg.grant.nof_tb == 1) {
+      if (pinfo > 0 && pinfo < 5) {
+        pmi = pinfo - 1;
+      } else {
+        ERROR("Not Implemented");
+        return SRSLTE_ERROR;
+      }
+    } else {
+      if (pinfo < 2) {
+        pmi = pinfo;
+      } else {
+        ERROR("Not Implemented");
+        return SRSLTE_ERROR;
+      }
+    }
+  }
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, rvidx2, mimo_type, pmi);
 }
 
 int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti)
@@ -303,6 +324,7 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint
 
 int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti, uint16_t rnti)
 {
+  srslte_mimo_type_t mimo_type;
   srslte_dci_msg_t dci_msg;
   srslte_ra_dl_dci_t dci_unpacked;
   srslte_ra_dl_grant_t grant; 
@@ -355,7 +377,39 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
       }
     }
 
-    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, rvidx2)) {
+    switch(dci_msg.format) {
+      case SRSLTE_DCI_FORMAT1:
+      case SRSLTE_DCI_FORMAT1A:
+        mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+        break;
+      case SRSLTE_DCI_FORMAT2:
+        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+        }
+        break;
+      case SRSLTE_DCI_FORMAT2A:
+        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_CDD;
+        }
+        break;
+
+      /* Not implemented formats */
+      case SRSLTE_DCI_FORMAT0:
+      case SRSLTE_DCI_FORMAT1C:
+      case SRSLTE_DCI_FORMAT1B:
+      case SRSLTE_DCI_FORMAT1D:
+      case SRSLTE_DCI_FORMAT2B:
+      default:
+        ERROR("Transmission mode not supported.");
+        return SRSLTE_ERROR;
+    }
+
+    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, rvidx2, mimo_type, dci_unpacked.pinfo)) {
+      ERROR("Configuing PDSCH");
       return SRSLTE_ERROR; 
     }
     
@@ -374,7 +428,10 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
         q->pkt_errors++;
       } else if (ret == SRSLTE_ERROR_INVALID_INPUTS) {
         fprintf(stderr, "Error calling srslte_pdsch_decode()\n");      
-      } 
+      }
+
+      /* If we are in TM4 (Closed-Loop MIMO), compute condition number */
+
     }
   
   /*
@@ -395,6 +452,13 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   }
 }
 
+int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
+  float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
+  return srslte_pdsch_ri_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
+                                    SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp),
+                                    ri, pmi, current_sinr);
+}
+
 uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q) {
   return q->last_location.ncce; 
 }
diff --git a/lib/src/phy/utils/algebra.c b/lib/src/phy/utils/algebra.c
new file mode 100644
index 000000000..69ac08749
--- /dev/null
+++ b/lib/src/phy/utils/algebra.c
@@ -0,0 +1,72 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+
+#include <immintrin.h>
+
+#include "srslte/phy/utils/algebra.h"
+
+#ifdef LV_HAVE_SSE
+
+inline void srslte_algebra_2x2_zf_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                      __m128 *x0, __m128 *x1, float norm) {
+  __m128 detmult1 = _MM_PROD_PS(h00, h11);
+  __m128 detmult2 = _MM_PROD_PS(h01, h10);
+
+  __m128 det = _mm_sub_ps(detmult1, detmult2);
+  __m128 detconj = _MM_CONJ_PS(det);
+  __m128 detabs2 = _MM_PROD_PS(det, detconj);
+  __m128 detabs2rec = _mm_rcp_ps(detabs2);
+  detabs2rec = _mm_moveldup_ps(detabs2rec);
+  __m128 detrec = _mm_mul_ps(_mm_mul_ps(detconj, detabs2rec),
+                                (__m128) {norm, norm, norm, norm});
+
+  *x0 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h11, y0), _MM_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h00, y1), _MM_PROD_PS(h10, y0)), detrec);
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+inline void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                 __m256 *x0, __m256 *x1, float norm) {
+  __m256 detmult1 = _MM256_PROD_PS(h00, h11);
+  __m256 detmult2 = _MM256_PROD_PS(h01, h10);
+
+  __m256 det = _mm256_sub_ps(detmult1, detmult2);
+  __m256 detconj = _MM256_CONJ_PS(det);
+  __m256 sqdet = _mm256_mul_ps(det, det);
+  __m256 detabs2 = _mm256_add_ps(_mm256_movehdup_ps(sqdet), _mm256_moveldup_ps(sqdet));
+  __m256 detabs2rec = _mm256_rcp_ps(detabs2);
+  __m256 detrec = _mm256_mul_ps(_mm256_mul_ps(detconj, detabs2rec),
+                                (__m256) {norm, norm, norm, norm, norm, norm, norm, norm});
+
+  *x0 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h11, y0), _MM256_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h00, y1), _MM256_PROD_PS(h10, y0)), detrec);
+}
+
+#endif /* LV_HAVE_AVX */

From c474b24d3245ed69b66f9fbcc39fc21408f16df4 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 7 Aug 2017 13:17:11 +0200
Subject: [PATCH 064/269] add basic NAS configuration class

---
 lib/include/srslte/common/interfaces_common.h | 11 +++++++++++
 srsue/hdr/upper/nas.h                         |  4 ++--
 srsue/src/upper/nas.cc                        | 10 +++++-----
 3 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/lib/include/srslte/common/interfaces_common.h b/lib/include/srslte/common/interfaces_common.h
index 44b1c5338..d919d9fe5 100644
--- a/lib/include/srslte/common/interfaces_common.h
+++ b/lib/include/srslte/common/interfaces_common.h
@@ -34,6 +34,17 @@
 
 namespace srslte {
 
+class srslte_nas_config_t
+{
+public:
+  srslte_nas_config_t(uint32_t lcid_ = 0)
+    :lcid(lcid_)
+    {}
+
+  uint32_t lcid;
+};
+
+
 class srslte_pdcp_config_t
 {
 public:
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index ae3dfe246..32afd52bb 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -66,7 +66,7 @@ public:
             rrc_interface_nas   *rrc_,
             gw_interface_nas    *gw_,
             srslte::log         *nas_log_,
-            uint32_t            lcid_);
+            srslte::srslte_nas_config_t cfg_);
   void stop();
 
   emm_state_t get_state();
@@ -84,7 +84,7 @@ private:
   rrc_interface_nas  *rrc;
   usim_interface_nas *usim;
   gw_interface_nas   *gw;
-  uint32_t           default_lcid;
+  srslte::srslte_nas_config_t cfg;
 
   emm_state_t        state;
    
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 440ac77e4..1595a2cec 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -44,14 +44,14 @@ void nas::init(usim_interface_nas *usim_,
                rrc_interface_nas  *rrc_,
                gw_interface_nas   *gw_,
                srslte::log        *nas_log_,
-               uint32_t           lcid_)
+               srslte::srslte_nas_config_t cfg_)
 {
   pool    = byte_buffer_pool::get_instance();
   usim    = usim_;
   rrc     = rrc_;
   gw      = gw_;
   nas_log = nas_log_;
-  default_lcid = lcid_;
+  cfg     = cfg_;
 }
 
 void nas::stop()
@@ -574,7 +574,7 @@ void nas::send_attach_request()
   liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT*)msg);
 
   nas_log->info("Sending attach request\n");
-  rrc->write_sdu(default_lcid, msg);
+  rrc->write_sdu(cfg.lcid, msg);
 }
 
 void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg)
@@ -616,7 +616,7 @@ void nas::send_service_request()
   uint8_t mac[4];
   integrity_generate(&k_nas_int[16],
                       count_ul,
-                      default_lcid-1,
+                      cfg.lcid-1,
                       SECURITY_DIRECTION_UPLINK,
                       &msg->msg[0],
                       2,
@@ -627,7 +627,7 @@ void nas::send_service_request()
   msg->msg[3] = mac[3];
   msg->N_bytes++;
   nas_log->info("Sending service request\n");
-  rrc->write_sdu(default_lcid, msg);
+  rrc->write_sdu(cfg.lcid, msg);
 }
 
 void nas::send_esm_information_response(){}

From 9c49e27778740234e9fdb3f9751b06f4274f56a3 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 7 Aug 2017 13:18:18 +0200
Subject: [PATCH 065/269] remove unused variable

---
 lib/src/phy/rf/rf_soapy_imp.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 75711f6d8..31649af69 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -447,7 +447,6 @@ int rf_soapy_send_timed(void *h,
     rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
     timeNs = secs * 1000000000;
     timeNs = timeNs + (frac_secs * 1000000000);
-    int num_channels = 1;
     int n = 0;
 
     if(!handler->tx_stream_active){

From 2bf288ff59b2afefc9f7b03bd7ad6d466f5fa67a Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 16 Aug 2017 17:39:49 +0200
Subject: [PATCH 066/269] Solved infinite loop and/or segmentation fault in the
 SCH decoder for non-SSE compilation.

---
 lib/include/srslte/phy/fec/turbodecoder_simd.h | 4 ++--
 lib/src/phy/fec/turbodecoder_gen.c             | 4 ++++
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd.h b/lib/include/srslte/phy/fec/turbodecoder_simd.h
index 7ca3914ca..d7bc284d2 100644
--- a/lib/include/srslte/phy/fec/turbodecoder_simd.h
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd.h
@@ -48,13 +48,13 @@
 // The constant SRSLTE_TDEC_NPAR defines the maximum number of parallel CB supported by all SIMD decoders 
 #ifdef ENABLE_SIMD_INTER
   #include "srslte/phy/fec/turbodecoder_simd_inter.h"
-  #if LV_HAVE_AVX2
+  #ifdef LV_HAVE_AVX2
     #define SRSLTE_TDEC_NPAR_INTRA 2
   #else
     #define SRSLTE_TDEC_NPAR_INTRA 1
   #endif
 #else
-  #if LV_HAVE_AVX2
+  #ifdef LV_HAVE_AVX2
     #define SRSLTE_TDEC_NPAR 2
   #else
     #define SRSLTE_TDEC_NPAR 1
diff --git a/lib/src/phy/fec/turbodecoder_gen.c b/lib/src/phy/fec/turbodecoder_gen.c
index 396d94caa..649c3363a 100644
--- a/lib/src/phy/fec/turbodecoder_gen.c
+++ b/lib/src/phy/fec/turbodecoder_gen.c
@@ -328,6 +328,9 @@ void srslte_tdec_gen_iteration(srslte_tdec_gen_t * h, float * input, uint32_t lo
   } else {
     fprintf(stderr, "Error CB index not set (call srslte_tdec_gen_reset() first\n");    
   }
+
+  // Increase number of iterations
+  h->n_iter++;
 }
 
 int srslte_tdec_gen_reset(srslte_tdec_gen_t * h, uint32_t long_cb)
@@ -339,6 +342,7 @@ int srslte_tdec_gen_reset(srslte_tdec_gen_t * h, uint32_t long_cb)
   }
   memset(h->w, 0, sizeof(float) * long_cb);
   h->current_cbidx = srslte_cbsegm_cbindex(long_cb);
+  h->current_cb_len = long_cb;
   if (h->current_cbidx < 0) {
     fprintf(stderr, "Invalid CB length %d\n", long_cb);
     return -1; 

From 2d1166e3fa23d4230b9518b690e4a76aa2a8c3a1 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:13:03 +0200
Subject: [PATCH 067/269] Added FMA flag

---
 cmake/modules/FindSSE.cmake | 40 +++++++++++++++++++++++++++++++++++--
 1 file changed, 38 insertions(+), 2 deletions(-)

diff --git a/cmake/modules/FindSSE.cmake b/cmake/modules/FindSSE.cmake
index 30be8a206..3440f01c1 100644
--- a/cmake/modules/FindSSE.cmake
+++ b/cmake/modules/FindSSE.cmake
@@ -7,6 +7,7 @@ include(CheckCSourceRuns)
 option(ENABLE_SSE "Enable compile-time SSE4.1 support." ON)
 option(ENABLE_AVX "Enable compile-time AVX support."  ON)
 option(ENABLE_AVX2 "Enable compile-time AVX2 support."  ON)
+option(ENABLE_FMA "Enable compile-time FMA support."  ON)
 
 if (ENABLE_SSE)
     #
@@ -97,8 +98,43 @@ if (ENABLE_SSE)
       if (HAVE_AVX2)
           message(STATUS "AVX2 is enabled - target CPU must support it")
       endif()
-  endif()
+    endif()
+
+    if (ENABLE_FMA)
+
+        #
+        # Check compiler for AVX intrinsics
+        #
+        if (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_CLANG )
+            set(CMAKE_REQUIRED_FLAGS "-mfma")
+            check_c_source_runs("
+            #include <immintrin.h>
+            int main()
+            {
+              __m256 a, b, c, r;
+              const float src[8] = { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f };
+              float dst[8];
+              a = _mm256_loadu_ps( src );
+              b = _mm256_loadu_ps( src );
+              c = _mm256_loadu_ps( src );
+              r = _mm256_fmadd_ps( a, b, c );
+              _mm256_storeu_ps( dst, r );
+              int i = 0;
+              for( i = 0; i < 8; i++ ){
+                if( ( src[i] * src[i] + src[i] ) != dst[i] ){
+                  return -1;
+                }
+              }
+              return 0;
+            }"
+                    HAVE_FMA)
+        endif()
+
+        if (HAVE_FMA)
+            message(STATUS "FMA is enabled - target CPU must support it")
+        endif()
+    endif()
 
 endif()
 
-mark_as_advanced(HAVE_SSE, HAVE_AVX, HAVE_AVX2)
+mark_as_advanced(HAVE_SSE, HAVE_AVX, HAVE_AVX2, HAVE_FMA)

From d1709e06af56e78d5d179f34d07ada79dd8bbd32 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:14:37 +0200
Subject: [PATCH 068/269] Added algebra SSE, AVX (+FMA) implementatiokns with
 test

---
 lib/include/srslte/phy/utils/algebra.h | 129 ++++++--
 lib/src/phy/utils/algebra.c            | 224 +++++++++++--
 lib/src/phy/utils/test/CMakeLists.txt  |   9 +
 lib/src/phy/utils/test/algebra_test.c  | 415 +++++++++++++++++++++++++
 4 files changed, 737 insertions(+), 40 deletions(-)
 create mode 100644 lib/src/phy/utils/test/algebra_test.c

diff --git a/lib/include/srslte/phy/utils/algebra.h b/lib/include/srslte/phy/utils/algebra.h
index 41d5e0025..8e731a29f 100644
--- a/lib/include/srslte/phy/utils/algebra.h
+++ b/lib/include/srslte/phy/utils/algebra.h
@@ -29,42 +29,127 @@
 
 #include "srslte/config.h"
 
-#ifdef LV_HAVE_SSE
+/*
+ * Generic Macros
+ */
+#define RANDOM_CF() (((float)rand())/((float)RAND_MAX) + _Complex_I*((float)rand())/((float)RAND_MAX))
 
-#define _MM_MULJ_PS(X) _mm_permute_ps(_MM_CONJ_PS(X), 0b10110001)
-#define _MM_CONJ_PS(X) (_mm_xor_ps(X, (__m128){0.0f, -0.0f, 0.0f, -0.0f}))
+/*
+ * SSE Macros
+ */
+#ifdef LV_HAVE_SSE
+#define _MM_SWAP(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,3,0,1)))
+#define _MM_PERM(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,1,3,0)))
+#define _MM_MULJ_PS(X) _MM_SWAP(_MM_CONJ_PS(X))
+#define _MM_CONJ_PS(X) (_mm_xor_ps(X, _mm_set_ps(-0.0f, 0.0f, -0.0f, 0.0f)))
+#define _MM_SQMOD_PS(X) _MM_PERM(_mm_hadd_ps(_mm_mul_ps(X,X), _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f)))
 #define _MM_PROD_PS(a, b) _mm_addsub_ps(_mm_mul_ps(a,_mm_moveldup_ps(b)),_mm_mul_ps(\
                             _mm_shuffle_ps(a,a,0xB1),_mm_movehdup_ps(b)))
 
-SRSLTE_API void srslte_algebra_2x2_zf_sse(__m128 y0,
-                                          __m128 y1,
-                                          __m128 h00,
-                                          __m128 h01,
-                                          __m128 h10,
-                                          __m128 h11,
-                                          __m128 *x0,
-                                          __m128 *x1,
-                                          float norm);
-
 #endif /* LV_HAVE_SSE */
 
+
+/*
+ * AVX Macros
+ */
 #ifdef LV_HAVE_AVX
 
 #define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
-#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, (__m256){0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f}))
+#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, _mm256_set_ps(-0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f)))
+
+#ifdef LV_HAVE_FMA
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+#define _MM256_PROD_PS(a, b) _mm256_fmaddsub_ps(a,_mm256_moveldup_ps(b),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#else
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_add_ps(_mm256_mul_ps(A,A), _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
 #define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
                               _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#endif /* LV_HAVE_FMA */
+#endif /* LV_HAVE_AVX */
+
+/*
+ * AVX extension with FMA Macros
+ */
+#ifdef LV_HAVE_FMA
+
+#define _MM256_SQMOD_ADD_PS(A, B, C) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_fmadd_ps(B, B, C)),\
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+
+#define _MM256_PROD_ADD_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmaddsub_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+
+#define _MM256_PROD_SUB_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmsubadd_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+#endif /* LV_HAVE_FMA */
+
+/* Generic implementation for complex reciprocal */
+SRSLTE_API cf_t srslte_algebra_cf_recip_gen(cf_t a);
+
+/* Generic implementation for 2x2 determinant */
+SRSLTE_API cf_t srslte_algebra_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11);
 
-SRSLTE_API void srslte_algebra_2x2_zf_avx(__m256 y0,
-                                          __m256 y1,
-                                          __m256 h00,
-                                          __m256 h01,
-                                          __m256 h10,
-                                          __m256 h11,
-                                          __m256 *x0,
-                                          __m256 *x1,
+/* Generic implementation for 2x2 Matrix Inversion */
+SRSLTE_API void srslte_algebra_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+                                           cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11);
+
+/* Generic implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_algebra_2x2_zf_gen(cf_t y0, cf_t y1,
+                                          cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                          cf_t *x0, cf_t *x1,
+                                          float norm);
+
+/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1,
+                                            cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                            cf_t *x0, cf_t *x1,
+                                            float noise_estimate,
+                                            float norm);
+
+#ifdef LV_HAVE_SSE
+
+/* SSE implementation for complex reciprocal */
+SRSLTE_API __m128 srslte_algebra_cf_recip_sse(__m128 a);
+
+/* SSE implementation for 2x2 determinant */
+SRSLTE_API __m128 srslte_algebra_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11);
+
+/* SSE implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_algebra_2x2_zf_sse(__m128 y0, __m128 y1,
+                                          __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                          __m128 *x0, __m128 *x1,
+                                          float norm);
+
+/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1,
+                                            __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                            __m128 *x0, __m128 *x1,
+                                            float noise_estimate, float norm);
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+/* AVX implementation for complex reciprocal */
+SRSLTE_API __m256 srslte_algebra_cf_recip_avx(__m256 a);
+
+/* AVX implementation for 2x2 determinant */
+SRSLTE_API __m256 srslte_algebra_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11);
+
+/* AVX implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1,
+                                          __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                          __m256 *x0, __m256 *x1,
                                           float norm);
 
+/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_algebra_2x2_mmse_avx(__m256 y0, __m256 y1,
+                                            __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                            __m256 *x0, __m256 *x1,
+                                            float noise_estimate, float norm);
+
 #endif /* LV_HAVE_AVX */
 
 #endif //SRSLTE_ALGEBRA_H
diff --git a/lib/src/phy/utils/algebra.c b/lib/src/phy/utils/algebra.c
index 69ac08749..3d1de2ffa 100644
--- a/lib/src/phy/utils/algebra.c
+++ b/lib/src/phy/utils/algebra.c
@@ -24,49 +24,237 @@
  *
  */
 
-
+#include <complex.h>
 #include <immintrin.h>
 
 #include "srslte/phy/utils/algebra.h"
 
+
+/* Generic implementation for complex reciprocal */
+inline cf_t srslte_algebra_cf_recip_gen(cf_t a) {
+  return conjf(a) / (crealf(a) * crealf(a) + cimagf(a) * cimagf(a));
+}
+
+/* Generic implementation for 2x2 determinant */
+inline cf_t srslte_algebra_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11) {
+  return a00 * a11 - a01 * a10;
+}
+
+/* 2x2 Matrix inversion, generic implementation */
+inline void srslte_algebra_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+                                       cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11) {
+  cf_t div = srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(a00, a01, a10, a11));
+  *r00 = a11 * div;
+  *r01 = -a01 * div;
+  *r10 = -a10 * div;
+  *r11 = a00 * div;
+}
+
+/* Generic implementation for Zero Forcing (ZF) solver */
+inline void srslte_algebra_2x2_zf_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                      cf_t *x0, cf_t *x1, float norm) {
+  cf_t _norm = srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(h00, h01, h10, h11)) * norm;
+  *x0 = (y0 * h11 - h01 * y1) * _norm;
+  *x1 = (y1 * h00 - h10 * y0) * _norm;
+}
+
+/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                        cf_t *x0, cf_t *x1, float noise_estimate, float norm) {
+  /* Create conjugated matrix */
+  cf_t _h00 = conjf(h00);
+  cf_t _h01 = conjf(h01);
+  cf_t _h10 = conjf(h10);
+  cf_t _h11 = conjf(h11);
+
+  /* 1. A = H' x H + No*/
+  cf_t a00 = _h00 * h00 + _h10 * h10 + noise_estimate;
+  cf_t a01 = _h00 * h01 + _h10 * h11;
+  cf_t a10 = _h01 * h00 + _h11 * h10;
+  cf_t a11 = _h01 * h01 + _h11 * h11 + noise_estimate;
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  cf_t b00 = a11;
+  cf_t b01 = -a01;
+  cf_t b10 = -a10;
+  cf_t b11 = a00;
+  cf_t _norm = norm * srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(a00, a01, a10, a11));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+  cf_t w00 = b00 * _h00 + b01 * _h01;
+  cf_t w01 = b00 * _h10 + b01 * _h11;
+  cf_t w10 = b10 * _h00 + b11 * _h01;
+  cf_t w11 = b10 * _h10 + b11 * _h11;
+
+  /* 4. X = W x Y */
+  *x0 = (y0 * w00 + y1 * w01) * _norm;
+  *x1 = (y0 * w10 + y1 * w11) * _norm;
+}
+
 #ifdef LV_HAVE_SSE
 
+/* SSE implementation for complex reciprocal */
+inline __m128 srslte_algebra_cf_recip_sse(__m128 a) {
+  __m128 conj = _MM_CONJ_PS(a);
+  __m128 sqabs = _mm_mul_ps(a, a);
+  sqabs = _mm_add_ps(_mm_movehdup_ps(sqabs), _mm_moveldup_ps(sqabs));
+
+  __m128 recp = _mm_rcp_ps(sqabs);
+
+  return _mm_mul_ps(recp, conj);
+}
+
+/* SSE implementation for 2x2 determinant */
+inline __m128 srslte_algebra_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11) {
+  return _mm_sub_ps(_MM_PROD_PS(a00, a11), _MM_PROD_PS(a01, a10));
+}
+
+/* SSE implementation for Zero Forcing (ZF) solver */
 inline void srslte_algebra_2x2_zf_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
                                       __m128 *x0, __m128 *x1, float norm) {
   __m128 detmult1 = _MM_PROD_PS(h00, h11);
   __m128 detmult2 = _MM_PROD_PS(h01, h10);
 
   __m128 det = _mm_sub_ps(detmult1, detmult2);
-  __m128 detconj = _MM_CONJ_PS(det);
-  __m128 detabs2 = _MM_PROD_PS(det, detconj);
-  __m128 detabs2rec = _mm_rcp_ps(detabs2);
-  detabs2rec = _mm_moveldup_ps(detabs2rec);
-  __m128 detrec = _mm_mul_ps(_mm_mul_ps(detconj, detabs2rec),
-                                (__m128) {norm, norm, norm, norm});
+  __m128 detrec = _mm_mul_ps(srslte_algebra_cf_recip_sse(det), _mm_set1_ps(norm));
 
   *x0 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h11, y0), _MM_PROD_PS(h01, y1)), detrec);
   *x1 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h00, y1), _MM_PROD_PS(h10, y0)), detrec);
 }
 
+/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                        __m128 *x0, __m128 *x1, float noise_estimate, float norm) {
+  __m128 _noise_estimate = _mm_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate);
+  __m128 _norm = _mm_set1_ps(norm);
+
+  /* Create conjugated matrix */
+  __m128 _h00 = _MM_CONJ_PS(h00);
+  __m128 _h01 = _MM_CONJ_PS(h01);
+  __m128 _h10 = _MM_CONJ_PS(h10);
+  __m128 _h11 = _MM_CONJ_PS(h11);
+
+  /* 1. A = H' x H + No*/
+  __m128 a00 = _mm_add_ps(_mm_add_ps(_MM_SQMOD_PS(h00), _MM_SQMOD_PS(h10)), _noise_estimate);
+  __m128 a01 = _mm_add_ps(_MM_PROD_PS(_h00, h01), _MM_PROD_PS(_h10, h11));
+  __m128 a10 = _mm_add_ps(_MM_PROD_PS(_h01, h00), _MM_PROD_PS(_h11, h10));
+  __m128 a11 = _mm_add_ps(_mm_add_ps(_MM_SQMOD_PS(h01), _MM_SQMOD_PS(h11)), _noise_estimate);
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  __m128 b00 = a11;
+  __m128 b01 = _mm_xor_ps(a01, _mm_set1_ps(-0.0f));
+  __m128 b10 = _mm_xor_ps(a10, _mm_set1_ps(-0.0f));
+  __m128 b11 = a00;
+  _norm = _mm_mul_ps(_norm, srslte_algebra_cf_recip_sse(srslte_algebra_2x2_det_sse(a00, a01, a10, a11)));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+  __m128 w00 = _mm_add_ps(_MM_PROD_PS(b00, _h00), _MM_PROD_PS(b01, _h01));
+  __m128 w01 = _mm_add_ps(_MM_PROD_PS(b00, _h10), _MM_PROD_PS(b01, _h11));
+  __m128 w10 = _mm_add_ps(_MM_PROD_PS(b10, _h00), _MM_PROD_PS(b11, _h01));
+  __m128 w11 = _mm_add_ps(_MM_PROD_PS(b10, _h10), _MM_PROD_PS(b11, _h11));
+
+  /* 4. X = W x Y */
+  *x0 = _MM_PROD_PS(_mm_add_ps(_MM_PROD_PS(y0, w00), _MM_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM_PROD_PS(_mm_add_ps(_MM_PROD_PS(y0, w10), _MM_PROD_PS(y1, w11)), _norm);
+}
+
 #endif /* LV_HAVE_SSE */
 
 #ifdef LV_HAVE_AVX
 
+/* AVX implementation for complex reciprocal */
+inline __m256 srslte_algebra_cf_recip_avx(__m256 a) {
+  __m256 conj = _MM256_CONJ_PS(a);
+  __m256 sqabs = _mm256_mul_ps(a, a);
+  sqabs = _mm256_add_ps(_mm256_movehdup_ps(sqabs), _mm256_moveldup_ps(sqabs));
+
+  __m256 recp = _mm256_rcp_ps(sqabs);
+
+  return _mm256_mul_ps(recp, conj);
+}
+
+/* AVX implementation for 2x2 determinant */
+inline __m256 srslte_algebra_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11) {
+#ifdef LV_HAVE_FMA
+  return _MM256_PROD_SUB_PS(a00, a11, _MM256_PROD_PS(a01, a10));
+#else
+  return _mm256_sub_ps(_MM256_PROD_PS(a00, a11), _MM256_PROD_PS(a01, a10));
+#endif /* LV_HAVE_FMA */
+}
+
+/* AVX implementation for Zero Forcing (ZF) solver */
 inline void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
-                                 __m256 *x0, __m256 *x1, float norm) {
-  __m256 detmult1 = _MM256_PROD_PS(h00, h11);
-  __m256 detmult2 = _MM256_PROD_PS(h01, h10);
-
-  __m256 det = _mm256_sub_ps(detmult1, detmult2);
-  __m256 detconj = _MM256_CONJ_PS(det);
-  __m256 sqdet = _mm256_mul_ps(det, det);
-  __m256 detabs2 = _mm256_add_ps(_mm256_movehdup_ps(sqdet), _mm256_moveldup_ps(sqdet));
-  __m256 detabs2rec = _mm256_rcp_ps(detabs2);
-  __m256 detrec = _mm256_mul_ps(_mm256_mul_ps(detconj, detabs2rec),
-                                (__m256) {norm, norm, norm, norm, norm, norm, norm, norm});
+                                      __m256 *x0, __m256 *x1, float norm) {
+
+  __m256 det = srslte_algebra_2x2_det_avx(h00, h01, h10, h11);
+  __m256 detrec = _mm256_mul_ps(srslte_algebra_cf_recip_avx(det), _mm256_set1_ps(norm));
 
+#ifdef LV_HAVE_FMA
+  *x0 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h11, y0, _MM256_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h00, y1, _MM256_PROD_PS(h10, y0)), detrec);
+#else
   *x0 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h11, y0), _MM256_PROD_PS(h01, y1)), detrec);
   *x1 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h00, y1), _MM256_PROD_PS(h10, y0)), detrec);
+#endif /* LV_HAVE_FMA */
+}
+
+/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_algebra_2x2_mmse_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                        __m256 *x0, __m256 *x1, float noise_estimate, float norm) {
+  __m256 _noise_estimate = _mm256_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate,
+                                         0.0f, noise_estimate, 0.0f, noise_estimate);
+  __m256 _norm = _mm256_set1_ps(norm);
+
+  /* Create conjugated matrix */
+  __m256 _h00 = _MM256_CONJ_PS(h00);
+  __m256 _h01 = _MM256_CONJ_PS(h01);
+  __m256 _h10 = _MM256_CONJ_PS(h10);
+  __m256 _h11 = _MM256_CONJ_PS(h11);
+
+  /* 1. A = H' x H + No*/
+#ifdef LV_HAVE_FMA
+  __m256 a00 = _MM256_SQMOD_ADD_PS(h00, h10, _noise_estimate);
+  __m256 a01 = _MM256_PROD_ADD_PS(_h00, h01, _MM256_PROD_PS(_h10, h11));
+  __m256 a10 = _MM256_PROD_ADD_PS(_h01, h00, _MM256_PROD_PS(_h11, h10));
+  __m256 a11 = _MM256_SQMOD_ADD_PS(h01, h11, _noise_estimate);
+#else
+  __m256 a00 = _mm256_add_ps(_MM256_SQMOD_PS(h00, h10), _noise_estimate);
+  __m256 a01 = _mm256_add_ps(_MM256_PROD_PS(_h00, h01), _MM256_PROD_PS(_h10, h11));
+  __m256 a10 = _mm256_add_ps(_MM256_PROD_PS(_h01, h00), _MM256_PROD_PS(_h11, h10));
+  __m256 a11 = _mm256_add_ps(_MM256_SQMOD_PS(h01, h11), _noise_estimate);
+#endif /* LV_HAVE_FMA */
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  __m256 b00 = a11;
+  __m256 b01 = _mm256_xor_ps(a01, _mm256_set1_ps(-0.0f));
+  __m256 b10 = _mm256_xor_ps(a10, _mm256_set1_ps(-0.0f));
+  __m256 b11 = a00;
+  _norm = _mm256_mul_ps(_norm, srslte_algebra_cf_recip_avx(srslte_algebra_2x2_det_avx(a00, a01, a10, a11)));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+#ifdef LV_HAVE_FMA
+  __m256 w00 = _MM256_PROD_ADD_PS(b00, _h00, _MM256_PROD_PS(b01, _h01));
+  __m256 w01 = _MM256_PROD_ADD_PS(b00, _h10, _MM256_PROD_PS(b01, _h11));
+  __m256 w10 = _MM256_PROD_ADD_PS(b10, _h00, _MM256_PROD_PS(b11, _h01));
+  __m256 w11 = _MM256_PROD_ADD_PS(b10, _h10, _MM256_PROD_PS(b11, _h11));
+#else
+  __m256 w00 = _mm256_add_ps(_MM256_PROD_PS(b00, _h00), _MM256_PROD_PS(b01, _h01));
+  __m256 w01 = _mm256_add_ps(_MM256_PROD_PS(b00, _h10), _MM256_PROD_PS(b01, _h11));
+  __m256 w10 = _mm256_add_ps(_MM256_PROD_PS(b10, _h00), _MM256_PROD_PS(b11, _h01));
+  __m256 w11 = _mm256_add_ps(_MM256_PROD_PS(b10, _h10), _MM256_PROD_PS(b11, _h11));
+#endif /* LV_HAVE_FMA */
+
+  /* 4. X = W x Y */
+#ifdef LV_HAVE_FMA
+  *x0 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w00, _MM256_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w10, _MM256_PROD_PS(y1, w11)), _norm);
+#else
+  *x0 = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w00), _MM256_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w10), _MM256_PROD_PS(y1, w11)), _norm);
+#endif /* LV_HAVE_FMA */
 }
 
 #endif /* LV_HAVE_AVX */
diff --git a/lib/src/phy/utils/test/CMakeLists.txt b/lib/src/phy/utils/test/CMakeLists.txt
index 494c20c16..1f516da8f 100644
--- a/lib/src/phy/utils/test/CMakeLists.txt
+++ b/lib/src/phy/utils/test/CMakeLists.txt
@@ -33,3 +33,12 @@ add_test(dft_dc dft_test -b -d)   # Backwards first & handle dc internally
 add_test(dft_odd dft_test -N 255) # Odd-length
 add_test(dft_odd_dc dft_test -N 255 -b -d) # Odd-length, backwards first, handle dc
 
+########################################################################
+# Algebra TEST
+########################################################################
+
+add_executable(algebra_test algebra_test.c)
+target_link_libraries(algebra_test srslte_phy)
+
+add_test(algebra_2x2_zf_solver_test algebra_test -z)
+add_test(algebra_2x2_mmse_solver_test algebra_test -m)
diff --git a/lib/src/phy/utils/test/algebra_test.c b/lib/src/phy/utils/test/algebra_test.c
new file mode 100644
index 000000000..a5bc7e773
--- /dev/null
+++ b/lib/src/phy/utils/test/algebra_test.c
@@ -0,0 +1,415 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <complex.h>
+#include <stdbool.h>
+#include <immintrin.h>
+#include <sys/time.h>
+
+#include "srslte/phy/utils/algebra.h"
+
+
+bool zf_solver = false;
+bool mmse_solver = false;
+bool verbose = false;
+
+double elapsed_us(struct timeval *ts_start, struct timeval *ts_end) {
+  if (ts_end->tv_usec > ts_start->tv_usec) {
+    return ((double) ts_end->tv_sec - (double) ts_start->tv_sec) * 1000000 +
+           (double) ts_end->tv_usec - (double) ts_start->tv_usec;
+  } else {
+    return ((double) ts_end->tv_sec - (double) ts_start->tv_sec - 1) * 1000000 +
+           ((double) ts_end->tv_usec + 1000000) - (double) ts_start->tv_usec;
+  }
+}
+
+#define NOF_REPETITIONS 1000
+#define RUN_TEST(FUNCTION) /*TYPE NAME (void)*/ { \
+  int i;\
+  struct timeval start, end;\
+  gettimeofday(&start, NULL); \
+  bool ret = true; \
+  for (i = 0; i < NOF_REPETITIONS; i++) {ret &= FUNCTION ();}\
+  gettimeofday(&end, NULL);\
+  if (verbose) printf("%32s: %s ... %6.2f us/call\n", #FUNCTION, (ret)?"Pass":"Fail", \
+                      elapsed_us(&start, &end)/NOF_REPETITIONS);\
+  passed &= ret;\
+}
+
+void usage(char *prog) {
+  printf("Usage: %s [mzvh]\n", prog);
+  printf("\t-m Test Minimum Mean Squared Error (MMSE) solver\n");
+  printf("\t-z Test Zero Forcing (ZF) solver\n");
+  printf("\t-v Verbose\n");
+  printf("\t-h Show this message\n");
+}
+
+void parse_args(int argc, char **argv) {
+  int opt;
+  while ((opt = getopt(argc, argv, "mzvh")) != -1) {
+    switch (opt) {
+      case 'm':
+        mmse_solver = true;
+        break;
+      case 'z':
+        zf_solver = true;
+        break;
+      case 'v':
+        verbose = true;
+        break;
+      case 'h':
+      default:
+        usage(argv[0]);
+        exit(-1);
+    }
+  }
+}
+
+bool test_zf_solver_gen(void) {
+  cf_t x0, x1, cf_error0, cf_error1;
+  float error;
+
+  cf_t x0_gold = RANDOM_CF();
+  cf_t x1_gold = RANDOM_CF();
+  cf_t h00 = RANDOM_CF();
+  cf_t h01 = RANDOM_CF();
+  cf_t h10 = RANDOM_CF();
+  cf_t h11 = (1 - h01 * h10) / h00;
+  cf_t y0 = x0_gold * h00 + x1_gold * h01;
+  cf_t y1 = x0_gold * h10 + x1_gold * h11;
+
+  srslte_algebra_2x2_zf_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 1.0f);
+
+  cf_error0 = x0 - x0_gold;
+  cf_error1 = x1 - x1_gold;
+  error = crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+          crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-6);
+}
+
+bool test_mmse_solver_gen(void) {
+  cf_t x0, x1, cf_error0, cf_error1;
+  float error;
+
+  cf_t x0_gold = RANDOM_CF();
+  cf_t x1_gold = RANDOM_CF();
+  cf_t h00 = RANDOM_CF();
+  cf_t h01 = RANDOM_CF();
+  cf_t h10 = RANDOM_CF();
+  cf_t h11 = (1 - h01 * h10) / h00;
+  cf_t y0 = x0_gold * h00 + x1_gold * h01;
+  cf_t y1 = x0_gold * h10 + x1_gold * h11;
+
+  srslte_algebra_2x2_mmse_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 0.0f, 1.0f);
+
+  cf_error0 = x0 - x0_gold;
+  cf_error1 = x1 - x1_gold;
+  error = crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+          crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-6);
+}
+
+#ifdef LV_HAVE_SSE
+
+bool test_zf_solver_sse(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m128 _y0 = _mm_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m128 _y1 = _mm_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m128 _h00 = _mm_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m128 _h01 = _mm_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m128 _h10 = _mm_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m128 _h11 = _mm_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m128 _x0, _x1;
+
+  srslte_algebra_2x2_zf_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+
+
+  __attribute__((aligned(128))) cf_t x0[2];
+  __attribute__((aligned(128))) cf_t x1[2];
+
+  _mm_store_ps((float *) x0, _x0);
+  _mm_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+bool test_mmse_solver_sse(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m128 _y0 = _mm_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m128 _y1 = _mm_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m128 _h00 = _mm_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m128 _h01 = _mm_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m128 _h10 = _mm_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m128 _h11 = _mm_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m128 _x0, _x1;
+
+  srslte_algebra_2x2_mmse_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+
+
+  __attribute__((aligned(128))) cf_t x0[2];
+  __attribute__((aligned(128))) cf_t x1[2];
+
+  _mm_store_ps((float *) x0, _x0);
+  _mm_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+bool test_zf_solver_avx(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m256 _y0 = _mm256_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2),
+                             cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m256 _y1 = _mm256_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2),
+                             cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m256 _h00 = _mm256_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2),
+                              cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m256 _h01 = _mm256_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2),
+                              cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m256 _h10 = _mm256_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2),
+                              cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m256 _h11 = _mm256_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2),
+                              cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m256 _x0, _x1;
+
+  srslte_algebra_2x2_zf_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+
+
+  __attribute__((aligned(256))) cf_t x0[4];
+  __attribute__((aligned(256))) cf_t x1[4];
+
+  _mm256_store_ps((float *) x0, _x0);
+  _mm256_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+bool test_mmse_solver_avx(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m256 _y0 = _mm256_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2),
+                             cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m256 _y1 = _mm256_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2),
+                             cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m256 _h00 = _mm256_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2),
+                              cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m256 _h01 = _mm256_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2),
+                              cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m256 _h10 = _mm256_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2),
+                              cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m256 _h11 = _mm256_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2),
+                              cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m256 _x0, _x1;
+
+  srslte_algebra_2x2_mmse_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+
+
+  __attribute__((aligned(256))) cf_t x0[4];
+  __attribute__((aligned(256))) cf_t x1[4];
+
+  _mm256_store_ps((float *) x0, _x0);
+  _mm256_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+#endif /* LV_HAVE_AVX */
+
+
+int main(int argc, char **argv) {
+  bool passed = true;
+  int ret = 0;
+
+  parse_args(argc, argv);
+
+  if (zf_solver) {
+    RUN_TEST(test_zf_solver_gen);
+
+#ifdef LV_HAVE_SSE
+    RUN_TEST(test_zf_solver_sse);
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+    RUN_TEST(test_zf_solver_avx);
+#endif /* LV_HAVE_AVX */
+  }
+
+  if (mmse_solver) {
+    RUN_TEST(test_mmse_solver_gen);
+
+#ifdef LV_HAVE_SSE
+    RUN_TEST(test_mmse_solver_sse);
+#endif /* LV_HAVE_SSE */
+
+
+#ifdef LV_HAVE_AVX
+    RUN_TEST(test_mmse_solver_avx);
+#endif /* LV_HAVE_AVX */
+  }
+
+  printf("%s!\n", (passed) ? "Ok" : "Failed");
+
+  if (!passed) {
+    exit(ret);
+  }
+
+  exit(ret);
+}

From a9d9c92205e6050367f0bea73b4242d9b76e5b48 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:17:48 +0200
Subject: [PATCH 069/269] Added MMSE in SSE and AVX plus subroutines refactor

---
 lib/include/srslte/phy/common/phy_common.h |    5 +
 lib/include/srslte/phy/mimo/precoding.h    |   14 +-
 lib/src/phy/mimo/precoding.c               | 1622 ++++++++++++++++----
 lib/src/phy/mimo/test/CMakeLists.txt       |   15 +-
 lib/src/phy/mimo/test/precoder_test.c      |   62 +-
 lib/src/phy/mimo/test/predecoder_mex.c     |   34 +-
 6 files changed, 1430 insertions(+), 322 deletions(-)

diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index e1c6f1b0d..9859bdfb0 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -175,6 +175,11 @@ typedef enum SRSLTE_API {
   SRSLTE_MIMO_TYPE_CDD
 } srslte_mimo_type_t;
 
+typedef enum SRSLTE_API {
+  SRSLTE_MIMO_DECODER_ZF,
+  SRSLTE_MIMO_DECODER_MMSE
+} srslte_mimo_decoder_t;
+
 typedef enum SRSLTE_API {
   SRSLTE_MOD_BPSK = 0, 
   SRSLTE_MOD_QPSK, 
diff --git a/lib/include/srslte/phy/mimo/precoding.h b/lib/include/srslte/phy/mimo/precoding.h
index 222c085b6..b32b975d2 100644
--- a/lib/include/srslte/phy/mimo/precoding.h
+++ b/lib/include/srslte/phy/mimo/precoding.h
@@ -107,6 +107,8 @@ SRSLTE_API int srslte_predecoding_type(cf_t *y,
                                        srslte_mimo_type_t type, 
                                        float noise_estimate);
 
+SRSLTE_API void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo_decoder);
+
 SRSLTE_API int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], 
                                              cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], 
                                              cf_t *x[SRSLTE_MAX_LAYERS],
@@ -118,11 +120,11 @@ SRSLTE_API int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS],
                                              srslte_mimo_type_t type, 
                                              float noise_estimate);
 
-int srslte_precoding_pmi_select (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                 uint32_t nof_symbols,
-                                 float noise_estimate,
-                                 int nof_layers,
-                                 uint32_t *pmi,
-                                 float sinr[SRSLTE_MAX_CODEBOOKS]);
+SRSLTE_API int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                           uint32_t nof_symbols,
+                                           float noise_estimate,
+                                           int nof_layers,
+                                           uint32_t *pmi,
+                                           float sinr[SRSLTE_MAX_CODEBOOKS]);
 
 #endif /* PRECODING_H_ */
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index ba950f196..37081d225 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -36,21 +36,20 @@
 #include "srslte/phy/utils/debug.h"
 
 #ifdef LV_HAVE_SSE
-#include <xmmintrin.h>
-#include <pmmintrin.h>
+#include <immintrin.h>
+#include "srslte/phy/utils/algebra.h"
 int srslte_predecoding_single_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 int srslte_predecoding_diversity2_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_symbols);
 #endif
 
 #ifdef LV_HAVE_AVX
 #include <immintrin.h>
-#include <srslte/srslte.h>
-#include <srslte/phy/utils/algebra.h>
-
+#include "srslte/phy/utils/algebra.h"
 int srslte_predecoding_single_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 #endif
 
 
+static srslte_mimo_decoder_t mimo_decoder = SRSLTE_MIMO_DECODER_MMSE;
 
 /************************************************
  * 
@@ -569,7 +568,7 @@ int srslte_precoding_mimo_2x2_gen(cf_t W[2][2], cf_t *y[SRSLTE_MAX_PORTS], cf_t
   return SRSLTE_SUCCESS; 
 }
 
-// SSE implementation of ZF 2x2 CCD equalizer
+// AVX implementation of ZF 2x2 CCD equalizer
 #ifdef LV_HAVE_AVX
 
 int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
@@ -578,7 +577,10 @@ int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
                                       uint32_t nof_symbols) {
   uint32_t i = 0;
 
-  for (i = 0; i < nof_symbols; i += 4) {
+  __m256 mask0 = _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f);
+  __m256 mask1 = _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f);
+
+  for (i = 0; i < nof_symbols - 3; i += 4) {
     /* Load channel */
     __m256 h00i = _mm256_load_ps((float *) &h[0][0][i]);
     __m256 h01i = _mm256_load_ps((float *) &h[0][1][i]);
@@ -586,14 +588,10 @@ int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
     __m256 h11i = _mm256_load_ps((float *) &h[1][1][i]);
 
     /* Apply precoding */
-    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i,
-                                                   (__m256) {+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f}));
-    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i,
-                                                   (__m256) {+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f}));
-    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i,
-                                                   (__m256) {-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f}));
-    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i,
-                                                   (__m256) {-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f}));
+    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, mask0));
+    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, mask0));
+    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, mask1));
+    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, mask1));
 
     __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
     __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
@@ -619,7 +617,7 @@ int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
                                       uint32_t nof_symbols) {
   uint32_t i = 0;
 
-  for (i = 0; i < nof_symbols; i += 2) {
+  for (i = 0; i < nof_symbols - 1; i += 2) {
     /* Load channel */
     __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
     __m128 h01i = _mm_load_ps((float *) &h[0][1][i]);
@@ -627,10 +625,10 @@ int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
     __m128 h11i = _mm_load_ps((float *) &h[1][1][i]);
 
     /* Apply precoding */
-    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, (__m128) {+0.0f, +0.0f, -0.0f, -0.0f}));
-    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, (__m128) {+0.0f, +0.0f, -0.0f, -0.0f}));
-    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
-    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, (__m128) {-0.0f, -0.0f, +0.0f, +0.0f}));
+    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
 
     __m128 y0 = _mm_load_ps((float *) &y[0][i]);
     __m128 y1 = _mm_load_ps((float *) &y[1][i]);
@@ -681,7 +679,7 @@ int srslte_predecoding_ccd_2x2_zf_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_
   return SRSLTE_SUCCESS;
 }
 
-int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], 
+int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
                               int nof_rxant, int nof_ports, int nof_layers, int nof_symbols) 
 {
   if (nof_ports == 2 && nof_rxant == 2) {
@@ -707,223 +705,137 @@ int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORT
   return SRSLTE_ERROR;
 }
 
-/* PMI Select for 1 layer */
-int srslte_precoding_pmi_select_1l (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
-                                    float noise_estimate, uint32_t *pmi,
-                                    float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
-
-#define SQRT1_2 ((float)M_SQRT1_2);
-  float max_sinr = 0.0;
-  uint32_t i, count;
-
-  for (i = 0; i < 4; i++) {
-    sinr_list[i] = 0;
-    count = 0;
-
-    for (uint32_t j = 0; j < nof_symbols; j += 100) {
-      /* 0. Load channel matrix */
-      cf_t h00 = h[0][0][j];
-      cf_t h01 = h[1][0][j];
-      cf_t h10 = h[0][1][j];
-      cf_t h11 = h[1][1][j];
+// AVX implementation of MMSE 2x2 CCD equalizer
+#ifdef LV_HAVE_AVX
 
-      /* 1. B = W'* H' */
-      cf_t a0, a1;
-      switch(i) {
-        case 0:
-          a0 = conjf(h00) + conjf(h01);
-          a1 = conjf(h10) + conjf(h11);
-          break;
-        case 1:
-          a0 = conjf(h00) - conjf(h01);
-          a1 = conjf(h10) - conjf(h11);
-          break;
-        case 2:
-          a0 = conjf(h00) - _Complex_I * conjf(h01);
-          a1 = conjf(h10) - _Complex_I * conjf(h11);
-          break;
-        case 3:
-          a0 = conjf(h00) + _Complex_I * conjf(h01);
-          a1 = conjf(h10) + _Complex_I * conjf(h11);
-          break;
-      }
-      a0 *= SQRT1_2;
-      a1 *= SQRT1_2;
+int srslte_predecoding_ccd_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS],
+                                        cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS],
+                                        uint32_t nof_symbols, float noise_estimate) {
+  uint32_t i = 0;
 
-      /* 2. B = W' * H' * H = A * H */
-      cf_t b0 = a0*h00 + a1*h10;
-      cf_t b1 = a0*h01 + a1*h11;
+  for (i = 0; i < nof_symbols - 3; i += 4) {
+    /* Load channel */
+    __m256 h00i = _mm256_load_ps((float *) &h[0][0][i]);
+    __m256 h01i = _mm256_load_ps((float *) &h[0][1][i]);
+    __m256 h10i = _mm256_load_ps((float *) &h[1][0][i]);
+    __m256 h11i = _mm256_load_ps((float *) &h[1][1][i]);
 
-      /* 3. C = W' * H' * H * W' = B * W */
-      cf_t c;
-      switch(i) {
-        case 0:
-          c = b0 + b1;
-          break;
-        case 1:
-          c = b0 - b1;
-          break;
-        case 2:
-          c = b0 + _Complex_I*b1;
-          break;
-        case 3:
-          c = b0 - _Complex_I*b1;
-          break;
-        default:
-          return SRSLTE_ERROR;
-      }
-      c *= SQRT1_2;
+    /* Apply precoding */
+    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f)));
+    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f)));
+    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f)));
+    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f)));
 
-      /* Add for averaging */
-      sinr_list[i] += crealf(c);
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
 
-      count ++;
-    }
+    __m256 x0, x1;
 
-    /* Divide average by noise */
-    sinr_list[i] /= noise_estimate*count;
+    srslte_algebra_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
 
-    if (sinr_list[i] > max_sinr) {
-      max_sinr = sinr_list[i];
-      *pmi = i;
-    }
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
   }
 
-  INFO("Precoder PMI Select for 1 layer SINR=[%.1fdB; %.1fdB; %.1fdB; %.1fdB] PMI=%d\n", 10*log10(sinr_list[0]), 10*log10(sinr_list[1]),
-       10*log10(sinr_list[2]), 10*log10(sinr_list[3]), *pmi);
-
-  return i;
+  return nof_symbols;
 }
+#endif /* LV_HAVE_AVX */
 
-/* PMI Select for 2 layers */
-int srslte_precoding_pmi_select_2l (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
-                                    float noise_estimate, uint32_t *pmi,
-                                    float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
-
-  float max_sinr = 0.0;
-  uint32_t i, count;
-
-  for (i = 0; i < 2; i++) {
-    sinr_list[i] = 0;
-    count = 0;
+// SSE implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_SSE
 
-    for (uint32_t j = 0; j < nof_symbols; j += 100) {
-      /* 0. Load channel matrix */
-      cf_t h00 = h[0][0][j];
-      cf_t h01 = h[1][0][j];
-      cf_t h10 = h[0][1][j];
-      cf_t h11 = h[1][1][j];
+int srslte_predecoding_ccd_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols, float noise_estimate) {
+  uint32_t i = 0;
 
-      /* 1. B = W'* H' */
-      cf_t a00, a01, a10, a11;
-      switch(i) {
-        case 0:
-          a00 = conjf(h00) + conjf(h01);
-          a01 = conjf(h10) + conjf(h11);
-          a10 = conjf(h00) - conjf(h01);
-          a11 = conjf(h10) - conjf(h11);
-          break;
-        case 1:
-          a00 = conjf(h00) - _Complex_I*conjf(h01);
-          a01 = conjf(h10) - _Complex_I*conjf(h11);
-          a10 = conjf(h00) + _Complex_I*conjf(h01);
-          a11 = conjf(h10) + _Complex_I*conjf(h11);
-          break;
-        default:
-          return SRSLTE_ERROR;
-      }
-      a00 *= 0.5f;
-      a01 *= 0.5f;
-      a10 *= 0.5f;
-      a11 *= 0.5f;
+  for (i = 0; i < nof_symbols - 1; i += 2) {
+    /* Load channel */
+    __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
+    __m128 h01i = _mm_load_ps((float *) &h[0][1][i]);
+    __m128 h10i = _mm_load_ps((float *) &h[1][0][i]);
+    __m128 h11i = _mm_load_ps((float *) &h[1][1][i]);
 
-      /* 2. B = W' * H' * H = A * H */
-      cf_t b00 = a00*h00 + a01*h10;
-      cf_t b01 = a00*h01 + a01*h11;
-      cf_t b10 = a10*h00 + a11*h10;
-      cf_t b11 = a10*h01 + a11*h11;
+    /* Apply precoding */
+    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
 
-      /* 3. C = W' * H' * H * W' = B * W */
-      cf_t c00, c01, c10, c11;
-      switch(i) {
-        case 0:
-          c00 = b00 + b01;
-          c01 = b00 - b01;
-          c10 = b10 + b11;
-          c11 = b10 - b11;
-          break;
-        case 1:
-          c00 = b00 + _Complex_I*b01;
-          c01 = b00 - _Complex_I*b01;
-          c10 = b10 + _Complex_I*b11;
-          c11 = b10 - _Complex_I*b11;
-          break;
-        default:
-          return SRSLTE_ERROR;
-      }
-      c00 *= 0.5;
-      c01 *= 0.5;
-      c10 *= 0.5;
-      c11 *= 0.5;
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
 
-      /* 4. C += noise * I */
-      c00 += noise_estimate;
-      c11 += noise_estimate;
+    __m128 x0, x1;
 
-      /* 5. detC */
-      cf_t detC = c00*c11 - c01*c10;
-      cf_t inv_detC = conjf(detC)/(crealf(detC)*crealf(detC) + cimagf(detC)*cimagf(detC));
+    srslte_algebra_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
 
-      cf_t den0 = noise_estimate*c00*inv_detC;
-      cf_t den1 = noise_estimate*c11*inv_detC;
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+  }
 
-      float gamma0 = crealf((conjf(den0)/(crealf(den0)*crealf(den0) + cimagf(den0)*cimagf(den0))) - 1);
-      float gamma1 = crealf((conjf(den1)/(crealf(den1)*crealf(den1) + cimagf(den1)*cimagf(den1))) - 1);
+  return nof_symbols;
+}
+#endif /* LV_HAVE_SSE */
 
-      /* Add for averaging */
-      sinr_list[i] += (gamma0 + gamma1);
+// Generic implementation of ZF 2x2 CCD equalizer
+int srslte_predecoding_ccd_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                      int nof_symbols, float noise_estimate) {
+  cf_t h00, h01, h10, h11;
+  for (int i = 0; i < nof_symbols; i++) {
 
-      count ++;
-    }
+    // Even precoder
+    h00 = +h[0][0][i] + h[1][0][i];
+    h10 = +h[0][1][i] + h[1][1][i];
+    h01 = +h[0][0][i] - h[1][0][i];
+    h11 = +h[0][1][i] - h[1][1][i];
+    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
 
-    /* Divide average by noise */
-    sinr_list[i] /= (2*count);
+    i++;
 
-    if (sinr_list[i] > max_sinr) {
-      max_sinr = sinr_list[i];
-      *pmi = i;
-    }
+    // Odd precoder
+    h00 = h[0][0][i] - h[1][0][i];
+    h10 = h[0][1][i] - h[1][1][i];
+    h01 = h[0][0][i] + h[1][0][i];
+    h11 = h[0][1][i] + h[1][1][i];
+    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
   }
-
-  INFO("Precoder PMI Select for 2 layers SINR=[%.1fdB; %.1fdB] PMI=%d\n", 10*log10(sinr_list[0]), 10*log10(sinr_list[1]), *pmi);
-
-  return i;
+  return SRSLTE_SUCCESS;
 }
 
-int srslte_precoding_pmi_select (cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
-                                 float noise_estimate, int nof_layers, uint32_t *pmi,
-                                 float sinr[SRSLTE_MAX_CODEBOOKS]) {
-  int ret;
 
-  if (sinr == NULL || pmi == NULL) {
-    ERROR("Null pointer");
-    ret = SRSLTE_ERROR_INVALID_INPUTS;
-  } else if (nof_layers == 1) {
-    ret = srslte_precoding_pmi_select_1l(h, nof_symbols, noise_estimate, pmi, sinr);
-  } else if (nof_layers == 2) {
-    ret = srslte_precoding_pmi_select_2l(h, nof_symbols, noise_estimate, pmi, sinr);
+int srslte_predecoding_ccd_mmse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                              int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, float noise_estimate)
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_ccd_2x2_mmse_avx(y, h, x, nof_symbols, noise_estimate);
+#else
+      #ifdef LV_HAVE_SSE
+      return srslte_predecoding_ccd_2x2_mmse_sse(y, h, x, nof_symbols, noise_estimate);
+#else
+      return srslte_predecoding_ccd_2x2_mmse_gen(y, h, x, nof_symbols, noise_estimate);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+    } else {
+      fprintf(stderr, "Error predecoding CCD: Invalid number of layers %d\n", nof_layers);
+      return -1;
+    }
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
   } else {
-    ERROR("Wrong number of layers");
-    ret = SRSLTE_ERROR_INVALID_INPUTS;
+    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
   }
-
-  return ret;
+  return SRSLTE_ERROR;
 }
 
+#ifdef LV_HAVE_AVX
+
 // Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
-int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+int srslte_predecoding_multiplex_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                         cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
-  int i = 0;
   float norm = 1.0;
 
   switch(codebook_idx) {
@@ -935,12 +847,11 @@ int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
       norm = 2.0f;
       break;
     default:
-      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
       return SRSLTE_ERROR;
   }
 
-#ifdef LV_HAVE_AVX
-  for (/* i = 0*/; i < nof_symbols; i += 4) {
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
     __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
     __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
     __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
@@ -982,13 +893,33 @@ int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
     _mm256_store_ps((float *) &x[1][i], x1);
 
   }
-  if (i > nof_symbols) {
-    i -= 4;
-  }
+
+  return SRSLTE_SUCCESS;
+}
+
 #endif /* LV_HAVE_AVX */
 
 #ifdef LV_HAVE_SSE
-  for (/* i = 0*/; i < nof_symbols; i += 2) {
+
+// SSE implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
     __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
     __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
     __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
@@ -1030,12 +961,32 @@ int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
     _mm_store_ps((float *) &x[1][i], x1);
 
   }
-  if (i > nof_symbols) {
-    i -= 2;
-  }
+
+  return SRSLTE_SUCCESS;
+}
+
 #endif /* LV_HAVE_SSE */
 
-  for (/*int i = 0*/; i < nof_symbols; i++) {
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i++) {
     cf_t h00, h01, h10, h11, det;
 
     switch(codebook_idx) {
@@ -1071,22 +1022,211 @@ int srslte_predecoding_multiplex_2x2_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
   return SRSLTE_SUCCESS;
 }
 
-// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
-int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
-  int i = 0;
-
 #ifdef LV_HAVE_AVX
-  for (/* i = 0*/; i < nof_symbols; i += 4) {
-      __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
-      __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
-      __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
-      __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
 
-    __m256 h0, h1;
-    switch (codebook_idx) {
-      case 0:
-        h0 = _mm256_add_ps(_h00, _h10);
+// AVX implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm256_add_ps(_h00, _h10);
+        h01 = _mm256_sub_ps(_h00, _h10);
+        h10 = _mm256_add_ps(_h01, _h11);
+        h11 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h01 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h10 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
+        h11 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_algebra_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_AVX */
+
+
+#ifdef LV_HAVE_SSE
+
+// SSE implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm_add_ps(_h00, _h10);
+        h01 = _mm_sub_ps(_h00, _h10);
+        h10 = _mm_add_ps(_h01, _h11);
+        h11 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h01 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h10 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
+        h11 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_algebra_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+#endif /* LV_HAVE_SSE */
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i++) {
+    cf_t h00, h01, h10, h11;
+
+    switch(codebook_idx) {
+      case 0:
+        h00 = h[0][0][i];
+        h01 = h[1][0][i];
+        h10 = h[0][1][i];
+        h11 = h[1][1][i];
+        break;
+      case 1:
+        h00 = h[0][0][i] + h[1][0][i];
+        h01 = h[0][0][i] - h[1][0][i];
+        h10 = h[0][1][i] + h[1][1][i];
+        h11 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h00 = h[0][0][i] + _Complex_I*h[1][0][i];
+        h01 = h[0][0][i] - _Complex_I*h[1][0][i];
+        h10 = h[0][1][i] + _Complex_I*h[1][1][i];
+        h11 = h[0][1][i] - _Complex_I*h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, norm);
+  }
+  return SRSLTE_SUCCESS;
+}
+
+#ifdef LV_HAVE_AVX
+
+// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+int srslte_predecoding_multiplex_2x1_mrc_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                             cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h0, h1;
+    switch (codebook_idx) {
+      case 0:
+        h0 = _mm256_add_ps(_h00, _h10);
         h1 = _mm256_add_ps(_h01, _h11);
         break;
       case 1:
@@ -1094,12 +1234,12 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
         h1 = _mm256_sub_ps(_h01, _h11);
         break;
       case 2:
-        h0 = _mm256_add_ps(_h00, _mm256_permute_ps(_MM256_CONJ_PS(_h10), 0b10110001));
-        h1 = _mm256_add_ps(_h01, _mm256_permute_ps(_MM256_CONJ_PS(_h11), 0b10110001));
+        h0 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h1 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
         break;
       case 3:
-        h0 = _mm256_sub_ps(_h00, _mm256_permute_ps(_MM256_CONJ_PS(_h10), 0b10110001));
-        h1 = _mm256_sub_ps(_h01, _mm256_permute_ps(_MM256_CONJ_PS(_h11), 0b10110001));
+        h0 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h1 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
         break;
       default:
         fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
@@ -1113,8 +1253,7 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
     __m256 hh = _mm256_add_ps(hh0, hh1);
     __m256 hhrec = _mm256_rcp_ps(hh);
 
-    hhrec = _mm256_mul_ps(hhrec, (__m256){(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2,
-                                          (float) M_SQRT2,(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2});
+    hhrec = _mm256_mul_ps(hhrec, _mm256_set1_ps((float) M_SQRT2));
     __m256 y0 = _mm256_load_ps((float*)&y[0][i]);
     __m256 y1 = _mm256_load_ps((float*)&y[1][i]);
 
@@ -1124,13 +1263,20 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
     _mm256_store_ps((float*)&x[0][i], x0);
 
   }
-  if (i > nof_symbols) {
-    i -= 4;
-  }
+
+  return SRSLTE_SUCCESS;
+}
+
 #endif /* LV_HAVE_AVX */
 
+
+// SSE  implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
 #ifdef LV_HAVE_SSE
-  for (/* i = 0*/; i < nof_symbols; i += 2) {
+
+int srslte_predecoding_multiplex_2x1_mrc_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                             cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
     __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
     __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
     __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
@@ -1147,12 +1293,12 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
         h1 = _mm_sub_ps(_h01, _h11);
         break;
       case 2:
-        h0 = _mm_add_ps(_h00, _mm_permute_ps(_MM_CONJ_PS(_h10), 0b10110001));
-        h1 = _mm_add_ps(_h01, _mm_permute_ps(_MM_CONJ_PS(_h11), 0b10110001));
+        h0 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h1 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
         break;
       case 3:
-        h0 = _mm_sub_ps(_h00, _mm_permute_ps(_MM_CONJ_PS(_h10), 0b10110001));
-        h1 = _mm_sub_ps(_h01, _mm_permute_ps(_MM_CONJ_PS(_h11), 0b10110001));
+        h0 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h1 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
         break;
       default:
         fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
@@ -1166,7 +1312,7 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
     __m128 hh = _mm_add_ps(hh0, hh1);
     __m128 hhrec = _mm_rcp_ps(hh);
 
-    hhrec = _mm_mul_ps(hhrec, (__m128){(float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2, (float) M_SQRT2});
+    hhrec = _mm_mul_ps(hhrec, _mm_set1_ps((float) M_SQRT2));
 
     __m128 y0 = _mm_load_ps((float*)&y[0][i]);
     __m128 y1 = _mm_load_ps((float*)&y[1][i]);
@@ -1177,12 +1323,16 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
     _mm_store_ps((float*)&x[0][i], x0);
 
   }
-  if (i > nof_symbols) {
-    i -= 2;
-  }
+
+  return SRSLTE_SUCCESS;
+}
+
 #endif /* LV_HAVE_SSE */
 
-  for (/*i = 0*/; i < nof_symbols; i += 1) {
+// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+int srslte_predecoding_multiplex_2x1_mrc_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  for (int i = 0; i < nof_symbols; i += 1) {
     cf_t h0, h1;
     float hh;
 
@@ -1215,23 +1365,59 @@ int srslte_predecoding_multiplex_2x1_mrc(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSL
   return SRSLTE_SUCCESS;
 }
 
-int srslte_predecoding_multiplex_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
-                              int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols)
+int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                 int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols,
+                                 float noise_estimate)
 {
   if (nof_ports == 2 && nof_rxant == 2) {
     if (nof_layers == 2) {
-      return srslte_predecoding_multiplex_2x2_zf(y, h, x, codebook_idx, nof_symbols);
+      switch (mimo_decoder) {
+        case SRSLTE_MIMO_DECODER_ZF:
+#ifdef LV_HAVE_AVX
+          return srslte_predecoding_multiplex_2x2_zf_avx(y, h, x, codebook_idx, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+          return srslte_predecoding_multiplex_2x2_zf_sse(y, h, x, codebook_idx, nof_symbols);
+#else
+          return srslte_predecoding_multiplex_2x2_zf_gen(y, h, x, codebook_idx, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+          break;
+        case SRSLTE_MIMO_DECODER_MMSE:
+#ifdef LV_HAVE_AVX
+          return srslte_predecoding_multiplex_2x2_mmse_avx(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#else
+#ifdef LV_HAVE_SSE
+          return srslte_predecoding_multiplex_2x2_mmse_sse(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#else
+          return srslte_predecoding_multiplex_2x2_mmse_gen(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+          break;
+      }
     } else {
-      return srslte_predecoding_multiplex_2x1_mrc(y, h, x, codebook_idx, nof_symbols);
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_multiplex_2x1_mrc_avx(y, h, x, codebook_idx, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+      return srslte_predecoding_multiplex_2x1_mrc_sse(y, h, x, codebook_idx, nof_symbols);
+#else
+      return srslte_predecoding_multiplex_2x1_mrc_gen(y, h, x, codebook_idx, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
     }
   } else if (nof_ports == 4) {
-    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
+    ERROR("Error predecoding CCD: Only 2 ports supported");
   } else {
     fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
   }
   return SRSLTE_ERROR;
 }
 
+void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo_decoder) {
+  mimo_decoder = _mimo_decoder;
+}
+
 /* 36.211 v10.3.0 Section 6.3.4 */
 int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                   cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_ports, int nof_layers,
@@ -1251,7 +1437,14 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
   switch (type) {
   case SRSLTE_MIMO_TYPE_CDD:
     if (nof_layers >= 2 && nof_layers <= 4) {
-      return srslte_predecoding_ccd_zf(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols);
+      switch (mimo_decoder) {
+        case SRSLTE_MIMO_DECODER_ZF:
+          return srslte_predecoding_ccd_zf(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols);
+          break;
+        case SRSLTE_MIMO_DECODER_MMSE:
+          return srslte_predecoding_ccd_mmse(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, noise_estimate);
+          break;
+      }
     } else {
       fprintf(stderr,
           "Invalid number of layers %d\n", nof_layers);
@@ -1277,7 +1470,8 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
     }
     break;
   case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-    return srslte_predecoding_multiplex_zf(y, h, x, nof_rxant, nof_ports, nof_layers, codebook_idx, nof_symbols);
+    return srslte_predecoding_multiplex(y, h, x, nof_rxant, nof_ports, nof_layers, codebook_idx, nof_symbols,
+                                        noise_estimate);
     default:
       return SRSLTE_ERROR;
   }
@@ -1344,39 +1538,102 @@ int srslte_precoding_diversity(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PO
   }
 }
 
-int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports, int nof_symbols) 
+#ifdef LV_HAVE_AVX
+
+int srslte_precoding_cdd_2x2_avx(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
 {
-  int i;
-  if (nof_ports == 2) {
-    if (nof_layers != 2) {
-      fprintf(stderr, "Invalid number of layers %d for 2 ports\n", nof_layers);
-      return -1; 
-    }
-    for (i = 0; i < nof_symbols; i++) {
-      y[0][i] =  (x[0][i]+x[1][i])/2;
-      y[1][i] =  (x[0][i]-x[1][i])/2;
-      i++;
-      y[0][i] =  (x[0][i]+x[1][i])/2;
-      y[1][i] = (-x[0][i]+x[1][i])/2;
-    }
-    return 2 * i;
-  } else if (nof_ports == 4) {
-    fprintf(stderr, "Not implemented\n");
-    return -1;
-  } else {
-    fprintf(stderr, "Number of ports must be 2 or 4 for transmit diversity (nof_ports=%d)\n", nof_ports);
-    return -1;
+  __m256 norm_avx = _mm256_set1_ps(0.5f);
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
+    __m256 x0 = _mm256_load_ps((float*) &x[0][i]);
+    __m256 x1 = _mm256_load_ps((float*) &x[1][i]);
+
+    __m256 y0 = _mm256_mul_ps(norm_avx, _mm256_add_ps(x0, x1));
+
+    x0 = _mm256_xor_ps(x0, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f));
+    x1 = _mm256_xor_ps(x1, _mm256_set_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f));
+
+    __m256 y1 = _mm256_mul_ps(norm_avx, _mm256_add_ps(x0, x1));
+
+    _mm256_store_ps((float*)&y[0][i], y0);
+    _mm256_store_ps((float*)&y[1][i], y1);
   }
+
+  return 2*nof_symbols;
 }
 
-int srslte_precoding_multiplex(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports,
-                               int codebook_idx, uint32_t nof_symbols)
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+
+int srslte_precoding_cdd_2x2_sse(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
 {
-  int i;
-  if (nof_ports == 2) {
-    if (nof_layers == 1) {
-      switch(codebook_idx) {
-        case 0:
+  __m128 norm_sse = _mm_set1_ps(0.5f);
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 x0 = _mm_load_ps((float*) &x[0][i]);
+    __m128 x1 = _mm_load_ps((float*) &x[1][i]);
+
+    __m128 y0 = _mm_mul_ps(norm_sse, _mm_add_ps(x0, x1));
+
+    x0 = _mm_xor_ps(x0, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f));
+    x1 = _mm_xor_ps(x1, _mm_set_ps(+0.0f, +0.0f, -0.0f, -0.0f));
+
+    __m128 y1 = _mm_mul_ps(norm_sse, _mm_add_ps(x0, x1));
+
+    _mm_store_ps((float*)&y[0][i], y0);
+    _mm_store_ps((float*)&y[1][i], y1);
+  }
+
+  return 2 * nof_symbols;
+}
+
+#endif /* LV_HAVE_SSE */
+
+
+int srslte_precoding_cdd_2x2_gen(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
+{
+  for (int i = 0; i < nof_symbols; i++) {
+    y[0][i] =  (x[0][i]+x[1][i])/2.0f;
+    y[1][i] =  (x[0][i]-x[1][i])/2.0f;
+    i++;
+    y[0][i] =  (x[0][i]+x[1][i])/2.0f;
+    y[1][i] = (-x[0][i]+x[1][i])/2.0f;
+  }
+  return 2 * nof_symbols;
+}
+
+int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports, int nof_symbols)
+{
+  if (nof_ports == 2) {
+    if (nof_layers != 2) {
+      fprintf(stderr, "Invalid number of layers %d for 2 ports\n", nof_layers);
+      return -1;
+    }
+#ifdef LV_HAVE_AVX
+    return srslte_precoding_cdd_2x2_avx(x, y, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+    return srslte_precoding_cdd_2x2_sse(x, y, nof_symbols);
+#else
+    return srslte_precoding_cdd_2x2_gen(x, y, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Not implemented\n");
+    return -1;
+  } else {
+    fprintf(stderr, "Number of ports must be 2 or 4 for transmit diversity (nof_ports=%d)\n", nof_ports);
+    return -1;
+  }
+}
+
+int srslte_precoding_multiplex(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports,
+                               int codebook_idx, uint32_t nof_symbols)
+{
+  int i = 0;
+  if (nof_ports == 2) {
+    if (nof_layers == 1) {
+      switch(codebook_idx) {
+        case 0:
           srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
           srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
           break;
@@ -1404,13 +1661,65 @@ int srslte_precoding_multiplex(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PO
           srslte_vec_sc_prod_cfc(x[1], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
           break;
         case 1:
-          for (i = 0; i < nof_symbols; i++) {
+#ifdef LV_HAVE_AVX
+          for (; i < nof_symbols - 3; i += 4) {
+            __m256 x0 = _mm256_load_ps((float*)&x[0][i]);
+            __m256 x1 = _mm256_load_ps((float*)&x[1][i]);
+
+            __m256 y0 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_add_ps(x0, x1));
+            __m256 y1 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_sub_ps(x0, x1));
+
+            _mm256_store_ps((float*)&y[0][i], y0);
+            _mm256_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+          for (; i < nof_symbols - 1; i += 2) {
+            __m128 x0 = _mm_load_ps((float*)&x[0][i]);
+            __m128 x1 = _mm_load_ps((float*)&x[1][i]);
+
+            __m128 y0 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_add_ps(x0, x1));
+            __m128 y1 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_sub_ps(x0, x1));
+
+            _mm_store_ps((float*)&y[0][i], y0);
+            _mm_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_SSE */
+
+          for (; i < nof_symbols; i++) {
             y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
             y[1][i] = 0.5f*x[0][i] - 0.5f*x[1][i];
           }
           break;
         case 2:
-          for (i = 0; i < nof_symbols; i++) {
+#ifdef LV_HAVE_AVX
+          for (; i < nof_symbols - 3; i += 4) {
+            __m256 x0 = _mm256_load_ps((float*)&x[0][i]);
+            __m256 x1 = _mm256_load_ps((float*)&x[1][i]);
+
+            __m256 y0 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_add_ps(x0, x1));
+            __m256 y1 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _MM256_MULJ_PS(_mm256_sub_ps(x0, x1)));
+
+            _mm256_store_ps((float*)&y[0][i], y0);
+            _mm256_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+          for (; i < nof_symbols - 1; i += 2) {
+            __m128 x0 = _mm_load_ps((float*)&x[0][i]);
+            __m128 x1 = _mm_load_ps((float*)&x[1][i]);
+
+            __m128 y0 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_add_ps(x0, x1));
+            __m128 y1 = _mm_mul_ps(_mm_set1_ps(0.5f), _MM_MULJ_PS(_mm_sub_ps(x0, x1)));
+
+            _mm_store_ps((float*)&y[0][i], y0);
+            _mm_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_SSE */
+
+          for (; i < nof_symbols; i++) {
             y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
             y[1][i] = 0.5f*_Complex_I*x[0][i] - 0.5f*_Complex_I*x[1][i];
           }
@@ -1473,3 +1782,744 @@ int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS],
   return SRSLTE_ERROR;
 }
 
+#define PMI_SEL_PRECISION 24
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+#define SQRT1_2 ((float)M_SQRT1_2)
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += PMI_SEL_PRECISION) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a0, a1;
+      switch (i) {
+        case 0:
+          a0 = conjf(h00) + conjf(h01);
+          a1 = conjf(h10) + conjf(h11);
+          break;
+        case 1:
+          a0 = conjf(h00) - conjf(h01);
+          a1 = conjf(h10) - conjf(h11);
+          break;
+        case 2:
+          a0 = conjf(h00) - _Complex_I * conjf(h01);
+          a1 = conjf(h10) - _Complex_I * conjf(h11);
+          break;
+        case 3:
+          a0 = conjf(h00) + _Complex_I * conjf(h01);
+          a1 = conjf(h10) + _Complex_I * conjf(h11);
+          break;
+      }
+      a0 *= SQRT1_2;
+      a1 *= SQRT1_2;
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b0 = a0 * h00 + a1 * h10;
+      cf_t b1 = a0 * h01 + a1 * h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c;
+      switch (i) {
+        case 0:
+          c = b0 + b1;
+          break;
+        case 1:
+          c = b0 - b1;
+          break;
+        case 2:
+          c = b0 + _Complex_I * b1;
+          break;
+        case 3:
+          c = b0 - _Complex_I * b1;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c *= SQRT1_2;
+
+      /* Add for averaging */
+      sinr_list[i] += crealf(c);
+
+      count++;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#ifdef LV_HAVE_SSE
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  float max_sinr = 0.0;
+  uint32_t i, count;
+  __m128 sse_norm = _mm_set1_ps(0.5f);
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 2 + 1; j += PMI_SEL_PRECISION * 2) {
+      /* 0. Load channel matrix */
+      __m128 h00 = _mm_set_ps(crealf(h[0][0][j]),
+                              cimagf(h[0][0][j]),
+                              crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][0][j + PMI_SEL_PRECISION]));
+      __m128 h01 = _mm_set_ps(crealf(h[1][0][j]),
+                              cimagf(h[1][0][j]),
+                              crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][0][j + PMI_SEL_PRECISION]));
+      __m128 h10 = _mm_set_ps(crealf(h[0][1][j]),
+                              cimagf(h[0][1][j]),
+                              crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][1][j + PMI_SEL_PRECISION]));
+      __m128 h11 = _mm_set_ps(crealf(h[1][1][j]),
+                              cimagf(h[1][1][j]),
+                              crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][1][j + PMI_SEL_PRECISION]));
+
+      /* 1. B = W'* H' */
+      __m128 a0, a1;
+      switch (i) {
+        case 0:
+          a0 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a1 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 1:
+          a0 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a1 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 2:
+          a0 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a1 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+        case 3:
+          a0 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a1 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      __m128 b0 = _mm_add_ps(_MM_PROD_PS(a0, h00), _MM_PROD_PS(a1, h10));
+      __m128 b1 = _mm_add_ps(_MM_PROD_PS(a0, h01), _MM_PROD_PS(a1, h11));
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m128 c;
+      switch (i) {
+        case 0:
+          c = _mm_add_ps(b0, b1);
+          break;
+        case 1:
+          c = _mm_sub_ps(b0, b1);
+          break;
+        case 2:
+          c = _mm_sub_ps(b0, _MM_MULJ_PS(b1));
+          break;
+        case 3:
+          c = _mm_add_ps(b0, _MM_MULJ_PS(b1));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c = _mm_mul_ps(c, sse_norm);
+
+      /* Add for averaging */
+      __attribute__((aligned(128))) float gamma[4];
+      _mm_store_ps(gamma, c);
+      sinr_list[i] += gamma[0] + gamma[2];
+
+      count += 2;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  float max_sinr = 0.0;
+  uint32_t i, count;
+  __m256 avx_norm = _mm256_set1_ps(0.5f);
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
+      /* 0. Load channel matrix */
+      __m256 h00 = _mm256_set_ps(crealf(h[0][0][j]),
+                                 cimagf(h[0][0][j]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h01 = _mm256_set_ps(crealf(h[1][0][j]),
+                                 cimagf(h[1][0][j]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h10 = _mm256_set_ps(crealf(h[0][1][j]),
+                                 cimagf(h[0][1][j]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
+      __m256 h11 = _mm256_set_ps(crealf(h[1][1][j]),
+                                 cimagf(h[1][1][j]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 3]));
+
+      /* 1. B = W'* H' */
+      __m256 a0, a1;
+      switch (i) {
+        case 0:
+          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 1:
+          a0 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 2:
+          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+        default:
+          a0 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+#ifdef  LV_HAVE_FMA
+      __m256 b0 = _MM256_PROD_ADD_PS(a0, h00, _MM256_PROD_PS(a1, h10));
+      __m256 b1 = _MM256_PROD_ADD_PS(a0, h01, _MM256_PROD_PS(a1, h11));
+#else
+      __m256 b0 = _mm256_add_ps(_MM256_PROD_PS(a0, h00), _MM256_PROD_PS(a1, h10));
+      __m256 b1 = _mm256_add_ps(_MM256_PROD_PS(a0, h01), _MM256_PROD_PS(a1, h11));
+#endif /* LV_HAVE_FMA */
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m256 c;
+      switch (i) {
+        case 0:
+          c = _mm256_add_ps(b0, b1);
+          break;
+        case 1:
+          c = _mm256_sub_ps(b0, b1);
+          break;
+        case 2:
+          c = _mm256_sub_ps(b0, _MM256_MULJ_PS(b1));
+          break;
+        case 3:
+          c = _mm256_add_ps(b0, _MM256_MULJ_PS(b1));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c = _mm256_mul_ps(c, avx_norm);
+
+      /* Add for averaging */
+      __attribute__((aligned(256))) float gamma[8];
+      _mm256_store_ps(gamma, c);
+      sinr_list[i] += gamma[0] + gamma[2] + gamma[4] + gamma[6];
+
+      count += 4;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_AVX */
+
+int srslte_precoding_pmi_select_1l(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                   float noise_estimate, uint32_t *pmi,
+                                   float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  int ret;
+#ifdef LV_HAVE_AVX
+  ret = srslte_precoding_pmi_select_1l_avx(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  #ifdef LV_HAVE_SSE
+  ret = srslte_precoding_pmi_select_1l_sse(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  ret = srslte_precoding_pmi_select_1l_gen(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#endif
+#endif
+  INFO("Precoder PMI Select for 1 layer SINR=[%.1fdB; %.1fdB; %.1fdB; %.1fdB] PMI=%d\n", 10 * log10(sinr_list[0]),
+       10 * log10(sinr_list[1]), 10 * log10(sinr_list[2]), 10 * log10(sinr_list[3]), *pmi);
+
+  return ret;
+}
+
+int srslte_precoding_pmi_select_2l_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += PMI_SEL_PRECISION) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = conjf(h00) + conjf(h01);
+          a01 = conjf(h10) + conjf(h11);
+          a10 = conjf(h00) - conjf(h01);
+          a11 = conjf(h10) - conjf(h11);
+          break;
+        case 1:
+          a00 = conjf(h00) - _Complex_I * conjf(h01);
+          a01 = conjf(h10) - _Complex_I * conjf(h11);
+          a10 = conjf(h00) + _Complex_I * conjf(h01);
+          a11 = conjf(h10) + _Complex_I * conjf(h11);
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b00 = a00 * h00 + a01 * h10;
+      cf_t b01 = a00 * h01 + a01 * h11;
+      cf_t b10 = a10 * h00 + a11 * h10;
+      cf_t b11 = a10 * h01 + a11 * h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = b00 + b01;
+          c01 = b00 - b01;
+          c10 = b10 + b11;
+          c11 = b10 - b11;
+          break;
+        case 1:
+          c00 = b00 + _Complex_I * b01;
+          c01 = b00 - _Complex_I * b01;
+          c10 = b10 + _Complex_I * b11;
+          c11 = b10 - _Complex_I * b11;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 *= 0.25;
+      c01 *= 0.25;
+      c10 *= 0.25;
+      c11 *= 0.25;
+
+      /* 4. C += noise * I */
+      c00 += noise_estimate;
+      c11 += noise_estimate;
+
+      /* 5. detC */
+      cf_t detC = c00 * c11 - c01 * c10;
+      cf_t inv_detC = conjf(detC) / (crealf(detC) * crealf(detC) + cimagf(detC) * cimagf(detC));
+
+      cf_t den0 = noise_estimate * c00 * inv_detC;
+      cf_t den1 = noise_estimate * c11 * inv_detC;
+
+      float gamma0 = crealf((conjf(den0) / (crealf(den0) * crealf(den0) + cimagf(den0) * cimagf(den0))) - 1);
+      float gamma1 = crealf((conjf(den1) / (crealf(den1) * crealf(den1) + cimagf(den1) * cimagf(den1))) - 1);
+
+      /* Add for averaging */
+      sinr_list[i] += (gamma0 + gamma1);
+
+      count++;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= (2 * count);
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#ifdef LV_HAVE_SSE
+
+int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  __m128 sse_noise_estimate = (__m128) {noise_estimate, 0.0f, noise_estimate, 0.0f};
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 2 + 1; j += PMI_SEL_PRECISION * 2) {
+      /* 0. Load channel matrix */
+      __m128 h00 = _mm_set_ps(crealf(h[0][0][j]),
+                              cimagf(h[0][0][j]),
+                              crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][0][j + PMI_SEL_PRECISION]));
+      __m128 h01 = _mm_set_ps(crealf(h[1][0][j]),
+                              cimagf(h[1][0][j]),
+                              crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][0][j + PMI_SEL_PRECISION]));
+      __m128 h10 = _mm_set_ps(crealf(h[0][1][j]),
+                              cimagf(h[0][1][j]),
+                              crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][1][j + PMI_SEL_PRECISION]));
+      __m128 h11 = _mm_set_ps(crealf(h[1][1][j]),
+                              cimagf(h[1][1][j]),
+                              crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][1][j + PMI_SEL_PRECISION]));
+
+      /* 1. B = W'* H' */
+      __m128 a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a01 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          a10 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a11 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 1:
+          a00 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a01 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          a10 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a11 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      __m128 b00 = _mm_add_ps(_MM_PROD_PS(a00, h00), _MM_PROD_PS(a01, h10));
+      __m128 b01 = _mm_add_ps(_MM_PROD_PS(a00, h01), _MM_PROD_PS(a01, h11));
+      __m128 b10 = _mm_add_ps(_MM_PROD_PS(a10, h00), _MM_PROD_PS(a11, h10));
+      __m128 b11 = _mm_add_ps(_MM_PROD_PS(a10, h01), _MM_PROD_PS(a11, h11));
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m128 c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = _mm_add_ps(b00, b01);
+          c01 = _mm_sub_ps(b00, b01);
+          c10 = _mm_add_ps(b10, b11);
+          c11 = _mm_sub_ps(b10, b11);
+          break;
+        case 1:
+          c00 = _mm_add_ps(b00, _MM_MULJ_PS(b01));
+          c01 = _mm_sub_ps(b00, _MM_MULJ_PS(b01));
+          c10 = _mm_add_ps(b10, _MM_MULJ_PS(b11));
+          c11 = _mm_sub_ps(b10, _MM_MULJ_PS(b11));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 = _mm_mul_ps(c00, _mm_set1_ps(0.25f));
+      c01 = _mm_mul_ps(c01, _mm_set1_ps(0.25f));
+      c10 = _mm_mul_ps(c10, _mm_set1_ps(0.25f));
+      c11 = _mm_mul_ps(c11, _mm_set1_ps(0.25f));
+
+      /* 4. C += noise * I */
+      c00 = _mm_add_ps(c00, sse_noise_estimate);
+      c11 = _mm_add_ps(c11, sse_noise_estimate);
+
+      /* 5. detC */
+      __m128 detC = _mm_sub_ps(_MM_PROD_PS(c00, c11), _MM_PROD_PS(c01, c10));
+      __m128 inv_detC = srslte_algebra_cf_recip_sse(detC);
+
+      __m128 den0 = _MM_PROD_PS(c00, inv_detC);
+      __m128 den1 = _MM_PROD_PS(c11, inv_detC);
+
+      __m128 gamma0 = _mm_sub_ps(srslte_algebra_cf_recip_sse(den0), _mm_set1_ps(1.0f));
+      __m128 gamma1 = _mm_sub_ps(srslte_algebra_cf_recip_sse(den1), _mm_set1_ps(1.0f));
+
+      /* Add for averaging */
+      __m128 sum = _MM_SWAP(_mm_add_ps(gamma0, gamma1));
+      __m128 sinr_sse = _mm_hadd_ps(sum, sum);
+      __attribute__((aligned(128))) float sinr[4];
+      _mm_store_ps(sinr, sinr_sse);
+
+      sinr_list[i] += sinr[0];
+
+      count += 2;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= (2 * count * noise_estimate);
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  __m256 avx_noise_estimate = _mm256_setr_ps(noise_estimate, 0.0f, noise_estimate, 0.0f,
+                                        noise_estimate, 0.0f, noise_estimate, 0.0f);
+  __m256 avx_norm = _mm256_set1_ps(0.5f);
+  __m256 avx_ones = _mm256_set1_ps(1.0f);
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
+      /* 0. Load channel matrix */
+      __m256 h00 = _mm256_set_ps(crealf(h[0][0][j]),
+                                 cimagf(h[0][0][j]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h01 = _mm256_set_ps(crealf(h[1][0][j]),
+                                 cimagf(h[1][0][j]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h10 = _mm256_set_ps(crealf(h[0][1][j]),
+                                 cimagf(h[0][1][j]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
+      __m256 h11 = _mm256_set_ps(crealf(h[1][1][j]),
+                                 cimagf(h[1][1][j]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 3]));
+
+      /* 1. B = W'* H' */
+      __m256 a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a01 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          a10 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a11 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 1:
+          a00 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a01 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          a10 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a11 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      a00 = _mm256_mul_ps(a00, avx_norm);
+      a01 = _mm256_mul_ps(a01, avx_norm);
+      a10 = _mm256_mul_ps(a10, avx_norm);
+      a11 = _mm256_mul_ps(a11, avx_norm);
+
+      /* 2. B = W' * H' * H = A * H */
+#ifdef LV_HAVE_FMA
+      __m256 b00 = _MM256_PROD_ADD_PS(a00, h00, _MM256_PROD_PS(a01, h10));
+      __m256 b01 = _MM256_PROD_ADD_PS(a00, h01, _MM256_PROD_PS(a01, h11));
+      __m256 b10 = _MM256_PROD_ADD_PS(a10, h00, _MM256_PROD_PS(a11, h10));
+      __m256 b11 = _MM256_PROD_ADD_PS(a10, h01, _MM256_PROD_PS(a11, h11));
+#else
+      __m256 b00 = _mm256_add_ps(_MM256_PROD_PS(a00, h00), _MM256_PROD_PS(a01, h10));
+      __m256 b01 = _mm256_add_ps(_MM256_PROD_PS(a00, h01), _MM256_PROD_PS(a01, h11));
+      __m256 b10 = _mm256_add_ps(_MM256_PROD_PS(a10, h00), _MM256_PROD_PS(a11, h10));
+      __m256 b11 = _mm256_add_ps(_MM256_PROD_PS(a10, h01), _MM256_PROD_PS(a11, h11));
+#endif /* LV_HAVE_FMA */
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m256 c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = _mm256_add_ps(b00, b01);
+          c01 = _mm256_sub_ps(b00, b01);
+          c10 = _mm256_add_ps(b10, b11);
+          c11 = _mm256_sub_ps(b10, b11);
+          break;
+        case 1:
+          c00 = _mm256_add_ps(b00, _MM256_MULJ_PS(b01));
+          c01 = _mm256_sub_ps(b00, _MM256_MULJ_PS(b01));
+          c10 = _mm256_add_ps(b10, _MM256_MULJ_PS(b11));
+          c11 = _mm256_sub_ps(b10, _MM256_MULJ_PS(b11));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 = _mm256_mul_ps(c00, avx_norm);
+      c01 = _mm256_mul_ps(c01, avx_norm);
+      c10 = _mm256_mul_ps(c10, avx_norm);
+      c11 = _mm256_mul_ps(c11, avx_norm);
+
+      /* 4. C += noise * I */
+      c00 = _mm256_add_ps(c00, avx_noise_estimate);
+      c11 = _mm256_add_ps(c11, avx_noise_estimate);
+
+      /* 5. detC */
+#ifdef LV_HAVE_FMA
+      __m256 detC = _MM256_PROD_SUB_PS(c00, c11, _MM256_PROD_PS(c01, c10));
+#else
+      __m256 detC = _mm256_sub_ps(_MM256_PROD_PS(c00, c11), _MM256_PROD_PS(c01, c10));
+#endif /* LV_HAVE_FMA */
+      __m256 inv_detC = srslte_algebra_cf_recip_avx(detC);
+
+      __m256 den0 = _MM256_PROD_PS(c00, inv_detC);
+      __m256 den1 = _MM256_PROD_PS(c11, inv_detC);
+
+      __m256 gamma0 = _mm256_sub_ps(srslte_algebra_cf_recip_avx(den0), avx_ones);
+      __m256 gamma1 = _mm256_sub_ps(srslte_algebra_cf_recip_avx(den1), avx_ones);
+
+      /* Add for averaging */
+      __m256 sinr_avx = _mm256_permute_ps(_mm256_add_ps(gamma0, gamma1), 0b00101000);
+      __attribute__((aligned(256))) float sinr[8];
+      _mm256_store_ps(sinr, sinr_avx);
+
+      sinr_list[i] += sinr[0] + sinr[2] + sinr[4] + sinr[6];
+
+      count += 4;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= 2 * count * noise_estimate;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_AVX */
+
+/* PMI Select for 2 layers */
+int srslte_precoding_pmi_select_2l(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                   float noise_estimate, uint32_t *pmi,
+                                   float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  int ret;
+#ifdef LV_HAVE_AVX
+  ret = srslte_precoding_pmi_select_2l_avx(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  #ifdef LV_HAVE_SSE
+  ret = srslte_precoding_pmi_select_2l_sse(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  ret = srslte_precoding_pmi_select_2l_gen(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+
+  INFO("Precoder PMI Select for 2 layers SINR=[%.1fdB; %.1fdB] PMI=%d\n", 10 * log10(sinr_list[0]),
+       10 * log10(sinr_list[1]), *pmi);
+
+  return ret;
+}
+
+int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                float noise_estimate, int nof_layers, uint32_t *pmi,
+                                float sinr[SRSLTE_MAX_CODEBOOKS]) {
+  int ret;
+
+  if (sinr == NULL || pmi == NULL) {
+    ERROR("Null pointer");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  } else if (nof_layers == 1) {
+    ret = srslte_precoding_pmi_select_1l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else if (nof_layers == 2) {
+    ret = srslte_precoding_pmi_select_2l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else {
+    ERROR("Wrong number of layers");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return ret;
+}
diff --git a/lib/src/phy/mimo/test/CMakeLists.txt b/lib/src/phy/mimo/test/CMakeLists.txt
index e0894a24d..2397bfc49 100644
--- a/lib/src/phy/mimo/test/CMakeLists.txt
+++ b/lib/src/phy/mimo/test/CMakeLists.txt
@@ -52,14 +52,21 @@ add_test(precoding_single precoding_test -n 1000 -m single)
 add_test(precoding_diversity2 precoding_test -n 1000 -m diversity -l 2 -p 2) 
 add_test(precoding_diversity4 precoding_test -n 1024 -m diversity -l 4 -p 4) 
 
-add_test(precoding_cdd_2x2 precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000)
+add_test(precoding_cdd_2x2_zf precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000 -d zf)
+add_test(precoding_cdd_2x2_mmse precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000 -d mmse)
 
 add_test(precoding_multiplex_1l_cb0 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 0)
 add_test(precoding_multiplex_1l_cb1 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 1)
 add_test(precoding_multiplex_1l_cb2 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 2)
 add_test(precoding_multiplex_1l_cb3 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 3)
 
-add_test(precoding_multiplex_2l_cb0 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0)
-add_test(precoding_multiplex_2l_cb1 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1)
-add_test(precoding_multiplex_2l_cb2 precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2)
+add_test(precoding_multiplex_2l_cb0_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0 -d zf)
+add_test(precoding_multiplex_2l_cb1_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1 -d zf)
+add_test(precoding_multiplex_2l_cb2_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2 -d zf)
+
+
+add_test(precoding_multiplex_2l_cb0_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0 -d mmse)
+add_test(precoding_multiplex_2l_cb1_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1 -d mmse)
+add_test(precoding_multiplex_2l_cb2_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2 -d mmse)
+
 
diff --git a/lib/src/phy/mimo/test/precoder_test.c b/lib/src/phy/mimo/test/precoder_test.c
index bc23004f1..1bbd9e3c0 100644
--- a/lib/src/phy/mimo/test/precoder_test.c
+++ b/lib/src/phy/mimo/test/precoder_test.c
@@ -34,6 +34,7 @@
 #include <stdbool.h>
 
 #include "srslte/srslte.h"
+#include "srslte/phy/channel/ch_awgn.h"
 
 #define MSE_THRESHOLD	0.0005
 
@@ -41,18 +42,24 @@ int nof_symbols = 1000;
 uint32_t codebook_idx = 0;
 int nof_layers = 1, nof_tx_ports = 1, nof_rx_ports = 1, nof_re = 1;
 char *mimo_type_name = NULL;
+char decoder_type_name [16] = "zf";
+float snr_db = 100.0f;
 
 void usage(char *prog) {
   printf(
       "Usage: %s -m [single|diversity|multiplex|cdd] -l [nof_layers] -p [nof_tx_ports]\n"
           " -r [nof_rx_ports]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
-  printf("\t-c codebook_idx [Default %d]\n\n", codebook_idx);
+  printf("\t-c codebook_idx [Default %d]\n", codebook_idx);
+  printf("\t-s SNR in dB [Default %.1fdB]*\n", snr_db);
+  printf("\t-d decoder type [zf|mmse] [Default %s]\n", decoder_type_name);
+  printf("\n");
+  printf("* Performance test example:\n\t for snr in {0..20..1}; do ./precoding_test -m single -s $snr; done; \n\n", decoder_type_name);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "mplnrc")) != -1) {
+  while ((opt = getopt(argc, argv, "mplnrcds")) != -1) {
     switch (opt) {
     case 'n':
       nof_symbols = atoi(argv[optind]);
@@ -72,6 +79,12 @@ void parse_args(int argc, char **argv) {
     case 'c':
       codebook_idx = (uint32_t) atoi(argv[optind]);
       break;
+    case 'd':
+      strncpy(decoder_type_name, argv[optind], 16);
+      break;
+    case 's':
+      snr_db = (float) atof(argv[optind]);
+      break;
     default:
       usage(argv[0]);
       exit(-1);
@@ -134,8 +147,17 @@ void populate_channel(srslte_mimo_type_t type, cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_
   }
 }
 
+static void awgn(cf_t *y[SRSLTE_MAX_PORTS], uint32_t n, float snr) {
+  int i;
+  float std_dev = powf(10, - (snr + 3.0f) / 20.0f);
+
+  for (i = 0; i < nof_rx_ports; i++) {
+    srslte_ch_awgn_c(y[i], y[i], std_dev, n);
+  }
+}
+
 int main(int argc, char **argv) {
-  int i, j, k;
+  int i, j, k, nof_errors = 0, ret = SRSLTE_SUCCESS;
   float mse;
   cf_t *x[SRSLTE_MAX_LAYERS], *r[SRSLTE_MAX_PORTS], *y[SRSLTE_MAX_PORTS], *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
       *xr[SRSLTE_MAX_LAYERS];
@@ -247,27 +269,48 @@ int main(int argc, char **argv) {
     }
   }
 
+  awgn(r, (uint32_t) nof_re, snr_db);
+
+  /* If CDD or Spatial muliplex choose decoder */
+  if (strncmp(decoder_type_name, "zf", 16) == 0) {
+    srslte_predecoding_set_mimo_decoder(SRSLTE_MIMO_DECODER_ZF);
+  } else if (strncmp(decoder_type_name, "mmse", 16) == 0) {
+    srslte_predecoding_set_mimo_decoder(SRSLTE_MIMO_DECODER_MMSE);
+  } else {
+    ret = SRSLTE_ERROR;
+    goto quit;
+  }
+
+
   /* predecoding / equalization */
   struct timeval t[3];
   gettimeofday(&t[1], NULL);
   srslte_predecoding_type_multi(r, h, xr, nof_rx_ports, nof_tx_ports, nof_layers,
-                                codebook_idx, nof_re, type, 0);
+                                codebook_idx, nof_re, type, powf(10, -snr_db/10));
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
-  printf("Execution Time: %ld us\n", t[0].tv_usec);
-  
+
   /* check errors */
   mse = 0;
   for (i = 0; i < nof_layers; i++) {
     for (j = 0; j < nof_symbols; j++) {
       mse += cabsf(xr[i][j] - x[i][j]);
+
+      if ((crealf(xr[i][j]) > 0) != (crealf(x[i][j]) > 0)) {
+        nof_errors ++;
+      }
+      if ((cimagf(xr[i][j]) > 0) != (cimagf(x[i][j]) > 0)) {
+        nof_errors ++;
+      }
     }
   }
-  printf("MSE: %f\n", mse/ nof_layers / nof_symbols );
+  printf("SNR: %5.1fdB;\tExecution time: %5ldus;\tMSE: %.6f;\tBER: %.6f\n", snr_db, t[0].tv_usec,
+         mse / nof_layers / nof_symbols, (float) nof_errors / (4.0f * nof_re));
   if (mse / nof_layers / nof_symbols > MSE_THRESHOLD) {
-    exit(-1);
+    ret = SRSLTE_ERROR;
   } 
 
+  quit:
   /* Free all data */
   for (i = 0; i < nof_layers; i++) {
     free(x[i]);
@@ -284,6 +327,5 @@ int main(int argc, char **argv) {
     }
   }
 
-  printf("Ok\n");
-  exit(0); 
+  exit(ret);
 }
diff --git a/lib/src/phy/mimo/test/predecoder_mex.c b/lib/src/phy/mimo/test/predecoder_mex.c
index e04b390d0..0465e1ef4 100644
--- a/lib/src/phy/mimo/test/predecoder_mex.c
+++ b/lib/src/phy/mimo/test/predecoder_mex.c
@@ -35,8 +35,10 @@
 #define HEST     prhs[1]
 #define NEST     prhs[2]
 #define NLAYERS  prhs[3]
-#define TXSCHEME prhs[4]
-#define NOF_INPUTS 5
+#define NCW      prhs[4]
+#define TXSCHEME prhs[5]
+#define CODEBOOK prhs[6]
+#define NOF_INPUTS 7
 
 
 void help()
@@ -58,6 +60,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   uint32_t nof_layers;
   uint32_t nof_tx_ports = 1;
   uint32_t nof_codewords = 1;
+  uint32_t codebook_idx = 0;
   float noise_estimate = 0;
   cf_t *x[SRSLTE_MAX_LAYERS];
   cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
@@ -81,6 +84,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   /* Read number of layers */
   nof_layers = (uint32_t) mxGetScalar(NLAYERS);
 
+  /* Read number of codewords */
+  nof_codewords = (uint32_t) mxGetScalar(NCW);
+
   if (nof_layers > SRSLTE_MAX_LAYERS) {
     mexErrMsgTxt("Too many layers\n");
     return;
@@ -109,7 +115,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
 
   /* Print parameters trace */
-  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_symbols=%d\n", nof_tx_ports, nof_rx_ants, nof_layers, nof_symbols);
+  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_codewords=%d, codebook_idx=%d, nof_symbols=%d\n",
+            nof_tx_ports, nof_rx_ants, nof_layers, nof_codewords, codebook_idx, nof_symbols);
 
   /* Read noise estimate */
   if (nrhs >= NOF_INPUTS) {
@@ -134,7 +141,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 
   /* Allocate memory for intermediate data */
   for (i = 0; i < nof_tx_ports; i++) {
-    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols*nof_layers);
   }
 
   /* Allocate memory for channel estimate */
@@ -155,15 +162,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mxGetString_700(TXSCHEME, txscheme, 32);
   }
 
-  if (!strcmp(txscheme, "Port0")) {
-    type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-  } else if (!strcmp(txscheme, "TxDiversity")) {
-    type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-  } else if (!strcmp(txscheme, "CDD")) {    
-    type = SRSLTE_MIMO_TYPE_CDD;
-  } else if (!strcmp(txscheme, "SpatialMux")) {
-    type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-  } else {
+  codebook_idx = (uint32_t) mxGetScalar(CODEBOOK);
+
+  if (srslte_str2mimotype(txscheme, &type)) {
     mexPrintf("Unsupported TxScheme=%s\n", txscheme);
     return; 
   }
@@ -176,18 +177,19 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   /* Set output pointer */
   cf_t *d[SRSLTE_MAX_CODEWORDS];
   for (i = 0; i<nof_codewords; i++) {
-    d[i] = output;
+    d[i] = &output[i*nof_symbols*nof_layers/nof_codewords];
   }
 
   /* Pre-decode */
-  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, nof_symbols, type, noise_estimate);
+  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, codebook_idx, nof_symbols, type,
+                                noise_estimate);
 
   /* Layer de-mapper */
   srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
   
   /* Write output */
   if (nlhs >= 1) {
-    mexutils_write_cf(output, &plhs[0], nof_symbols*nof_layers*nof_rx_ants/nof_tx_ports, 1);
+    mexutils_write_cf(output, &plhs[0], nof_symbols, nof_codewords);
   }
 
   /* Free memory */

From 48d508aeba681f57796acea2ad631261686e0c43 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:19:19 +0200
Subject: [PATCH 070/269] Added srs_lte_cpy for aligned copy which improves a
 bit performance for aligned data

---
 lib/include/srslte/phy/utils/vector.h |  3 +++
 lib/src/phy/mimo/layermap.c           |  8 +++++++-
 lib/src/phy/mimo/test/layermap_test.c |  6 +++---
 lib/src/phy/utils/vector.c            | 21 +++++++++++++++++++++
 4 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index 74cd60172..c8d8d1942 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -172,6 +172,9 @@ SRSLTE_API void srslte_vec_abs_square_cf(cf_t *x, float *abs_square, uint32_t le
 /* argument of each vector element */
 SRSLTE_API void srslte_vec_arg_cf(cf_t *x, float *arg, uint32_t len);
 
+/* Copy 256 bit aligned vector */
+SRSLTE_API void srs_vec_cf_cpy(cf_t *src, cf_t *dst, int len);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/src/phy/mimo/layermap.c b/lib/src/phy/mimo/layermap.c
index 868bf2f6a..a5bfc8b92 100644
--- a/lib/src/phy/mimo/layermap.c
+++ b/lib/src/phy/mimo/layermap.c
@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <string.h>
+#include <srslte/phy/utils/vector.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/layermap.h"
@@ -51,7 +52,12 @@ int srslte_layermap_diversity(cf_t *d, cf_t *x[SRSLTE_MAX_LAYERS], int nof_layer
 
 int srslte_layermap_multiplex(cf_t *d[SRSLTE_MAX_CODEWORDS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_cw, int nof_layers,
     int nof_symbols[SRSLTE_MAX_CODEWORDS]) {
-  if (nof_cw == 1) {
+  if (nof_cw == nof_layers) {
+    for (int i = 0; i < nof_cw; i++) {
+      srs_vec_cf_cpy(x[i], d[i], (uint32_t) nof_symbols[0]);
+    }
+    return nof_symbols[0];
+  } else if (nof_cw == 1) {
     return srslte_layermap_diversity(d[0], x, nof_layers, nof_symbols[0]);
   } else {
     int n[2];
diff --git a/lib/src/phy/mimo/test/layermap_test.c b/lib/src/phy/mimo/test/layermap_test.c
index 6fcc72682..6f026f9d0 100644
--- a/lib/src/phy/mimo/test/layermap_test.c
+++ b/lib/src/phy/mimo/test/layermap_test.c
@@ -96,19 +96,19 @@ int main(int argc, char **argv) {
   }
 
   for (i=0;i<nof_cw;i++) {
-    d[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    d[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!d[i]) {
       perror("malloc");
       exit(-1);
     }
-    dp[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    dp[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!dp[i]) {
       perror("malloc");
       exit(-1);
     }
   }
   for (i=0;i<nof_layers;i++) {
-    x[i] = malloc(sizeof(cf_t) * nof_symbols);
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!x[i]) {
       perror("malloc");
       exit(-1);
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 30527510d..ff676d6f4 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -843,3 +843,24 @@ void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset,
   }
 }
 
+void srs_vec_cf_cpy(cf_t *dst, cf_t *src, int len) {
+  int i = 0;
+
+#ifdef LV_HAVE_AVX
+    for (; i < len - 3; i += 4) {
+      _mm256_store_ps((float *) &dst[i], _mm256_load_ps((float *) &src[i]));
+    }
+#endif /* LV_HAVE_AVX */
+#ifdef LV_HAVE_SSE
+    for (; i < len - 1; i += 2) {
+      _mm_store_ps((float *) &dst[i], _mm_load_ps((float *) &src[i]));
+    }
+  for (; i < len; i++) {
+    ((__m64*) dst)[i] = ((__m64*) src)[i];
+  }
+#else
+  for (; i < len; i++) {
+    dst[i] = src[i];
+  }
+#endif /* LV_HAVE_SSE */
+}

From 94325d13d230293a4ef1160c265f8cefc1d678ed Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:23:03 +0200
Subject: [PATCH 071/269] Scalable PDSCH with MAX_CODEWORDS PDSCH plus decoding
 threads for bitrate increase

---
 lib/examples/cell_measurement.c               |   2 +-
 lib/examples/pdsch_ue.c                       |   2 +-
 lib/include/srslte/phy/phch/pdsch.h           |  68 ++-
 lib/include/srslte/phy/phch/ra.h              |   7 +
 lib/include/srslte/phy/phch/sch.h             |   7 +
 lib/include/srslte/phy/ue/ue_dl.h             |   7 +-
 lib/include/srslte/phy/utils/debug.h          |   7 +-
 lib/src/phy/phch/pdsch.c                      | 530 +++++++++++-------
 lib/src/phy/phch/sch.c                        |  35 +-
 lib/src/phy/phch/test/dlsch_encode_test_mex.c |   8 +-
 lib/src/phy/phch/test/pdsch_test.c            |  27 +-
 lib/src/phy/phch/test/pdsch_test_mex.c        |   6 +-
 lib/src/phy/ue/ue_dl.c                        |   8 +-
 srsue/src/phy/phch_worker.cc                  |   2 +-
 14 files changed, 485 insertions(+), 231 deletions(-)

diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index 9c7856daa..90b963db8 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -320,7 +320,7 @@ int main(int argc, char **argv) {
             } else if (n == 0) {
               printf("CFO: %+6.4f kHz, SFO: %+6.4f kHz, NOI: %.2f, PDCCH-Det: %.3f\r",
                       srslte_ue_sync_get_cfo(&ue_sync)/1000, srslte_ue_sync_get_sfo(&ue_sync)/1000, 
-                      srslte_sch_average_noi(&ue_dl.pdsch.dl_sch),
+                      srslte_pdsch_average_noi(&ue_dl.pdsch),
                       (float) ue_dl.nof_detected/nof_trials);                
               nof_trials++; 
             } else {
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 1c6059d9f..24bdba55b 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -846,7 +846,7 @@ void *plot_thread_run(void *arg) {
       plot_scatter_setNewData(&pscatequal_pdcch, ue_dl.pdcch.d, 36*ue_dl.pdcch.nof_cce);
     }
     
-    plot_scatter_setNewData(&pscatequal, ue_dl.pdsch.d, nof_symbols);
+    plot_scatter_setNewData(&pscatequal, ue_dl.pdsch.d[0], nof_symbols);
     
     if (plot_sf_idx == 1) {
       if (prog_args.net_port_signal > 0) {
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index fc250a7d5..b4620be69 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -35,6 +35,13 @@
 #ifndef PDSCH_
 #define PDSCH_
 
+#ifndef SRSLTE_SINGLE_THREAD
+
+#include <pthread.h>
+#include <semaphore.h>
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
 #include "srslte/config.h"
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/precoding.h"
@@ -48,11 +55,38 @@
 #include "srslte/phy/phch/pdsch_cfg.h"
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
-  srslte_sequence_t seq2[SRSLTE_NSUBFRAMES_X_FRAME];
+  srslte_sequence_t seq[SRSLTE_MAX_CODEWORDS][SRSLTE_NSUBFRAMES_X_FRAME];
   bool sequence_generated;
 } srslte_pdsch_user_t;
 
+#ifndef SRSLTE_SINGLE_THREAD
+
+typedef struct {
+  /* Thread identifier: they must set before thread creation */
+  uint32_t codeword_idx;
+  void *pdsch_ptr;
+
+  /* Configuration Encoder/Decoder: they must be set before posting start semaphore */
+  srslte_pdsch_cfg_t *cfg;
+  uint16_t rnti;
+
+  /* Encoder/Decoder data pointers: they must be set before posting start semaphore  */
+  uint8_t *data;
+  void *softbuffer;
+
+  /* Execution status */
+  int ret_status;
+
+  /* Semaphores */
+  sem_t start;
+  sem_t finish;
+
+  /* Thread kill flag */
+  bool quit;
+} srslte_pdsch_thread_args_t;
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
 /* PDSCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
@@ -66,10 +100,8 @@ typedef struct SRSLTE_API {
   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; /* Channel estimation (Rx only) */
   cf_t *symbols[SRSLTE_MAX_PORTS];              /* PDSCH Encoded/Decoded Symbols */
   cf_t *x[SRSLTE_MAX_LAYERS];                   /* Layer mapped */
-  cf_t *d;                                      /* Modulated/Demodulated codeword 1 */
-  cf_t *d2;                                     /* Modulated/Demodulated codeword 2 */
-  void *e;
-  void *e2;
+  cf_t *d[SRSLTE_MAX_CODEWORDS];                /* Modulated/Demodulated codewords */
+  void *e[SRSLTE_MAX_CODEWORDS];
 
   /* tx & rx objects */
   srslte_modem_table_t mod[4];
@@ -77,8 +109,15 @@ typedef struct SRSLTE_API {
   // This is to generate the scrambling seq for multiple CRNTIs
   srslte_pdsch_user_t **users;
   
-  srslte_sch_t dl_sch;
-  
+  srslte_sch_t dl_sch[SRSLTE_MAX_CODEWORDS];
+
+#ifndef SRSLTE_SINGLE_THREAD
+
+  pthread_t threads[SRSLTE_MAX_CODEWORDS];
+  srslte_pdsch_thread_args_t thread_args[SRSLTE_MAX_CODEWORDS];
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
 } srslte_pdsch_t;
 
 SRSLTE_API int srslte_pdsch_init(srslte_pdsch_t *q, 
@@ -151,7 +190,18 @@ SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
                                          uint16_t rnti,
                                          uint8_t *data[SRSLTE_MAX_CODEWORDS]);
 
-SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q); 
+SRSLTE_API int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
+                                          srslte_pdsch_cfg_t *cfg,
+                                          cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                          float noise_estimate,
+                                          uint32_t nof_ce,
+                                          uint32_t *ri,
+                                          uint32_t *pmi,
+                                          float *current_sinr);
+
+SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, int max_iter);
+
+SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q);
 
 SRSLTE_API uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q); 
 
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 5fab0f10b..8a025ad97 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -223,6 +223,13 @@ SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
                                                   srslte_ra_nbits_t *nbits,
                                                   srslte_ra_nbits_t *nbits2);
 
+SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
+                                                  uint32_t cfi,
+                                                  srslte_cell_t cell,
+                                                  uint32_t sf_idx,
+                                                  srslte_ra_nbits_t *nbits,
+                                                  srslte_ra_nbits_t *nbits2);
+
 SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, 
                                                uint32_t nof_prb, 
                                                uint32_t nof_ctrl_symbols); 
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index 128cfea6f..4fa1b3b0f 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -96,6 +96,13 @@ SRSLTE_API int srslte_dlsch_encode(srslte_sch_t *q,
                                    uint8_t *data, 
                                    uint8_t *e_bits);
 
+SRSLTE_API int srslte_dlsch_encode2(srslte_sch_t *q,
+                                    srslte_pdsch_cfg_t *cfg,
+                                    srslte_softbuffer_tx_t *softbuffer,
+                                    uint8_t *data,
+                                    uint8_t *e_bits,
+                                    int codeword_idx);
+
 SRSLTE_API int srslte_dlsch_encode_multi(srslte_sch_t *q,
                                    srslte_pdsch_cfg_t *cfg,
                                    srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 0c77ba911..88442b93d 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -191,12 +191,17 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q,
                                         uint32_t tti,
                                         uint16_t rnti);
 
-SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q, 
+SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q,
                                               cf_t *input[SRSLTE_MAX_PORTS], 
                                               uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                               uint32_t tti,
                                               uint16_t rnti);
 
+SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
+                                          uint32_t *ri,
+                                          uint32_t *pmi,
+                                          float *current_sinr);
+
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
                                           uint32_t sf_idx, 
                                           uint32_t n_prb_lowest, 
diff --git a/lib/include/srslte/phy/utils/debug.h b/lib/include/srslte/phy/utils/debug.h
index e1021bc2a..e4b4756f8 100644
--- a/lib/include/srslte/phy/utils/debug.h
+++ b/lib/include/srslte/phy/utils/debug.h
@@ -63,6 +63,11 @@ SRSLTE_API extern int srslte_verbose;
 #define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO) \
   fprintf(stdout, "[INFO]:  " _fmt, __VA_ARGS__)
 
-#define ERROR(_fmt) fprintf(stderr, "%s.%d: " _fmt "\n", __FILE__, __LINE__)
+#if CMAKE_BUILD_TYPE==Debug
+/* In debug mode, it prints out the  */
+#define ERROR(_fmt, ...) fprintf(stderr, "%s.%d: " _fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)
+#else
+#define ERROR(_fmt, ...) fprintf(stderr, "[ERROR in %s]:" _fmt "\n", __FUNCTION__, ##__VA_ARGS__)
+#endif /* CMAKE_BUILD_TYPE==Debug */
 
 #endif // DEBUG_H
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 8cdfd0884..3ff2a910b 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -32,9 +32,13 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
-#include <srslte/phy/phch/pdsch.h>
-#include <srslte/phy/phch/ra.h>
-#include <srslte/phy/phch/pdsch_cfg.h>
+
+#ifndef SRSLTE_SINGLE_THREAD
+
+#include <pthread.h>
+#include <semaphore.h>
+
+#endif /* SRSLTE_SINGLE_THREAD */
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
@@ -60,6 +64,13 @@ extern int indices[100000];
 extern int indices_ptr; 
 #endif
 
+#ifndef SRSLTE_SINGLE_THREAD
+
+static void *srslte_pdsch_encode_thread (void *arg);
+static void *srslte_pdsch_decode_thread (void *arg);
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
 float srslte_pdsch_coderate(uint32_t tbs, uint32_t nof_re) 
 {
   return (float) (tbs + 24)/(nof_re);
@@ -234,28 +245,38 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       }
       srslte_modem_table_bytes(&q->mod[i]);
     }
-    
-    srslte_sch_init(&q->dl_sch);
-    
-    // Allocate int16_t for reception (LLRs)
-    q->e = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
-    if (!q->e) {
-      goto clean;
-    }
-    
-    q->e2 = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
-    if (!q->e) {
-      goto clean;
-    }
 
-    q->d = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-    if (!q->d) {
-      goto clean;
-    }
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (srslte_sch_init(&q->dl_sch[i])) {
+        ERROR("Initiating DL SCH");
+        goto clean;
+      }
 
-    q->d2 = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-    if (!q->d) {
-      goto clean;
+      // Allocate int16_t for reception (LLRs)
+      q->e[i] = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
+      if (!q->e[i]) {
+        goto clean;
+      }
+
+      q->d[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->d[i]) {
+        goto clean;
+      }
+
+#ifndef SRSLTE_SINGLE_THREAD
+      if (sem_init(&q->thread_args[i].start, 0, 0)) {
+        ERROR("Creating semaphore");
+        goto clean;
+      }
+      if (sem_init(&q->thread_args[i].finish, 0, 0)) {
+        ERROR("Creating semaphore");
+        goto clean;
+      }
+      q->thread_args[i].codeword_idx = (uint32_t) i;
+      q->thread_args[i].pdsch_ptr = q;
+      pthread_create(&q->threads[i], NULL, (is_receiver) ? srslte_pdsch_decode_thread : srslte_pdsch_encode_thread,
+                     (void *) &q->thread_args[i]);
+#endif /* SRSLTE_SINGLE_THREAD */
     }
 
     /* Layer mapped symbols memory allocation */
@@ -290,10 +311,11 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       perror("malloc");
       goto clean;
     }
-    
-    ret = SRSLTE_SUCCESS;
-  }
-  clean: 
+ }
+
+  ret = SRSLTE_SUCCESS;
+
+  clean:
   if (ret == SRSLTE_ERROR) {
     srslte_pdsch_free(q);
   }
@@ -315,18 +337,31 @@ int srslte_pdsch_init_rx_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t n
 void srslte_pdsch_free(srslte_pdsch_t *q) {
   int i;
 
-  if (q->e) {
-    free(q->e);
-  }
-  if (q->e2) {
-    free(q->e2);
-  }
-  if (q->d) {
-    free(q->d);
-  }
-  if (q->d2) {
-    free(q->d2);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+
+#ifndef SRSLTE_SINGLE_THREAD
+    /* Stop threads */
+    q->thread_args[i].quit = true;
+    sem_post(&q->thread_args[i].start);
+    pthread_join(q->threads[i], NULL);
+    pthread_detach(q->threads[i]);
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
+    if (q->e[i]) {
+      free(q->e[i]);
+    }
+
+    if (q->d[i]) {
+      free(q->d[i]);
+    }
+
+    /* Free sch objects */
+    srslte_sch_free(&q->dl_sch[i]);
+
+
   }
+
   for (i = 0; i < q->cell.nof_ports; i++) {
     if (q->x[i]) {
       free(q->x[i]);
@@ -354,8 +389,6 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     srslte_modem_table_free(&q->mod[i]);
   }
 
-  srslte_sch_free(&q->dl_sch);
-
   bzero(q, sizeof(srslte_pdsch_t));
 
 }
@@ -454,18 +487,17 @@ int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_r
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  */
 int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
-  uint32_t i;  
+  uint32_t i, j;
   if (!q->users[rnti]) {
     q->users[rnti] = calloc(1, sizeof(srslte_pdsch_user_t));
     if (q->users[rnti]) {
       for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pdsch(&q->users[rnti]->seq[i], rnti, 0, 2 * i, q->cell.id,
-            q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          return SRSLTE_ERROR; 
-        }
-        if (srslte_sequence_pdsch(&q->users[rnti]->seq2[i], rnti, 1, 2 * i, q->cell.id,
-                                  q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          return SRSLTE_ERROR;
+        for (j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+          if (srslte_sequence_pdsch(&q->users[rnti]->seq[j][i], rnti, j, 2 * i, q->cell.id,
+                                    q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+            ERROR("Generating scrambling sequence");
+            return SRSLTE_ERROR;
+          }
         }
       }
       q->users[rnti]->sequence_generated = true;
@@ -474,70 +506,194 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
   return SRSLTE_SUCCESS;
 }
 
+static inline int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+                                               srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
+                                               uint32_t codeword_idx) {
+  srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
+  srslte_ra_nbits_t *nbits = (codeword_idx == 0) ? &cfg->nbits : &cfg->nbits2;
+  srslte_ra_mcs_t *mcs = (codeword_idx == 0) ? &cfg->grant.mcs : &cfg->grant.mcs2;
+
+  if (nbits->nof_bits) {
+    INFO("Encoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+         cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
+         nbits->nof_re, nbits->nof_bits, (codeword_idx == 0) ? cfg->rv : cfg->rv2);
+
+    /* Channel coding */
+    if (srslte_dlsch_encode2(dl_sch, cfg, softbuffer, data, pdsch->e[codeword_idx], codeword_idx)) {
+      ERROR("Error encoding TB %d", codeword_idx);
+      return SRSLTE_ERROR;
+    }
+
+    /* Bit scrambling */
+    if (!pdsch->users[rnti]) {
+      srslte_sequence_t seq;
+
+      if (srslte_sequence_pdsch(&seq, rnti, codeword_idx, 2 * cfg->sf_idx, pdsch->cell.id, nbits->nof_bits)) {
+        ERROR("Initialising scrambling sequence");
+        return SRSLTE_ERROR;
+      }
+      srslte_scrambling_bytes(&seq, (uint8_t *) pdsch->e[codeword_idx], nbits->nof_bits);
+      srslte_sequence_free(&seq);
+
+    } else {
+      srslte_scrambling_bytes(&pdsch->users[rnti]->seq[codeword_idx][cfg->sf_idx],
+                              (uint8_t *) pdsch->e[codeword_idx],
+                              nbits->nof_bits);
+    }
+
+    /* Bit mapping */
+    srslte_mod_modulate_bytes(&pdsch->mod[mcs->mod],
+                              (uint8_t *) pdsch->e[codeword_idx],
+                              pdsch->d[codeword_idx], nbits->nof_bits);
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+
+#ifndef SRSLTE_SINGLE_THREAD
+
+static void *srslte_pdsch_encode_thread(void *arg) {
+  srslte_pdsch_thread_args_t *q = (srslte_pdsch_thread_args_t *) arg;
+  uint32_t codeword_idx = q->codeword_idx;
+
+  INFO("[PDSCH Encoder CW %d] waiting for data\n", codeword_idx);
+
+  sem_wait(&q->start);
+  while (!q->quit) {
+    q->ret_status = srslte_pdsch_codeword_encode(q->pdsch_ptr, q->cfg, q->softbuffer, q->rnti, q->data, codeword_idx);
+
+    /* Post finish semaphore */
+    sem_post(&q->finish);
+
+    /* Wait for next loop */
+    sem_wait(&q->start);
+  }
+
+  pthread_exit(NULL);
+  return q;
+}
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
+static inline int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+                                               srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
+                                               uint32_t codeword_idx) {
+  srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
+  srslte_ra_nbits_t *nbits = (codeword_idx == 0) ? &cfg->nbits : &cfg->nbits2;
+  srslte_ra_mcs_t *mcs = (codeword_idx == 0) ? &cfg->grant.mcs : &cfg->grant.mcs2;
+
+  if (nbits->nof_bits) {
+    INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+         cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
+         nbits->nof_re, nbits->nof_bits, (codeword_idx == 0) ? cfg->rv : cfg->rv2);
+
+    srslte_demod_soft_demodulate_s(mcs->mod, pdsch->d[codeword_idx], pdsch->e[codeword_idx], cfg->nbits.nof_re);
+
+    if (pdsch->users[rnti] && pdsch->users[rnti]->sequence_generated) {
+      srslte_scrambling_s_offset(&pdsch->users[rnti]->seq[codeword_idx][cfg->sf_idx], pdsch->e[codeword_idx],
+                                 0, nbits->nof_bits);
+    } else {
+      srslte_sequence_t seq;
+      if (srslte_sequence_pdsch(&seq, rnti, codeword_idx, 2 * cfg->sf_idx, pdsch->cell.id, nbits->nof_bits)) {
+        ERROR("Initialising scrambling sequence");
+        return SRSLTE_ERROR;
+      }
+      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], codeword_idx, nbits->nof_bits);
+      srslte_sequence_free(&seq);
+    }
+
+    return srslte_dlsch_decode2(dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#ifndef SRSLTE_SINGLE_THREAD
+
+static void *srslte_pdsch_decode_thread(void *arg) {
+  srslte_pdsch_thread_args_t *q = (srslte_pdsch_thread_args_t *) arg;
+  uint32_t codeword_idx = q->codeword_idx;
+
+  INFO("[PDSCH Encoder CW %d] waiting for data\n", codeword_idx);
+
+  sem_wait(&q->start);
+  while (!q->quit) {
+    q->ret_status = srslte_pdsch_codeword_decode(q->pdsch_ptr, q->cfg, q->softbuffer, q->rnti, q->data, codeword_idx);
+
+    /* Post finish semaphore */
+    sem_post(&q->finish);
+
+    /* Wait for next loop */
+    sem_wait(&q->start);
+  }
+
+  pthread_exit(NULL);
+  return q;
+}
+
+#endif /* SRSLTE_SINGLE_THREAD */
+
+
 void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 {
   if (q->users[rnti]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      srslte_sequence_free(&q->users[rnti]->seq[i]);
-      srslte_sequence_free(&q->users[rnti]->seq2[i]);
+      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+        srslte_sequence_free(&q->users[rnti]->seq[j][i]);
+      }
     }
     free(q->users[rnti]);
-    q->users[rnti] = NULL; 
+    q->users[rnti] = NULL;
   }
 }
 
-int srslte_pdsch_decode(srslte_pdsch_t *q, 
+int srslte_pdsch_decode(srslte_pdsch_t *q,
                         srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
-                        cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate, 
-                        uint16_t rnti, uint8_t *data) 
+                        cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate,
+                        uint16_t rnti, uint8_t *data)
 {
-  cf_t *_sf_symbols[SRSLTE_MAX_PORTS]; 
+  cf_t *_sf_symbols[SRSLTE_MAX_PORTS];
   cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
-  _sf_symbols[0] = sf_symbols; 
+
+  _sf_symbols[0] = sf_symbols;
   for (int i=0;i<q->cell.nof_ports;i++) {
-    _ce[i][0] = ce[i]; 
+    _ce[i][0] = ce[i];
   }
   return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, &data);
 }
 
 /** Decodes the PDSCH from the received symbols
  */
-int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
+int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
                               srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, 
+                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
                               uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS])
 {
 
   /* Set pointers for layermapping & precoding */
-  uint32_t i, n;
+  uint32_t i;
   cf_t *x[SRSLTE_MAX_LAYERS];
   int ret = 0;
 
   if (q            != NULL &&
       sf_symbols   != NULL &&
-      data         != NULL && 
+      data         != NULL &&
       cfg          != NULL)
   {
-    
+
     INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: [%d %d], C_prb=%d\n",
-        cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re, 
+        cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re,
          cfg->nbits.nof_bits, cfg->rv, cfg->rv2, cfg->grant.nof_prb);
 
-    /* number of layers equals number of ports */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      x[i] = q->x[i];
-    }
-    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
-      
     for (int j=0;j<q->nof_rx_antennas;j++) {
       /* extract symbols */
-      n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
+      int n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
       if (n != cfg->nbits.nof_re) {
         fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
         return SRSLTE_ERROR;
       }
-      
+
       /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
         n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
@@ -545,25 +701,42 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
           fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
           return SRSLTE_ERROR;
         }
-      }      
+      }
     }
 
     INFO("PDSCH Layer demapper and predecode: mimo_type=%d, nof_layers=%d, nof_tb=%d\n", cfg->mimo_type,
          cfg->nof_layers, cfg->grant.nof_tb);
     if (q->cell.nof_ports == 1) {
       /* no need for layer demapping */
-      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
+      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d[0], q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
     } else {
       int nof_symbols [SRSLTE_MAX_CODEWORDS];
       nof_symbols[0] = cfg->nbits.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
       nof_symbols[1] = cfg->nbits2.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
 
-      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
-                                    cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, 0.0);
-      srslte_layerdemap_type(x, (cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, cfg->nof_layers, cfg->grant.nof_tb,
-                             nof_symbols[0], nof_symbols, cfg->mimo_type);
+      if (cfg->nof_layers == cfg->grant.nof_tb) {
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->d[i];
+        }
+
+        srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                      cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, noise_estimate);
+
+      } else {
+        /* number of layers equals number of ports */
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->x[i];
+        }
+        memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
+
+        srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                      cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, noise_estimate);
+
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+                               nof_symbols[0], nof_symbols, cfg->mimo_type);
+      }
     }
-    
+
     if (SRSLTE_VERBOSE_ISDEBUG()) {
       char filename[FILENAME_MAX];
       for (int j = 0; j < q->nof_rx_antennas; j++) {
@@ -586,47 +759,33 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       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
-    */
-    if (cfg->nbits.nof_bits) {
-      INFO("Decoding CW 0 (%d bits)\n", cfg->nbits.nof_bits);
-      srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
 
-      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-        srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
-      } else {
-        srslte_sequence_t seq;
-        if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-          return SRSLTE_ERROR;
-        }
-        srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
-        srslte_sequence_free(&seq);
-      }
+#ifndef SRSLTE_SINGLE_THREAD
 
-      ret |= srslte_dlsch_decode2(&q->dl_sch, cfg, &softbuffers[0], q->e, data[0], 0);
+    for (i = 0; i < cfg->grant.nof_tb; i++) {
+      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[i];
+      thread_args->cfg = cfg;
+      thread_args->softbuffer = &softbuffers[i];
+      thread_args->data = data[i];
+      thread_args->rnti = rnti;
+      sem_post(&thread_args->start);
     }
 
-    if (cfg->nbits2.nof_bits) {
-      INFO("Decoding CW 1 (%d bits)\n", cfg->nbits2.nof_bits);
-      srslte_demod_soft_demodulate_s(cfg->grant.mcs2.mod, q->d2, q->e2, cfg->nbits2.nof_re);
-
-      if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-        srslte_scrambling_s_offset(&q->users[rnti]->seq2[cfg->sf_idx], q->e2, 0, cfg->nbits2.nof_bits);
-      } else {
-        srslte_sequence_t seq;
-        if (srslte_sequence_pdsch(&seq, rnti, 1, 2 * cfg->sf_idx, q->cell.id, cfg->nbits2.nof_bits)) {
-          return SRSLTE_ERROR;
-        }
-        srslte_scrambling_s_offset(&seq, q->e2, 0, cfg->nbits2.nof_bits);
-        srslte_sequence_free(&seq);
-      }
-
-      ret |= srslte_dlsch_decode2(&q->dl_sch, cfg, &softbuffers[1], q->e2, data[1], 1);
+    for (i = 0; i < cfg->grant.nof_tb; i++) {
+      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[i];
+      sem_wait(&thread_args->finish);
+      ret |= thread_args->ret_status;
     }
 
+#else
+    /* 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
+    */
+    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
+        ret |= srslte_pdsch_codeword_decode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
+    }
+#endif /* SRSLTE_SINGLE_THREAD */
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
       DEBUG("SAVED FILE llr.dat: LLR estimates after demodulation and descrambling\n",0);
@@ -634,7 +793,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     }
 
     return ret;
-    
+
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
@@ -688,13 +847,13 @@ int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
       *pmi = (best_sinr_1l > best_sinr_2l) ? best_pmi_1l : best_pmi_2l;
     }
 
-    /* Set current condition number */
-    if (current_sinr != NULL) {
+    /* Set current SINR */
+    if (current_sinr != NULL && cfg->mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
       if (cfg->nof_layers == 1) {
         *current_sinr = sinr_1l[cfg->codebook_idx];
       } else if (cfg->nof_layers == 2) {
         *current_sinr = sinr_2l[cfg->codebook_idx - 1];
-      }else {
+      } else {
         ERROR("Not implemented number of layers");
         return SRSLTE_ERROR;
       }
@@ -713,16 +872,16 @@ int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
   return SRSLTE_SUCCESS;
 }
 
-int srslte_pdsch_encode(srslte_pdsch_t *q, 
+int srslte_pdsch_encode(srslte_pdsch_t *q,
                         srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
-                        uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) 
+                        uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
 {
-  
+
   int i;
   /* Set pointers for layermapping & precoding */
   cf_t *x[SRSLTE_MAX_LAYERS];
-  int ret = SRSLTE_ERROR_INVALID_INPUTS; 
-   
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
    if (q             != NULL &&
        cfg  != NULL)
   {
@@ -732,11 +891,11 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
         return SRSLTE_ERROR_INVALID_INPUTS;
       }
     }
-    
+
     if (cfg->grant.mcs.tbs == 0) {
-      return SRSLTE_ERROR_INVALID_INPUTS;      
+      return SRSLTE_ERROR_INVALID_INPUTS;
     }
-    
+
     if (cfg->nbits.nof_re > q->max_re) {
       fprintf(stderr,
           "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
@@ -745,7 +904,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     }
 
     INFO("Encoding PDSCH SF: %d, Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, 
+        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs,
          cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv);
 
     /* number of layers equals number of ports */
@@ -754,28 +913,28 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     }
     memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
 
-    if (srslte_dlsch_encode(&q->dl_sch, cfg, softbuffer, data, q->e)) {
+    if (srslte_dlsch_encode(&q->dl_sch[0], cfg, softbuffer, data, q->e[0])) {
       fprintf(stderr, "Error encoding TB\n");
       return SRSLTE_ERROR;
     }
 
     /* scramble */
     if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
+      srslte_scrambling_bytes(&q->users[rnti]->seq[0][cfg->sf_idx], (uint8_t*) q->e[0], cfg->nbits.nof_bits);
     } else {
       srslte_sequence_t seq;
       if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
         return SRSLTE_ERROR;
       }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
+      srslte_scrambling_bytes(&seq, (uint8_t*) q->e[0], cfg->nbits.nof_bits);
       srslte_sequence_free(&seq);
     }
-    
-    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e, q->d, cfg->nbits.nof_bits);
-    
+
+    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e[0], q->d[0], cfg->nbits.nof_bits);
+
     /* TODO: only diversity supported */
     if (q->cell.nof_ports > 1) {
-      srslte_layermap_diversity(q->d, x, q->cell.nof_ports, cfg->nbits.nof_re);
+      srslte_layermap_diversity(q->d[0], x, q->cell.nof_ports, cfg->nbits.nof_re);
       srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
           cfg->nbits.nof_re / q->cell.nof_ports);
     } else {
@@ -786,10 +945,10 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     for (i = 0; i < q->cell.nof_ports; i++) {
       srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
     }
-    
+
     ret = SRSLTE_SUCCESS;
-  } 
-  return ret; 
+  }
+  return ret;
 }
 
 int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
@@ -822,73 +981,57 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    INFO("Encoding PDSCH SF: %d (TB 1), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-         cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs,
-         cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv);
-    if (cfg->grant.nof_tb > 1) {
-      INFO("Encoding PDSCH SF: %d (TB 2), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-           cfg->sf_idx, srslte_mod_string(cfg->grant.mcs2.mod), cfg->grant.mcs2.tbs,
-           cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv2);
-    }
+#ifndef SRSLTE_SINGLE_THREAD
 
-    /* number of layers equals number of ports */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      x[i] = q->x[i];
+    for (int tb = 0; tb < cfg->grant.nof_tb; tb++) {
+      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[tb];
+      thread_args->cfg = cfg;
+      thread_args->softbuffer = &softbuffers[tb];
+      thread_args->data = data[tb];
+      thread_args->rnti = rnti;
+      sem_post(&thread_args->start);
     }
-    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
 
-    if (srslte_dlsch_encode_multi(&q->dl_sch, cfg, softbuffers, data, (uint8_t *[SRSLTE_MAX_CODEWORDS]) {q->e, q->e2})) {
-      fprintf(stderr, "Error encoding TB\n");
-      return SRSLTE_ERROR;
+    for (int tb = 0; tb < cfg->grant.nof_tb; tb++) {
+      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[tb];
+      sem_wait(&thread_args->finish);
+      ret |= thread_args->ret_status;
     }
 
-    /* scramble */
-    if (!q->users[rnti]) {
-      srslte_sequence_t seq;
+#else
+    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
+      ret |= srslte_pdsch_codeword_encode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
+    }
+#endif /* SRSLTE_SINGLE_THREAD */
 
-      if (cfg->nbits.nof_bits) {
-        if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-          return SRSLTE_ERROR;
+    if (q->cell.nof_ports > 1) {
+      int nof_symbols;
+      /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
+      if (cfg->nof_layers == cfg->grant.nof_tb) {
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->d[i];
         }
-        srslte_scrambling_bytes(&seq, (uint8_t *) q->e, cfg->nbits.nof_bits);
-        srslte_sequence_free(&seq);
-      }
-
-      if (cfg->nbits2.nof_bits) {
-        if (srslte_sequence_pdsch(&seq, rnti, 1, 2 * cfg->sf_idx, q->cell.id, cfg->nbits2.nof_bits)) {
-          return SRSLTE_ERROR;
+        nof_symbols = cfg->nbits.nof_re;
+      } else {
+        /* Initialise layer map pointers */
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->x[i];
         }
-        srslte_scrambling_bytes(&seq, (uint8_t *) q->e2, cfg->nbits2.nof_bits);
-        srslte_sequence_free(&seq);
-      }
-    } else {
-      if (cfg->nbits.nof_bits) {
-        srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t *) q->e, cfg->nbits.nof_bits);
-      }
+        memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-      if (cfg->nbits2.nof_bits) {
-        srslte_scrambling_bytes(&q->users[rnti]->seq2[cfg->sf_idx], (uint8_t *) q->e2, cfg->nbits2.nof_bits);
+        nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
+                                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits.nof_re, cfg->nbits2.nof_re}, cfg->mimo_type);
       }
-    }
-
-    if (cfg->nbits.nof_bits) {
-      srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t *) q->e, q->d, cfg->nbits.nof_bits);
-    }
 
-    if (cfg->nbits2.nof_bits) {
-      srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs2.mod], (uint8_t *) q->e2, q->d2, cfg->nbits2.nof_bits);
-    }
-
-    if (q->cell.nof_ports > 1) {
-      int nof_symbols = srslte_layermap_type((cf_t *[SRSLTE_MAX_CODEWORDS]) {q->d, q->d2}, x, cfg->grant.nof_tb, cfg->nof_layers,
-                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits.nof_re, cfg->nbits2.nof_re}, cfg->mimo_type);
+      /* Precode */
       srslte_precoding_type(x, q->symbols, cfg->nof_layers, q->cell.nof_ports, cfg->codebook_idx,
                             nof_symbols, cfg->mimo_type);
     } else {
-      memcpy(q->symbols[0], q->d, cfg->nbits.nof_re * sizeof(cf_t));
+      memcpy(q->symbols[0], q->d[0], cfg->nbits.nof_re * sizeof(cf_t));
     }
 
     /* mapping to resource elements */
+
     for (i = 0; i < q->cell.nof_ports; i++) {
       srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
     }
@@ -898,13 +1041,18 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
   return ret;
 }
 
-float srslte_pdsch_average_noi(srslte_pdsch_t *q) 
-{
-  return q->dl_sch.average_nof_iterations;
+void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, int max_iter) {
+  for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
+    srslte_sch_set_max_noi(&q->dl_sch[cw], max_iter);
+  }
+}
+
+float srslte_pdsch_average_noi(srslte_pdsch_t *q) {
+  return q->dl_sch[0].average_nof_iterations;
 }
 
 uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q) {
-  return q->dl_sch.nof_iterations;
+  return q->dl_sch[0].nof_iterations;
 }
 
 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 1caacacc9..4b78f6228 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -528,24 +528,25 @@ int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                         int16_t *e_bits, uint8_t *data, int codeword_idx)
 {
   uint32_t Nl = 1;
+  int ret = SRSLTE_ERROR;
 
   if (cfg->nof_layers != cfg->grant.nof_tb) {
     Nl = 2;
   }
 
   if (codeword_idx == 0) {
-    return decode_tb(q, softbuffer, &cfg->cb_segm,
+    ret = decode_tb(q, softbuffer, &cfg->cb_segm,
                      cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
                      e_bits, data);
-  }
-
-  if (codeword_idx == 1) {
-    return decode_tb(q, softbuffer, &cfg->cb_segm2,
+  } else if (codeword_idx == 1) {
+    ret = decode_tb(q, softbuffer, &cfg->cb_segm2,
                      cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
                      e_bits, data);
+  } else {
+    ERROR("Not implemented");
   }
 
-  return SRSLTE_ERROR;
+  return ret;
 }
 
 /**
@@ -567,6 +568,28 @@ int srslte_dlsch_encode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
                    data, e_bits);
 }
 
+int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
+                              uint8_t *data, uint8_t *e_bits, int codeword_idx) {
+  int ret = SRSLTE_ERROR;
+  uint32_t Nl = 1;
+
+  if (cfg->nof_layers != cfg->grant.nof_tb) {
+    Nl = 2;
+  }
+
+  if(codeword_idx == 0) {
+    /* Codeword 1 shall be encoded */
+    ret = encode_tb(q, softbuffer, &cfg->cb_segm, cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits, data, e_bits);
+  } else if(codeword_idx == 1) {
+    /* Codeword 2 shall be encoded */
+    ret = encode_tb(q, softbuffer, &cfg->cb_segm2, cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits, data, e_bits);
+  } else {
+    ERROR("Not implemented");
+  }
+
+  return ret;
+}
+
 int srslte_dlsch_encode_multi(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
                         uint8_t *data[SRSLTE_MAX_CODEWORDS], uint8_t *e_bits[SRSLTE_MAX_CODEWORDS])
 {
diff --git a/lib/src/phy/phch/test/dlsch_encode_test_mex.c b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
index f391ab111..c6181d9c5 100644
--- a/lib/src/phy/phch/test/dlsch_encode_test_mex.c
+++ b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
@@ -46,7 +46,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   int i;
   srslte_sch_t dlsch;
   srslte_pdsch_cfg_t cfg; 
-  srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
+  srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS];
   uint32_t nof_codewords = 1;
 
   memset(&dlsch, 0, sizeof(srslte_sch_t));
@@ -107,8 +107,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 
   /* Initialise buffers */
   for (i = 0; i < nof_codewords; i++) {
-    softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_tx_t));
-    if (srslte_softbuffer_tx_init(softbuffers[i], cell.nof_prb)) {
+    if (srslte_softbuffer_tx_init(&softbuffers[i], cell.nof_prb)) {
       mexErrMsgTxt("Error initiating DL-SCH soft buffer\n");
       return;
     }
@@ -151,8 +150,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   free(e_bits);
   free(e_bits_unpacked);
   for (i = 0; i < nof_codewords; i++) {
-    srslte_softbuffer_tx_free(softbuffers[i]);
-    free(softbuffers[i]);
+    srslte_softbuffer_tx_free(&softbuffers[i]);
   }
   return;
 }
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 8c79cc836..180e689d6 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -155,7 +155,8 @@ int main(int argc, char **argv) {
   int ret = -1;
   struct timeval t[3];
   srslte_softbuffer_tx_t softbuffers_tx[SRSLTE_MAX_CODEWORDS];
-  
+  int M=10;
+
   parse_args(argc,argv);
 
   /* Initialise to zeros */
@@ -265,6 +266,7 @@ int main(int argc, char **argv) {
       perror("srslte_vec_malloc");
       goto quit;
     }
+    bzero(data[0], sizeof(uint8_t) * grant.mcs.tbs);
   }
 
   if (grant.mcs2.tbs) {
@@ -273,6 +275,7 @@ int main(int argc, char **argv) {
       perror("srslte_vec_malloc");
       goto quit;
     }
+    bzero(data[1], sizeof(uint8_t) * grant.mcs2.tbs);
   }
 
   if (srslte_pdsch_init_rx_multi(&pdsch_rx, cell, nof_rx_antennas)) {
@@ -388,10 +391,18 @@ int main(int argc, char **argv) {
     }
     pdsch_cfg.rv = rv_idx;
     pdsch_cfg.rv2 = rv_idx2;
-    if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
-      fprintf(stderr, "Error encoding PDSCH\n");
-      goto quit;
+    gettimeofday(&t[1], NULL);
+    for (k = 0; k < M; k++) {
+      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
+        ERROR("Error encoding PDSCH");
+        goto quit;
+      }
     }
+    gettimeofday(&t[2], NULL);
+    get_time_interval(t);
+    printf("ENCODED in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n",
+           (float) t[0].tv_usec/M, (float) (grant.mcs.tbs + grant.mcs2.tbs)/1000.0f,
+           (float) (grant.mcs.tbs + grant.mcs2.tbs)*M/t[0].tv_usec);
 
   #ifdef DO_OFDM
     for (i = 0; i < cell.nof_ports; i++) {
@@ -422,9 +433,9 @@ int main(int argc, char **argv) {
 
 
   }
-  int M=10;
-  int r=0; 
-  srslte_sch_set_max_noi(&pdsch_rx.dl_sch, 10);
+  int r=0;
+  srslte_pdsch_set_max_noi(&pdsch_rx, 10);
+
   gettimeofday(&t[1], NULL);
   for (k = 0; k < M; k++) {
 #ifdef DO_OFDM
@@ -443,7 +454,7 @@ int main(int argc, char **argv) {
   }
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
-  INFO("DECODED %s in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n", r?"Error":"OK",
+  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 + grant.mcs2.tbs)/1000.0f,
          (float) (grant.mcs.tbs + grant.mcs2.tbs)*M/t[0].tv_usec);
   if (r) {
diff --git a/lib/src/phy/phch/test/pdsch_test_mex.c b/lib/src/phy/phch/test/pdsch_test_mex.c
index 5093c326d..eb198bcbe 100644
--- a/lib/src/phy/phch/test/pdsch_test_mex.c
+++ b/lib/src/phy/phch/test/pdsch_test_mex.c
@@ -213,7 +213,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     return;
   }
 
-  srslte_sch_set_max_noi(&pdsch.dl_sch, max_iterations);
+  srslte_pdsch_set_max_noi(&pdsch, max_iterations);
 
   bool input_fft_allocated = false; 
   int r=-1;
@@ -291,10 +291,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexutils_write_cf(pdsch.symbols[0], &plhs[2], cfg.nbits.nof_re, 1);  
   }
   if (nlhs >= 4) {
-    mexutils_write_cf(pdsch.d, &plhs[3], cfg.nbits.nof_re, 1);  
+    mexutils_write_cf(pdsch.d[0], &plhs[3], cfg.nbits.nof_re, 1);
   }
   if (nlhs >= 5) {
-    mexutils_write_s(pdsch.e, &plhs[4], cfg.nbits.nof_bits, 1);  
+    mexutils_write_s(pdsch.e[0], &plhs[4], cfg.nbits.nof_bits, 1);
   }
   if (nlhs >= 6) {
     uint32_t len = nof_antennas*cell.nof_ports*SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index c068f5899..85e851a3e 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -293,19 +293,19 @@ int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant,
   uint32_t pmi = 0;
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
-  if (q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-    if (q->pdsch_cfg.grant.nof_tb == 1) {
+  if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+    if (grant->nof_tb == 1) {
       if (pinfo > 0 && pinfo < 5) {
         pmi = pinfo - 1;
       } else {
-        ERROR("Not Implemented");
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, pinfo);
         return SRSLTE_ERROR;
       }
     } else {
       if (pinfo < 2) {
         pmi = pinfo;
       } else {
-        ERROR("Not Implemented");
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, pinfo);
         return SRSLTE_ERROR;
       }
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index ef3640a40..fce931837 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -432,7 +432,7 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
         
         /* Set decoder iterations */
         if (phy->args->pdsch_max_its > 0) {
-          srslte_sch_set_max_noi(&ue_dl.pdsch.dl_sch, phy->args->pdsch_max_its);
+          srslte_pdsch_set_max_noi(&ue_dl.pdsch, phy->args->pdsch_max_its);
         }
 
         

From 801b210511a39d84635af5bca5ee372fc7aab0ee Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 10:23:28 +0200
Subject: [PATCH 072/269] Include open RF multi

---
 lib/include/srslte/phy/rf/rf.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index bc74d17e7..42b2a691d 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -73,7 +73,12 @@ SRSLTE_API int srslte_rf_open(srslte_rf_t *h,
 
 SRSLTE_API int srslte_rf_open_multi(srslte_rf_t *h, 
                                     char *args, 
-                                    uint32_t nof_rx_antennas); 
+                                    uint32_t nof_rx_antennas);
+
+SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
+                                     char *args,
+                                     uint32_t nof_tx_antennas,
+                                     uint32_t nof_rx_antennas);
 
 SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
                                      char *args,

From f2db2db81b21e2a7291553a1fd6ec0a30bb4000d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 17 Aug 2017 16:23:43 +0200
Subject: [PATCH 073/269] Added PMI select C test and solved bugs (golden
 vectors generated from reference script)

---
 lib/src/phy/mimo/precoding.c            | 106 ++++++-------
 lib/src/phy/mimo/test/CMakeLists.txt    |   9 ++
 lib/src/phy/mimo/test/pmi_select_test.c | 144 +++++++++++++++++
 lib/src/phy/mimo/test/pmi_select_test.h | 203 ++++++++++++++++++++++++
 4 files changed, 406 insertions(+), 56 deletions(-)
 create mode 100644 lib/src/phy/mimo/test/pmi_select_test.c
 create mode 100644 lib/src/phy/mimo/test/pmi_select_test.h

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 37081d225..a79b75b0a 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -25,7 +25,6 @@
  */
 
 #include <stdlib.h>
-#include <assert.h>
 #include <complex.h>
 #include <string.h>
 #include <math.h>
@@ -1985,7 +1984,7 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
 
     for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
       /* 0. Load channel matrix */
-      __m256 h00 = _mm256_set_ps(crealf(h[0][0][j]),
+      __m256 h00 = _mm256_setr_ps(crealf(h[0][0][j]),
                                  cimagf(h[0][0][j]),
                                  crealf(h[0][0][j + PMI_SEL_PRECISION]),
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION]),
@@ -1993,7 +1992,7 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
-      __m256 h01 = _mm256_set_ps(crealf(h[1][0][j]),
+      __m256 h01 = _mm256_setr_ps(crealf(h[1][0][j]),
                                  cimagf(h[1][0][j]),
                                  crealf(h[1][0][j + PMI_SEL_PRECISION]),
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION]),
@@ -2001,7 +2000,7 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
-      __m256 h10 = _mm256_set_ps(crealf(h[0][1][j]),
+      __m256 h10 = _mm256_setr_ps(crealf(h[0][1][j]),
                                  cimagf(h[0][1][j]),
                                  crealf(h[0][1][j + PMI_SEL_PRECISION]),
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION]),
@@ -2009,7 +2008,7 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
-      __m256 h11 = _mm256_set_ps(crealf(h[1][1][j]),
+      __m256 h11 = _mm256_setr_ps(crealf(h[1][1][j]),
                                  cimagf(h[1][1][j]),
                                  crealf(h[1][1][j + PMI_SEL_PRECISION]),
                                  cimagf(h[1][1][j + PMI_SEL_PRECISION]),
@@ -2030,13 +2029,13 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
           a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
           break;
         case 2:
-          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
-          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
-          break;
-        default:
           a0 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
           a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
           break;
+        default:
+          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
       }
 
       /* 2. B = W' * H' * H = A * H */
@@ -2058,10 +2057,10 @@ int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
           c = _mm256_sub_ps(b0, b1);
           break;
         case 2:
-          c = _mm256_sub_ps(b0, _MM256_MULJ_PS(b1));
+          c = _mm256_add_ps(b0, _MM256_MULJ_PS(b1));
           break;
         case 3:
-          c = _mm256_add_ps(b0, _MM256_MULJ_PS(b1));
+          c = _mm256_sub_ps(b0, _MM256_MULJ_PS(b1));
           break;
         default:
           return SRSLTE_ERROR;
@@ -2216,7 +2215,9 @@ int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
   float max_sinr = 0.0;
   uint32_t i, count;
 
-  __m128 sse_noise_estimate = (__m128) {noise_estimate, 0.0f, noise_estimate, 0.0f};
+  __m128 sse_noise_estimate = _mm_setr_ps(noise_estimate, 0.0f, noise_estimate, 0.0f);
+  __m128 sse_norm = _mm_set1_ps(0.25f);
+  __m128 sse_ones = _mm_set1_ps(1.0f);
 
   for (i = 0; i < 2; i++) {
     sinr_list[i] = 0;
@@ -2224,22 +2225,22 @@ int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
 
     for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 2 + 1; j += PMI_SEL_PRECISION * 2) {
       /* 0. Load channel matrix */
-      __m128 h00 = _mm_set_ps(crealf(h[0][0][j]),
-                              cimagf(h[0][0][j]),
-                              crealf(h[0][0][j + PMI_SEL_PRECISION]),
-                              cimagf(h[0][0][j + PMI_SEL_PRECISION]));
-      __m128 h01 = _mm_set_ps(crealf(h[1][0][j]),
-                              cimagf(h[1][0][j]),
-                              crealf(h[1][0][j + PMI_SEL_PRECISION]),
-                              cimagf(h[1][0][j + PMI_SEL_PRECISION]));
-      __m128 h10 = _mm_set_ps(crealf(h[0][1][j]),
-                              cimagf(h[0][1][j]),
-                              crealf(h[0][1][j + PMI_SEL_PRECISION]),
-                              cimagf(h[0][1][j + PMI_SEL_PRECISION]));
-      __m128 h11 = _mm_set_ps(crealf(h[1][1][j]),
-                              cimagf(h[1][1][j]),
-                              crealf(h[1][1][j + PMI_SEL_PRECISION]),
-                              cimagf(h[1][1][j + PMI_SEL_PRECISION]));
+      __m128 h00 = _mm_setr_ps(crealf(h[0][0][j]),
+                               cimagf(h[0][0][j]),
+                               crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                               cimagf(h[0][0][j + PMI_SEL_PRECISION]));
+      __m128 h01 = _mm_setr_ps(crealf(h[1][0][j]),
+                               cimagf(h[1][0][j]),
+                               crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                               cimagf(h[1][0][j + PMI_SEL_PRECISION]));
+      __m128 h10 = _mm_setr_ps(crealf(h[0][1][j]),
+                               cimagf(h[0][1][j]),
+                               crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                               cimagf(h[0][1][j + PMI_SEL_PRECISION]));
+      __m128 h11 = _mm_setr_ps(crealf(h[1][1][j]),
+                               cimagf(h[1][1][j]),
+                               crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                               cimagf(h[1][1][j + PMI_SEL_PRECISION]));
 
       /* 1. B = W'* H' */
       __m128 a00, a01, a10, a11;
@@ -2284,38 +2285,38 @@ int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
         default:
           return SRSLTE_ERROR;
       }
-      c00 = _mm_mul_ps(c00, _mm_set1_ps(0.25f));
-      c01 = _mm_mul_ps(c01, _mm_set1_ps(0.25f));
-      c10 = _mm_mul_ps(c10, _mm_set1_ps(0.25f));
-      c11 = _mm_mul_ps(c11, _mm_set1_ps(0.25f));
+      c00 = _mm_mul_ps(c00, sse_norm);
+      c01 = _mm_mul_ps(c01, sse_norm);
+      c10 = _mm_mul_ps(c10, sse_norm);
+      c11 = _mm_mul_ps(c11, sse_norm);
 
       /* 4. C += noise * I */
       c00 = _mm_add_ps(c00, sse_noise_estimate);
       c11 = _mm_add_ps(c11, sse_noise_estimate);
 
       /* 5. detC */
-      __m128 detC = _mm_sub_ps(_MM_PROD_PS(c00, c11), _MM_PROD_PS(c01, c10));
+      __m128 detC = srslte_algebra_2x2_det_sse(c00, c01, c10, c11);
       __m128 inv_detC = srslte_algebra_cf_recip_sse(detC);
+      inv_detC = _mm_mul_ps(sse_noise_estimate, inv_detC);
 
       __m128 den0 = _MM_PROD_PS(c00, inv_detC);
       __m128 den1 = _MM_PROD_PS(c11, inv_detC);
 
-      __m128 gamma0 = _mm_sub_ps(srslte_algebra_cf_recip_sse(den0), _mm_set1_ps(1.0f));
-      __m128 gamma1 = _mm_sub_ps(srslte_algebra_cf_recip_sse(den1), _mm_set1_ps(1.0f));
+      __m128 gamma0 = _mm_sub_ps(_mm_rcp_ps(den0), sse_ones);
+      __m128 gamma1 = _mm_sub_ps(_mm_rcp_ps(den1), sse_ones);
 
       /* Add for averaging */
-      __m128 sum = _MM_SWAP(_mm_add_ps(gamma0, gamma1));
-      __m128 sinr_sse = _mm_hadd_ps(sum, sum);
+      __m128 sinr_sse = _mm_add_ps(gamma0, gamma1);
       __attribute__((aligned(128))) float sinr[4];
       _mm_store_ps(sinr, sinr_sse);
 
-      sinr_list[i] += sinr[0];
+      sinr_list[i] += sinr[0] + sinr[2];
 
       count += 2;
     }
 
     /* Divide average by noise */
-    sinr_list[i] /= (2 * count * noise_estimate);
+    sinr_list[i] /= 2 * count;
 
     if (sinr_list[i] > max_sinr) {
       max_sinr = sinr_list[i];
@@ -2339,7 +2340,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
 
   __m256 avx_noise_estimate = _mm256_setr_ps(noise_estimate, 0.0f, noise_estimate, 0.0f,
                                         noise_estimate, 0.0f, noise_estimate, 0.0f);
-  __m256 avx_norm = _mm256_set1_ps(0.5f);
+  __m256 avx_norm = _mm256_set1_ps(0.25f);
   __m256 avx_ones = _mm256_set1_ps(1.0f);
 
   for (i = 0; i < 2; i++) {
@@ -2348,7 +2349,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
 
     for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
       /* 0. Load channel matrix */
-      __m256 h00 = _mm256_set_ps(crealf(h[0][0][j]),
+      __m256 h00 = _mm256_setr_ps(crealf(h[0][0][j]),
                                  cimagf(h[0][0][j]),
                                  crealf(h[0][0][j + PMI_SEL_PRECISION]),
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION]),
@@ -2356,7 +2357,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
-      __m256 h01 = _mm256_set_ps(crealf(h[1][0][j]),
+      __m256 h01 = _mm256_setr_ps(crealf(h[1][0][j]),
                                  cimagf(h[1][0][j]),
                                  crealf(h[1][0][j + PMI_SEL_PRECISION]),
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION]),
@@ -2364,7 +2365,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
-      __m256 h10 = _mm256_set_ps(crealf(h[0][1][j]),
+      __m256 h10 = _mm256_setr_ps(crealf(h[0][1][j]),
                                  cimagf(h[0][1][j]),
                                  crealf(h[0][1][j + PMI_SEL_PRECISION]),
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION]),
@@ -2372,7 +2373,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
                                  crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
                                  cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
-      __m256 h11 = _mm256_set_ps(crealf(h[1][1][j]),
+      __m256 h11 = _mm256_setr_ps(crealf(h[1][1][j]),
                                  cimagf(h[1][1][j]),
                                  crealf(h[1][1][j + PMI_SEL_PRECISION]),
                                  cimagf(h[1][1][j + PMI_SEL_PRECISION]),
@@ -2399,10 +2400,6 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
         default:
           return SRSLTE_ERROR;
       }
-      a00 = _mm256_mul_ps(a00, avx_norm);
-      a01 = _mm256_mul_ps(a01, avx_norm);
-      a10 = _mm256_mul_ps(a10, avx_norm);
-      a11 = _mm256_mul_ps(a11, avx_norm);
 
       /* 2. B = W' * H' * H = A * H */
 #ifdef LV_HAVE_FMA
@@ -2445,18 +2442,15 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
       c11 = _mm256_add_ps(c11, avx_noise_estimate);
 
       /* 5. detC */
-#ifdef LV_HAVE_FMA
-      __m256 detC = _MM256_PROD_SUB_PS(c00, c11, _MM256_PROD_PS(c01, c10));
-#else
-      __m256 detC = _mm256_sub_ps(_MM256_PROD_PS(c00, c11), _MM256_PROD_PS(c01, c10));
-#endif /* LV_HAVE_FMA */
+      __m256 detC = srslte_algebra_2x2_det_avx(c00, c01, c10, c11);
       __m256 inv_detC = srslte_algebra_cf_recip_avx(detC);
+      inv_detC = _mm256_mul_ps(avx_noise_estimate, inv_detC);
 
       __m256 den0 = _MM256_PROD_PS(c00, inv_detC);
       __m256 den1 = _MM256_PROD_PS(c11, inv_detC);
 
-      __m256 gamma0 = _mm256_sub_ps(srslte_algebra_cf_recip_avx(den0), avx_ones);
-      __m256 gamma1 = _mm256_sub_ps(srslte_algebra_cf_recip_avx(den1), avx_ones);
+      __m256 gamma0 = _mm256_sub_ps(_mm256_rcp_ps(den0), avx_ones);
+      __m256 gamma1 = _mm256_sub_ps(_mm256_rcp_ps(den1), avx_ones);
 
       /* Add for averaging */
       __m256 sinr_avx = _mm256_permute_ps(_mm256_add_ps(gamma0, gamma1), 0b00101000);
@@ -2469,7 +2463,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
     }
 
     /* Divide average by noise */
-    sinr_list[i] /= 2 * count * noise_estimate;
+    sinr_list[i] /= 2 * count;
 
     if (sinr_list[i] > max_sinr) {
       max_sinr = sinr_list[i];
diff --git a/lib/src/phy/mimo/test/CMakeLists.txt b/lib/src/phy/mimo/test/CMakeLists.txt
index 2397bfc49..cae668d9e 100644
--- a/lib/src/phy/mimo/test/CMakeLists.txt
+++ b/lib/src/phy/mimo/test/CMakeLists.txt
@@ -69,4 +69,13 @@ add_test(precoding_multiplex_2l_cb0_mmse precoding_test -m multiplex -l 2 -p 2 -
 add_test(precoding_multiplex_2l_cb1_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1 -d mmse)
 add_test(precoding_multiplex_2l_cb2_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2 -d mmse)
 
+########################################################################
+# PMI SELECT TEST
+########################################################################
+
+add_executable(pmi_select_test pmi_select_test.c)
+target_link_libraries(pmi_select_test srslte_phy)
+
+add_test(pmi_select_test pmi_select_test)
+
 
diff --git a/lib/src/phy/mimo/test/pmi_select_test.c b/lib/src/phy/mimo/test/pmi_select_test.c
new file mode 100644
index 000000000..30d358e37
--- /dev/null
+++ b/lib/src/phy/mimo/test/pmi_select_test.c
@@ -0,0 +1,144 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+#include <unistd.h>
+#include <math.h>
+#include <time.h>
+#include <stdbool.h>
+#include <complex.h>
+
+
+#include "srslte/phy/utils/vector.h"
+#include "srslte/phy/mimo/precoding.h"
+#include "pmi_select_test.h"
+#include "srslte/phy/utils/debug.h"
+
+int main(int argc, char **argv) {
+  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  float noise_estimate;
+  float sinr_1l[SRSLTE_MAX_CODEBOOKS];
+  float sinr_2l[SRSLTE_MAX_CODEBOOKS];
+  uint32_t pmi[2];
+  uint32_t nof_symbols = (uint32_t) SRSLTE_SF_LEN_RE(6, SRSLTE_CP_NORM);
+  int ret = SRSLTE_ERROR;
+
+  /* Allocate channels */
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      h[i][j] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
+      if (!h[i][j]) {
+        goto clean;
+      }
+      bzero(h[i][j], sizeof(cf_t) * nof_symbols);
+    }
+  }
+
+  for (int c = 0; c < PMI_SELECT_TEST_NOF_CASES; c++) {
+    pmi_select_test_case_gold_t *gold = &pmi_select_test_case_gold[c];
+
+    /* Set channel */
+    for (int i = 0; i < 2; i++) {
+      for (int j = 0; j < 2; j++) {
+        cf_t hij = gold->h[i][j];
+
+        for (int k = 0; k < nof_symbols; k++) {
+          h[i][j][k] = hij;
+        }
+      }
+    }
+
+    /* Set noise estimate */
+    noise_estimate = gold->n;
+
+    /* PMI select for 1 layer */
+    ret = srslte_precoding_pmi_select(h, nof_symbols, noise_estimate, 1, &pmi[0], sinr_1l);
+    if (ret < 0) {
+      ERROR("During PMI selection for 1 layer");
+      goto clean;
+    }
+
+    /* Check SINR for 1 layer */
+    for (int i = 0; i < ret; i++) {
+      if (fabsf(gold->snri_1l[i] - sinr_1l[i]) > 0.1) {
+        ERROR("Test case %d failed computing 1 layer SINR for codebook %d (test=%.2f; gold=%.2f)\n",
+              c + 1, i, sinr_1l[i], gold->snri_1l[i]);
+        goto clean;
+      }
+    }
+
+    /* Check PMI select for 1 layer*/
+    if (pmi[0] != gold->pmi[0]) {
+      ERROR("Test case %d failed computing 1 layer PMI (test=%d; gold=%d)\n", c + 1, pmi[0], gold->pmi[0]);
+      goto clean;
+    }
+
+    /* PMI select for 2 layer */
+    ret = srslte_precoding_pmi_select(h, nof_symbols, noise_estimate, 2, &pmi[1], sinr_2l);
+    if (ret < 0) {
+      ERROR("During PMI selection for 2 layer");
+      goto clean;
+    }
+
+    /* Check SINR for 2 layer */
+    for (int i = 0; i < ret; i++) {
+      if (fabsf(gold->snri_2l[i] - sinr_2l[i]) > 0.1) {
+        ERROR("Test case %d failed computing 2 layer SINR for codebook %d (test=%.2f; gold=%.2f)\n",
+              c + 1, i, sinr_2l[i], gold->snri_2l[i]);
+        goto clean;
+      }
+    }
+
+    /* Check PMI select for 2 layer*/
+    if (pmi[1] != gold->pmi[1]) {
+      ERROR("Test case %d failed computing 2 layer PMI (test=%d; gold=%d)\n", c + 1, pmi[1], gold->pmi[1]);
+      goto clean;
+    }
+  }
+
+  /* Test passed */
+  ret = SRSLTE_SUCCESS;
+
+  clean:
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      if (h[i][j]) {
+        free(h[i][j]);
+      }
+    }
+  }
+
+  if (ret) {
+    printf("Failed!\n");
+  } else {
+    printf("Passed!\n");
+  }
+
+  return ret;
+}
\ No newline at end of file
diff --git a/lib/src/phy/mimo/test/pmi_select_test.h b/lib/src/phy/mimo/test/pmi_select_test.h
new file mode 100644
index 000000000..a55869a8b
--- /dev/null
+++ b/lib/src/phy/mimo/test/pmi_select_test.h
@@ -0,0 +1,203 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef PMI_SELECT_TEST_H
+#define PMI_SELECT_TEST_H
+
+#define PMI_SELECT_TEST_NOF_CASES 16
+
+typedef struct {
+  cf_t h[2][2];
+  float n;
+  float snri_1l[4];
+  float snri_2l[2];
+  uint32_t pmi[2];
+} pmi_select_test_case_gold_t;
+
+static pmi_select_test_case_gold_t pmi_select_test_case_gold [PMI_SELECT_TEST_NOF_CASES] = {
+  { /* Test case 1 */
+    .h = {
+      {+0.103430f+0.455920f*_Complex_I, +0.042050f+0.751883f*_Complex_I}, 
+      {-0.641951f-0.053356f*_Complex_I, +0.217847f+0.504428f*_Complex_I}
+      },
+    .n = 0.181048, 
+    .snri_1l = {5.793827f, 2.505115f, 3.350922f, 4.948020f},
+    .snri_2l = {2.015732f, 1.848130f},
+    .pmi = {0, 0},
+  },
+  { /* Test case 2 */
+    .h = {
+      {-0.957328f-0.624816f*_Complex_I, -0.741457f-0.657570f*_Complex_I}, 
+      {+0.720104f+0.351137f*_Complex_I, +0.593419f-0.200211f*_Complex_I}
+      },
+    .n = 0.935527, 
+    .snri_1l = {0.475069f, 3.077055f, 1.078656f, 2.473467f},
+    .snri_2l = {0.747362f, 0.594324f},
+    .pmi = {1, 0},
+  },
+  { /* Test case 3 */
+    .h = {
+      {-0.047530f-0.118039f*_Complex_I, -0.195528f-0.724032f*_Complex_I}, 
+      {-0.619953f+0.960678f*_Complex_I, -0.325868f-0.120700f*_Complex_I}
+      },
+    .n = 0.803842, 
+    .snri_1l = {1.331730f, 1.164592f, 1.660155f, 0.836167f},
+    .snri_2l = {0.554942f, 0.579321f},
+    .pmi = {2, 1},
+  },
+  { /* Test case 4 */
+    .h = {
+      {+0.635330f-0.751786f*_Complex_I, -0.536944f-0.185884f*_Complex_I}, 
+      {+0.282517f-0.864615f*_Complex_I, -0.484380f-0.780479f*_Complex_I}
+      },
+    .n = 0.529556, 
+    .snri_1l = {5.128973f, 0.465969f, 2.812367f, 2.782574f},
+    .snri_2l = {1.381190f, 0.818813f},
+    .pmi = {0, 0},
+  },
+  { /* Test case 5 */
+    .h = {
+      {-0.576996f+0.964470f*_Complex_I, -0.948065f+0.902764f*_Complex_I}, 
+      {+0.988240f-0.056784f*_Complex_I, +0.489282f+0.975071f*_Complex_I}
+      },
+    .n = 0.852921, 
+    .snri_1l = {2.772684f, 3.261802f, 5.698031f, 0.336455f},
+    .snri_2l = {0.768370f, 1.469069f},
+    .pmi = {2, 1},
+  },
+  { /* Test case 6 */
+    .h = {
+      {-0.381846f-0.998609f*_Complex_I, +0.903472f-0.393687f*_Complex_I}, 
+      {-0.772703f-0.261637f*_Complex_I, -0.765452f-0.759318f*_Complex_I}
+      },
+    .n = 0.711912, 
+    .snri_1l = {2.998736f, 2.538860f, 5.099274f, 0.438323f},
+    .snri_2l = {0.809381f, 1.371548f},
+    .pmi = {2, 1},
+  },
+  { /* Test case 7 */
+    .h = {
+      {+0.915028f-0.780771f*_Complex_I, -0.355424f+0.447925f*_Complex_I}, 
+      {+0.577968f+0.765204f*_Complex_I, +0.342972f-0.999014f*_Complex_I}
+      },
+    .n = 0.101944, 
+    .snri_1l = {12.424177f, 24.940449f, 5.411339f, 31.953288f},
+    .snri_2l = {4.610588f, 7.664146f},
+    .pmi = {3, 1},
+  },
+  { /* Test case 8 */
+    .h = {
+      {-0.259232f-0.654765f*_Complex_I, +0.829378f-0.793859f*_Complex_I}, 
+      {+0.997978f+0.212295f*_Complex_I, -0.659012f-0.176220f*_Complex_I}
+      },
+    .n = 0.255783, 
+    .snri_1l = {3.345813f, 9.635433f, 6.767844f, 6.213402f},
+    .snri_2l = {3.215386f, 2.640976f},
+    .pmi = {1, 0},
+  },
+  { /* Test case 9 */
+    .h = {
+      {-0.596630f+0.244853f*_Complex_I, -0.624622f+0.316537f*_Complex_I}, 
+      {+0.767545f-0.645831f*_Complex_I, +0.262828f+0.251697f*_Complex_I}
+      },
+    .n = 0.876456, 
+    .snri_1l = {0.367264f, 1.965908f, 1.215674f, 1.117498f},
+    .snri_2l = {0.579923f, 0.479609f},
+    .pmi = {1, 0},
+  },
+  { /* Test case 10 */
+    .h = {
+      {-0.643594f+0.172442f*_Complex_I, +0.291148f-0.026254f*_Complex_I}, 
+      {+0.768244f+0.678173f*_Complex_I, -0.498968f-0.896649f*_Complex_I}
+      },
+    .n = 0.739473, 
+    .snri_1l = {1.104856f, 2.455074f, 2.920106f, 0.639825f},
+    .snri_2l = {0.557672f, 0.658911f},
+    .pmi = {2, 1},
+  },
+  { /* Test case 11 */
+    .h = {
+      {+0.109032f-0.285542f*_Complex_I, -0.141055f+0.318945f*_Complex_I}, 
+      {+0.559445f-0.211656f*_Complex_I, -0.206665f-0.643045f*_Complex_I}
+      },
+    .n = 0.054295, 
+    .snri_1l = {8.472397f, 10.480333f, 4.074631f, 14.878099f},
+    .snri_2l = {2.121444f, 2.979095f},
+    .pmi = {3, 1},
+  },
+  { /* Test case 12 */
+    .h = {
+      {-0.505758f-0.710501f*_Complex_I, +0.803627f+0.023333f*_Complex_I}, 
+      {+0.964886f+0.987055f*_Complex_I, -0.031782f+0.525138f*_Complex_I}
+      },
+    .n = 0.966024, 
+    .snri_1l = {0.612742f, 3.102514f, 1.227107f, 2.488149f},
+    .snri_2l = {0.848010f, 0.701000f},
+    .pmi = {1, 0},
+  },
+  { /* Test case 13 */
+    .h = {
+      {+0.859761f-0.737655f*_Complex_I, -0.527019f+0.509133f*_Complex_I}, 
+      {-0.804956f-0.303794f*_Complex_I, -0.180451f-0.100433f*_Complex_I}
+      },
+    .n = 0.199335, 
+    .snri_1l = {4.402551f, 8.656756f, 10.092319f, 2.966987f},
+    .snri_2l = {2.048224f, 2.462759f},
+    .pmi = {2, 1},
+  },
+  { /* Test case 14 */
+    .h = {
+      {+0.473036f+0.227467f*_Complex_I, -0.593265f-0.308456f*_Complex_I}, 
+      {+0.536321f+0.445264f*_Complex_I, -0.517440f-0.765554f*_Complex_I}
+      },
+    .n = 0.180788, 
+    .snri_1l = {10.671400f, 0.736020f, 3.584109f, 7.823311f},
+    .snri_2l = {2.029078f, 0.914443f},
+    .pmi = {0, 0},
+  },
+  { /* Test case 15 */
+    .h = {
+      {-0.612271f+0.338114f*_Complex_I, -0.278903f+0.914426f*_Complex_I}, 
+      {-0.191213f-0.136670f*_Complex_I, -0.548440f+0.607628f*_Complex_I}
+      },
+    .n = 0.798189, 
+    .snri_1l = {2.309797f, 0.356735f, 0.731443f, 1.935089f},
+    .snri_2l = {0.577612f, 0.490806f},
+    .pmi = {0, 0},
+  },
+  { /* Test case 16 */
+    .h = {
+      {+0.990482f+0.513519f*_Complex_I, -0.576391f+0.922553f*_Complex_I}, 
+      {-0.341962f+0.139785f*_Complex_I, +0.524684f+0.217012f*_Complex_I}
+      },
+    .n = 0.365092, 
+    .snri_1l = {2.942635f, 4.964827f, 4.761949f, 3.145513f},
+    .snri_2l = {1.291431f, 1.267123f},
+    .pmi = {1, 0},
+  },
+};
+
+#endif /* PMI_SELECT_TEST_H */

From ed19a4d2006ce5c0ede56f78e5154db13bd18526 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 18 Aug 2017 16:48:21 +0200
Subject: [PATCH 074/269] Removed PDSCH threads and refactored RI/PMI select

---
 lib/include/srslte/phy/phch/pdsch.h |  57 +-------
 lib/include/srslte/phy/ue/ue_dl.h   |   7 +-
 lib/src/phy/phch/pdsch.c            | 212 +++-------------------------
 lib/src/phy/ue/ue_dl.c              |  55 +++++++-
 4 files changed, 82 insertions(+), 249 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index b4620be69..9a9b16788 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -35,13 +35,6 @@
 #ifndef PDSCH_
 #define PDSCH_
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-#include <pthread.h>
-#include <semaphore.h>
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
 #include "srslte/config.h"
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/precoding.h"
@@ -59,34 +52,6 @@ typedef struct {
   bool sequence_generated;
 } srslte_pdsch_user_t;
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-typedef struct {
-  /* Thread identifier: they must set before thread creation */
-  uint32_t codeword_idx;
-  void *pdsch_ptr;
-
-  /* Configuration Encoder/Decoder: they must be set before posting start semaphore */
-  srslte_pdsch_cfg_t *cfg;
-  uint16_t rnti;
-
-  /* Encoder/Decoder data pointers: they must be set before posting start semaphore  */
-  uint8_t *data;
-  void *softbuffer;
-
-  /* Execution status */
-  int ret_status;
-
-  /* Semaphores */
-  sem_t start;
-  sem_t finish;
-
-  /* Thread kill flag */
-  bool quit;
-} srslte_pdsch_thread_args_t;
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
 /* PDSCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
@@ -111,13 +76,6 @@ typedef struct SRSLTE_API {
   
   srslte_sch_t dl_sch[SRSLTE_MAX_CODEWORDS];
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-  pthread_t threads[SRSLTE_MAX_CODEWORDS];
-  srslte_pdsch_thread_args_t thread_args[SRSLTE_MAX_CODEWORDS];
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
 } srslte_pdsch_t;
 
 SRSLTE_API int srslte_pdsch_init(srslte_pdsch_t *q, 
@@ -190,14 +148,13 @@ SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
                                          uint16_t rnti,
                                          uint8_t *data[SRSLTE_MAX_CODEWORDS]);
 
-SRSLTE_API int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
-                                          srslte_pdsch_cfg_t *cfg,
-                                          cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                          float noise_estimate,
-                                          uint32_t nof_ce,
-                                          uint32_t *ri,
-                                          uint32_t *pmi,
-                                          float *current_sinr);
+SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
+                                       srslte_pdsch_cfg_t *cfg,
+                                       cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                       float noise_estimate,
+                                       uint32_t nof_ce,
+                                       uint32_t pmi[SRSLTE_MAX_LAYERS],
+                                       float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]);
 
 SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, int max_iter);
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 88442b93d..0fc8f9422 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -94,7 +94,12 @@ typedef struct SRSLTE_API {
   cf_t *sf_symbols_m[SRSLTE_MAX_PORTS]; 
   cf_t *ce[SRSLTE_MAX_PORTS]; // compatibility
   cf_t *ce_m[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
+
+  /* RI, PMI and SINR for MIMO statistics */
+  float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS];
+  uint32_t pmi[SRSLTE_MAX_LAYERS];
+  uint32_t ri;
+
   srslte_dci_format_t dci_format;
   uint64_t pkt_errors; 
   uint64_t pkts_total;
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 3ff2a910b..31642751f 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -33,13 +33,6 @@
 #include <assert.h>
 #include <math.h>
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-#include <pthread.h>
-#include <semaphore.h>
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
 #include "srslte/phy/phch/sch.h"
@@ -64,13 +57,6 @@ extern int indices[100000];
 extern int indices_ptr; 
 #endif
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-static void *srslte_pdsch_encode_thread (void *arg);
-static void *srslte_pdsch_decode_thread (void *arg);
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
 float srslte_pdsch_coderate(uint32_t tbs, uint32_t nof_re) 
 {
   return (float) (tbs + 24)/(nof_re);
@@ -263,20 +249,6 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
         goto clean;
       }
 
-#ifndef SRSLTE_SINGLE_THREAD
-      if (sem_init(&q->thread_args[i].start, 0, 0)) {
-        ERROR("Creating semaphore");
-        goto clean;
-      }
-      if (sem_init(&q->thread_args[i].finish, 0, 0)) {
-        ERROR("Creating semaphore");
-        goto clean;
-      }
-      q->thread_args[i].codeword_idx = (uint32_t) i;
-      q->thread_args[i].pdsch_ptr = q;
-      pthread_create(&q->threads[i], NULL, (is_receiver) ? srslte_pdsch_decode_thread : srslte_pdsch_encode_thread,
-                     (void *) &q->thread_args[i]);
-#endif /* SRSLTE_SINGLE_THREAD */
     }
 
     /* Layer mapped symbols memory allocation */
@@ -339,15 +311,6 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
 
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
 
-#ifndef SRSLTE_SINGLE_THREAD
-    /* Stop threads */
-    q->thread_args[i].quit = true;
-    sem_post(&q->thread_args[i].start);
-    pthread_join(q->threads[i], NULL);
-    pthread_detach(q->threads[i]);
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
     if (q->e[i]) {
       free(q->e[i]);
     }
@@ -359,7 +322,6 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     /* Free sch objects */
     srslte_sch_free(&q->dl_sch[i]);
 
-
   }
 
   for (i = 0; i < q->cell.nof_ports; i++) {
@@ -506,7 +468,7 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
   return SRSLTE_SUCCESS;
 }
 
-static inline int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
   srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
@@ -550,33 +512,7 @@ static inline int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pds
   return SRSLTE_SUCCESS;
 }
 
-
-#ifndef SRSLTE_SINGLE_THREAD
-
-static void *srslte_pdsch_encode_thread(void *arg) {
-  srslte_pdsch_thread_args_t *q = (srslte_pdsch_thread_args_t *) arg;
-  uint32_t codeword_idx = q->codeword_idx;
-
-  INFO("[PDSCH Encoder CW %d] waiting for data\n", codeword_idx);
-
-  sem_wait(&q->start);
-  while (!q->quit) {
-    q->ret_status = srslte_pdsch_codeword_encode(q->pdsch_ptr, q->cfg, q->softbuffer, q->rnti, q->data, codeword_idx);
-
-    /* Post finish semaphore */
-    sem_post(&q->finish);
-
-    /* Wait for next loop */
-    sem_wait(&q->start);
-  }
-
-  pthread_exit(NULL);
-  return q;
-}
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
-static inline int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
+static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
   srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
@@ -588,6 +524,10 @@ static inline int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pds
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
          nbits->nof_re, nbits->nof_bits, (codeword_idx == 0) ? cfg->rv : cfg->rv2);
 
+    /* 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
+     */
     srslte_demod_soft_demodulate_s(mcs->mod, pdsch->d[codeword_idx], pdsch->e[codeword_idx], cfg->nbits.nof_re);
 
     if (pdsch->users[rnti] && pdsch->users[rnti]->sequence_generated) {
@@ -609,32 +549,6 @@ static inline int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pds
   return SRSLTE_SUCCESS;
 }
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-static void *srslte_pdsch_decode_thread(void *arg) {
-  srslte_pdsch_thread_args_t *q = (srslte_pdsch_thread_args_t *) arg;
-  uint32_t codeword_idx = q->codeword_idx;
-
-  INFO("[PDSCH Encoder CW %d] waiting for data\n", codeword_idx);
-
-  sem_wait(&q->start);
-  while (!q->quit) {
-    q->ret_status = srslte_pdsch_codeword_decode(q->pdsch_ptr, q->cfg, q->softbuffer, q->rnti, q->data, codeword_idx);
-
-    /* Post finish semaphore */
-    sem_post(&q->finish);
-
-    /* Wait for next loop */
-    sem_wait(&q->start);
-  }
-
-  pthread_exit(NULL);
-  return q;
-}
-
-#endif /* SRSLTE_SINGLE_THREAD */
-
-
 void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 {
   if (q->users[rnti]) {
@@ -760,32 +674,9 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits.nof_re*sizeof(cf_t));
     }
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-    for (i = 0; i < cfg->grant.nof_tb; i++) {
-      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[i];
-      thread_args->cfg = cfg;
-      thread_args->softbuffer = &softbuffers[i];
-      thread_args->data = data[i];
-      thread_args->rnti = rnti;
-      sem_post(&thread_args->start);
-    }
-
-    for (i = 0; i < cfg->grant.nof_tb; i++) {
-      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[i];
-      sem_wait(&thread_args->finish);
-      ret |= thread_args->ret_status;
-    }
-
-#else
-    /* 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
-    */
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
         ret |= srslte_pdsch_codeword_decode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
     }
-#endif /* SRSLTE_SINGLE_THREAD */
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
       DEBUG("SAVED FILE llr.dat: LLR estimates after demodulation and descrambling\n",0);
@@ -799,71 +690,21 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
   }
 }
 
-int srslte_pdsch_ri_pmi_select(srslte_pdsch_t *q,
-                               srslte_pdsch_cfg_t *cfg,
-                               cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
-                               uint32_t *ri, uint32_t *pmi,
-                               float *current_sinr) {
-  uint32_t best_pmi_1l;
-  uint32_t best_pmi_2l;
-  float sinr_1l[SRSLTE_MAX_CODEBOOKS];
-  float sinr_2l[SRSLTE_MAX_CODEBOOKS];
-  float best_sinr_1l = 0.0;
-  float best_sinr_2l = 0.0;
-  int n1, n2;
+int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
+                                  srslte_pdsch_cfg_t *cfg,
+                                  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
+                                  uint32_t pmi[SRSLTE_MAX_LAYERS], float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]) {
 
   if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
-    n1 = srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, 1, &best_pmi_1l, sinr_1l);
-    if (n1 < 0) {
-      ERROR("PMI Select for 1 layer");
-      return SRSLTE_ERROR;
-    }
-
-    n2 = srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, 2, &best_pmi_2l, sinr_2l);
-    if (n2 < 0) {
-      ERROR("PMI Select for 2 layer");
-      return SRSLTE_ERROR;
-    }
-
-    for (int i = 0; i < n1; i++) {
-      if (sinr_1l[i] > best_sinr_1l) {
-        best_sinr_1l = sinr_1l[i];
-      }
-    }
-
-    for (int i = 0; i < n2; i++) {
-      if (sinr_2l[i] > best_sinr_2l) {
-        best_sinr_2l = sinr_2l[i];
-      }
-    }
-
-    /* Set RI */
-    if (ri != NULL) {
-      *ri = (best_sinr_1l > best_sinr_2l) ? 1 : 2;
-    }
-
-    /* Set PMI */
-    if (pmi != NULL) {
-      *pmi = (best_sinr_1l > best_sinr_2l) ? best_pmi_1l : best_pmi_2l;
-    }
-
-    /* Set current SINR */
-    if (current_sinr != NULL && cfg->mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-      if (cfg->nof_layers == 1) {
-        *current_sinr = sinr_1l[cfg->codebook_idx];
-      } else if (cfg->nof_layers == 2) {
-        *current_sinr = sinr_2l[cfg->codebook_idx - 1];
-      } else {
-        ERROR("Not implemented number of layers");
-        return SRSLTE_ERROR;
+    for (int nof_layers = 1; nof_layers <= cfg->nof_layers; nof_layers++ ) {
+      if (sinr[nof_layers - 1] && pmi) {
+        if (srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, nof_layers, &pmi[nof_layers - 1],
+                                        sinr[nof_layers - 1]) < 0) {
+          ERROR("PMI Select for %d layers", nof_layers);
+          return SRSLTE_ERROR;
+        }
       }
     }
-
-    /* Print Trace */
-    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
-      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
-           10*log10(*current_sinr), cfg->nof_layers, cfg->codebook_idx);
-    }
   } else {
     ERROR("Not implemented configuration");
     return SRSLTE_ERROR_INVALID_INPUTS;
@@ -981,28 +822,9 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-#ifndef SRSLTE_SINGLE_THREAD
-
-    for (int tb = 0; tb < cfg->grant.nof_tb; tb++) {
-      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[tb];
-      thread_args->cfg = cfg;
-      thread_args->softbuffer = &softbuffers[tb];
-      thread_args->data = data[tb];
-      thread_args->rnti = rnti;
-      sem_post(&thread_args->start);
-    }
-
-    for (int tb = 0; tb < cfg->grant.nof_tb; tb++) {
-      srslte_pdsch_thread_args_t *thread_args = &q->thread_args[tb];
-      sem_wait(&thread_args->finish);
-      ret |= thread_args->ret_status;
-    }
-
-#else
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
       ret |= srslte_pdsch_codeword_encode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
     }
-#endif /* SRSLTE_SINGLE_THREAD */
 
     if (q->cell.nof_ports > 1) {
       int nof_symbols;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 85e851a3e..3f51a3036 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -454,9 +454,58 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
 
 int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
   float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
-  return srslte_pdsch_ri_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
-                                    SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp),
-                                    ri, pmi, current_sinr);
+  float best_sinr = -INFINITY;
+  uint32_t best_pmi = 0, best_ri = 0;
+
+  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+    if (srslte_pdsch_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
+                                  SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), q->pmi, q->sinr)) {
+      ERROR("SINR calculation error");
+      return SRSLTE_ERROR;
+    }
+
+    /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
+    for (uint32_t nof_layers = 1; nof_layers <= q->pdsch_cfg.nof_layers; nof_layers++ ) {
+      if (q->sinr[nof_layers][q->pmi[nof_layers]] > best_sinr) {
+        best_sinr = q->sinr[nof_layers][q->pmi[nof_layers]];
+        best_pmi = q->pmi[nof_layers];
+        best_ri = nof_layers;
+      }
+    }
+
+    /* Set RI */
+    if (ri != NULL) {
+      *ri = best_ri;
+    }
+
+    /* Set PMI */
+    if (pmi != NULL) {
+      *pmi = best_pmi;
+    }
+
+    /* Set current SINR */
+    if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+      if (q->pdsch_cfg.nof_layers == 1) {
+        *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
+      } else if (q->pdsch_cfg.nof_layers == 2) {
+        *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
+      } else {
+        ERROR("Not implemented number of layers");
+        return SRSLTE_ERROR;
+      }
+    }
+
+    /* Print Trace */
+    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
+      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
+           10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+    }
+  } else {
+    ERROR("Not implemented configuration");
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return SRSLTE_SUCCESS;
 }
 
 uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q) {

From 9c9c07a845c7d150a476f0fe5a58a46e385624dd Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 21 Aug 2017 12:01:34 +0200
Subject: [PATCH 075/269] Improvement in how PDSCH UE shows downlink
 statistics.

---
 lib/examples/pdsch_ue.c                    | 64 ++++++++++++----------
 lib/include/srslte/phy/common/phy_common.h |  2 +
 lib/src/phy/common/phy_common.c            | 15 +++++
 3 files changed, 53 insertions(+), 28 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 24bdba55b..931a983fa 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -512,9 +512,14 @@ int main(int argc, char **argv) {
   // Variables for measurements 
   uint32_t nframes=0;
   uint32_t ri = 0, pmi = 0;
-  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0, sinr = 0.0;
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
+      sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS];
   bool decode_pdsch = false; 
 
+  for (int i = 0; i < SRSLTE_MAX_LAYERS; i++) {
+    bzero(sinr[i], sizeof(float)*SRSLTE_MAX_CODEBOOKS);
+  }
+
 #ifndef DISABLE_RF
   if (prog_args.rf_gain < 0) {
     srslte_ue_sync_start_agc(&ue_sync, srslte_rf_set_rx_gain_th_wrapper_, cell_detect_config.init_agc);
@@ -618,15 +623,6 @@ int main(int argc, char **argv) {
             enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0, enodebrate, 0.05);
             uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0:0.0, uerate, 0.01);
 
-            if (ue_dl.cell.nof_ports == 2 && ue_dl.pdsch.nof_rx_antennas == 2) {
-              float _sinr;
-              srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, &_sinr);
-
-              if (!isinff(_sinr) && !isnanf(_sinr)) {
-                sinr = SRSLTE_VEC_EMA(_sinr, sinr, 0.05f);
-              }
-            }
-
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -658,23 +654,34 @@ int main(int argc, char **argv) {
                      100 * (1 - (float) ue_dl.nof_detected / nof_trials),
                      (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
             } else {
-              printf("CFO: %+5.2f kHz, "
-                         "SNR: %+5.1f dB | %+5.1f dB, "
-                         "Rb: %6.2f / %6.2f Mbps, "
-                         "PDCCH-Miss: %5.2f%%, "
-                         "PDSCH-BLER: %5.2f%%, "
-                         "SINR: %3.1f dB RI: %d PMI: %d    \r",
+              /* Compute Rank Indicator (RI) and Precoding Matrix Indicator (PMI) */
+              srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, NULL);
+              for (uint32_t nl = 0; nl < SRSLTE_MAX_LAYERS; nl++) {
+                for (uint32_t cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb ++) {
+                  sinr[nl][cb] = SRSLTE_VEC_EMA(ue_dl.sinr[nl][cb], sinr[nl][cb], 0.05f);
+                }
+              }
 
-                     srslte_ue_sync_get_cfo(&ue_sync) / 1000,
-                     10 * log10(rsrp0 / noise),
-                     10 * log10(rsrp1 / noise),
-                     uerate,
-                     enodebrate,
-                     100 * (1 - (float) ue_dl.nof_detected / nof_trials),
-                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total,
-                     10 * log10(sinr),
-                     ri,
-                     pmi);
+              /* Print Results */
+              printf("\033[K    Tx scheme: %-10s\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
+              printf("\033[K   nof layers: %d \n", ue_dl.pdsch_cfg.nof_layers);
+              printf("\033[Knof codewords: %d \n", ue_dl.pdsch_cfg.grant.nof_tb);
+              printf("\033[K          CFO: %+5.2f kHz\n", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
+              printf("\033[K          SNR: %+5.1f dB | %+5.1f dB\n", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
+              printf("\033[K           Rb: %6.2f / %6.2f Mbps (net/maximum)\n", uerate, enodebrate);
+              printf("\033[K   PDCCH-Miss: %5.2f%%\n", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
+              printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+              printf("\033[K   PDSCH-BLER: %5.2f%%\n\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+              printf("\033[K\n");
+              printf("\033[KSINR (dB) Vs RI and PMI:\n");
+              printf("\033[K   | RI |   1   |   2   |\n");
+              printf("\033[K -------+-------+-------+\n");
+              printf("\033[K P |  0 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][0]), (ri == 1 && pmi == 0)?'*':' ', 10 * log10(sinr[1][0]), (ri == 2 && pmi == 0)?'*':' ');
+              printf("\033[K M |  1 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][1]), (ri == 1 && pmi == 1)?'*':' ', 10 * log10(sinr[1][1]), (ri == 2 && pmi == 1)?'*':' ');
+              printf("\033[K I |  2 | %5.2f%c|-------+ \n", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2)?'*':' ');
+              printf("\033[K   |  3 | %5.2f%c|         \n", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3)?'*':' ');
+              printf("\033[K\n\n");
+              printf("\033[20A");
             }
           }
           break;
@@ -683,7 +690,7 @@ int main(int argc, char **argv) {
         sfn++; 
         if (sfn == 1024) {
           sfn = 0; 
-          printf("\n");
+          printf("\033[20B");
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
@@ -715,7 +722,8 @@ int main(int argc, char **argv) {
         
     sf_cnt++;                  
   } // Main loop
-  
+  printf("\033[20B");
+
 #ifndef DISABLE_GRAPHICS
   if (!prog_args.disable_plots) {
     if (!pthread_kill(plot_thread, 0)) {
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index 9859bdfb0..ad9be3e0f 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -282,6 +282,8 @@ SRSLTE_API int srslte_band_get_fd_region(enum band_geographical_area region,
 SRSLTE_API int srslte_str2mimotype(char *mimo_type_str, 
                                    srslte_mimo_type_t *type);
 
+SRSLTE_API char *srslte_mimotype2str(srslte_mimo_type_t mimo_type);
+
 SRSLTE_API uint32_t srslte_tti_interval(uint32_t tti1, 
                                         uint32_t tti2); 
 
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 7be57c63a..8a1d0cb4e 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -446,6 +446,21 @@ int srslte_str2mimotype(char *mimo_type_str, srslte_mimo_type_t *type) {
   return SRSLTE_SUCCESS;
 }
 
+char *srslte_mimotype2str(srslte_mimo_type_t mimo_type) {
+  switch (mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      return "Single";
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      return "Diversity";
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      return "Multiplex";
+    case SRSLTE_MIMO_TYPE_CDD:
+      return "CDD";
+    default:
+      return "N/A";
+  }
+}
+
 float get_fd(struct lte_band *band, uint32_t dl_earfcn) {
   if (dl_earfcn >= band->dl_earfcn_offset) {
     return band->fd_low_mhz + 0.1*(dl_earfcn - band->dl_earfcn_offset);    

From cb4b4f4d4c40c254fdf22ae6fe743f844610063c Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 21 Aug 2017 12:03:03 +0200
Subject: [PATCH 076/269] Reduce of CFI to 1 for supporting modulation index
 28.

---
 lib/examples/pdsch_enodeb.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 1275360a4..1ec9dc84b 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -65,7 +65,7 @@ srslte_cell_t cell = {
 
 int net_port = -1; // -1 generates random dataThat means there is some problem sending samples to the device
 
-uint32_t cfi=3;
+uint32_t cfi = 1;
 uint32_t mcs_idx = 1, last_mcs_idx = 1;
 int nof_frames = -1;
 char mimo_type_str[32] = "single";

From 2c07a1618946e4cb35cf8f0cb0222bfd05e53eb4 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 21 Aug 2017 16:53:12 +0200
Subject: [PATCH 077/269] DL grant parameter are now vectors of MAX_CODEWORDS

---
 lib/examples/pdsch_enodeb.c             |  32 ++---
 lib/examples/pdsch_ue.c                 |  20 +--
 lib/include/srslte/phy/phch/pdsch.h     |   7 +-
 lib/include/srslte/phy/phch/pdsch_cfg.h |  13 +-
 lib/include/srslte/phy/phch/ra.h        |  10 +-
 lib/include/srslte/phy/phch/sch.h       |   6 -
 lib/include/srslte/phy/ue/ue_dl.h       |   5 +-
 lib/include/srslte/phy/utils/debug.h    |   4 +-
 lib/src/phy/phch/pdsch.c                | 176 +++++++++++-------------
 lib/src/phy/phch/ra.c                   |  71 ++++------
 lib/src/phy/phch/sch.c                  |  80 ++---------
 lib/src/phy/phch/test/pdsch_test.c      | 130 ++++++++---------
 lib/src/phy/ue/ue_dl.c                  |  61 ++++----
 srsenb/src/phy/phch_worker.cc           |   6 +-
 srsue/hdr/phy/phch_worker.h             |  17 ++-
 srsue/src/phy/phch_worker.cc            |  82 ++++++++---
 16 files changed, 331 insertions(+), 389 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 1ec9dc84b..1bc2d5230 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -86,7 +86,8 @@ srslte_pdsch_t pdsch;
 srslte_pdsch_cfg_t pdsch_cfg; 
 srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS];
 srslte_regs_t regs;
-srslte_ra_dl_dci_t ra_dl;  
+srslte_ra_dl_dci_t ra_dl;
+int rvidx[SRSLTE_MAX_CODEWORDS] = {0, 0};
 
 cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL}, *output_buffer [SRSLTE_MAX_PORTS] = {NULL};
 int sf_n_re, sf_n_samples;
@@ -423,7 +424,7 @@ int update_radl() {
   ra_dl.harq_process = 0;
   ra_dl.mcs_idx = mcs_idx;
   ra_dl.ndi = 0;
-  ra_dl.rv_idx = 0;
+  ra_dl.rv_idx = rvidx[0];
   ra_dl.alloc_type = SRSLTE_RA_ALLOC_TYPE0;
   ra_dl.type0_alloc.rbg_bitmask = prbset_to_bitmask();
   ra_dl.tb_en[0] = 1;
@@ -431,7 +432,7 @@ int update_radl() {
   if (nof_tb > 1) {
     ra_dl.mcs_idx_1 = mcs_idx;
     ra_dl.ndi_1 = 0;
-    ra_dl.rv_idx_1 = 0;
+    ra_dl.rv_idx_1 = rvidx[1];
     ra_dl.tb_en[1] = 1;
   }
 
@@ -513,7 +514,7 @@ void *net_thread_fnc(void *arg) {
     n = srslte_netsource_read(&net_source, &data2[rpm], DATA_BUFF_SZ-rpm);
     if (n > 0) {
       // FIXME: I assume that both transport blocks have same size in case of 2 tb are active
-      int nbytes = 1 + (pdsch_cfg.grant.mcs.tbs + pdsch_cfg.grant.mcs2.tbs - 1) / 8;
+      int nbytes = 1 + (pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs - 1) / 8;
       rpm += n; 
       INFO("received %d bytes. rpm=%d/%d\n",n,rpm,nbytes);
       wpm = 0; 
@@ -696,12 +697,11 @@ int main(int argc, char **argv) {
           INFO("Transmitting packet\n",0);
         }
       } else {
-        INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs.tbs);
-        for (i = 0; i < pdsch_cfg.grant.mcs.tbs / 8; i++) {
-          data[0][i] = rand() % 256;
-        }
-        for (i = 0; i < pdsch_cfg.grant.mcs2.tbs / 8; i++) {
-          data[1][i] = rand() % 256;
+        INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs);
+        for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
+          for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
+            data[tb][i] = (uint8_t) rand();
+          }
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
         if (sf_idx != 0 && sf_idx != 5) {
@@ -744,7 +744,7 @@ int main(int argc, char **argv) {
         /* Configure pdsch_cfg parameters */
         srslte_ra_dl_grant_t grant; 
         srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0, 0, pdsch_cfg.mimo_type, multiplex_pmi)) {
+        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
           fprintf(stderr, "Error configuring PDSCH\n");
           exit(-1);
         }
@@ -757,13 +757,9 @@ int main(int argc, char **argv) {
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            srslte_bit_pack_vector(data[0], data_tmp, pdsch_cfg.grant.mcs.tbs);
-            if (srslte_netsink_write(&net_sink, data_tmp, 1+(pdsch_cfg.grant.mcs.tbs-1)/8) < 0) {
-              fprintf(stderr, "Error sending data through UDP socket\n");
-            }
-            if (nof_tb > 1) {
-              srslte_bit_pack_vector(data[1], data_tmp, pdsch_cfg.grant.mcs2.tbs);
-              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs2.tbs - 1) / 8) < 0) {
+            for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
+              srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
+              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
                 fprintf(stderr, "Error sending data through UDP socket\n");
               }
             }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 931a983fa..60cc0a4db 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -594,8 +594,10 @@ int main(int argc, char **argv) {
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
                 // FIXME: UDP Data transmission does not work
-                srslte_netsink_write(&net_sink, data[0], 1 + (ue_dl.pdsch_cfg.grant.mcs.tbs - 1) / 8);
-                srslte_netsink_write(&net_sink, data[1], 1 + (ue_dl.pdsch_cfg.grant.mcs2.tbs - 1) / 8);
+                for (uint32_t tb = 0; tb < ue_dl.pdsch_cfg.grant.nof_tb; tb++) {
+                  srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+
+                }
               }
               
               #ifdef PRINT_CHANGE_SCHEDULIGN
@@ -616,12 +618,12 @@ int main(int argc, char **argv) {
                                     
             nof_trials++; 
             
-            rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1);
-            rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05);
-            rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05);
-            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);
-            enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0, enodebrate, 0.05);
-            uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs.tbs + ue_dl.pdsch_cfg.grant.mcs2.tbs)/1000.0:0.0, uerate, 0.01);
+            rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1f);
+            rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05f);
+            rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05f);
+            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05f);
+            enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f, enodebrate, 0.05f);
+            uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f:0.0f, uerate, 0.01f);
 
             nframes++;
             if (isnan(rsrq)) {
@@ -812,7 +814,7 @@ void *plot_thread_run(void *arg) {
   while(1) {
     sem_wait(&plot_sem);
     
-    uint32_t nof_symbols = ue_dl.pdsch_cfg.nbits.nof_re;
+    uint32_t nof_symbols = ue_dl.pdsch_cfg.nbits[0].nof_re;
     if (!prog_args.disable_plots_except_constellation) {      
       for (i = 0; i < nof_re; i++) {
         tmp_plot[i] = 20 * log10f(cabsf(ue_dl.sf_symbols[i]));
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 9a9b16788..99710b965 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -104,15 +104,14 @@ SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg,
                                 srslte_ra_dl_grant_t *grant, 
                                 uint32_t cfi, 
                                 uint32_t sf_idx, 
-                                uint32_t rvidx); 
+                                int rvidx);
 
 SRSLTE_API int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg,
                                       srslte_cell_t cell,
                                       srslte_ra_dl_grant_t *grant,
                                       uint32_t cfi,
                                       uint32_t sf_idx,
-                                      uint32_t rvidx,
-                                      uint32_t rvidx2,
+                                      int rvidx[SRSLTE_MAX_CODEWORDS],
                                       srslte_mimo_type_t mimo_type,
                                       uint32_t pmi);
 
@@ -156,7 +155,7 @@ SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                        uint32_t pmi[SRSLTE_MAX_LAYERS],
                                        float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]);
 
-SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, int max_iter);
+SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter);
 
 SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q);
 
diff --git a/lib/include/srslte/phy/phch/pdsch_cfg.h b/lib/include/srslte/phy/phch/pdsch_cfg.h
index 9664d826d..eb4927fbb 100644
--- a/lib/include/srslte/phy/phch/pdsch_cfg.h
+++ b/lib/include/srslte/phy/phch/pdsch_cfg.h
@@ -40,14 +40,11 @@
 #include "srslte/phy/fec/cbsegm.h"
 
 typedef struct SRSLTE_API {
-  srslte_cbsegm_t cb_segm; 
-  srslte_cbsegm_t cb_segm2;
-  srslte_ra_dl_grant_t grant; 
-  srslte_ra_nbits_t nbits; 
-  srslte_ra_nbits_t nbits2;
-  uint32_t rv; 
-  uint32_t rv2;
-  uint32_t sf_idx;  
+  srslte_cbsegm_t cb_segm[SRSLTE_MAX_CODEWORDS];
+  srslte_ra_dl_grant_t grant;
+  srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS];
+  uint32_t rv[SRSLTE_MAX_CODEWORDS];
+  uint32_t sf_idx;
   uint32_t nof_layers;
   uint32_t codebook_idx;
   srslte_mimo_type_t mimo_type;
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 8a025ad97..c99dba1a1 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -102,10 +102,8 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   bool prb_idx[2][SRSLTE_MAX_PRB];
   uint32_t nof_prb;  
-  uint32_t Qm;
-  uint32_t Qm2; 
-  srslte_ra_mcs_t mcs;
-  srslte_ra_mcs_t mcs2;
+  uint32_t Qm[SRSLTE_MAX_CODEWORDS];
+  srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
   uint32_t nof_tb;
 } srslte_ra_dl_grant_t;
 
@@ -206,8 +204,8 @@ SRSLTE_API int srslte_ra_dl_dci_to_grant(srslte_ra_dl_dci_t *dci,
 SRSLTE_API void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, 
                                             uint32_t cfi, 
                                             srslte_cell_t cell, 
-                                            uint32_t sf_idx, 
-                                            srslte_ra_nbits_t *nbits); 
+                                            uint32_t sf_idx,
+                                            srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
                                                   uint32_t cfi,
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index 4fa1b3b0f..a03387448 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -103,12 +103,6 @@ SRSLTE_API int srslte_dlsch_encode2(srslte_sch_t *q,
                                     uint8_t *e_bits,
                                     int codeword_idx);
 
-SRSLTE_API int srslte_dlsch_encode_multi(srslte_sch_t *q,
-                                   srslte_pdsch_cfg_t *cfg,
-                                   srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                                   uint8_t *e_bits[SRSLTE_MAX_CODEWORDS]);
-
 SRSLTE_API int srslte_dlsch_decode(srslte_sch_t *q, 
                                    srslte_pdsch_cfg_t *cfg,
                                    srslte_softbuffer_rx_t *softbuffer,
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 0fc8f9422..268a8c4d9 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -145,14 +145,13 @@ SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q,
                                       srslte_ra_dl_grant_t *grant, 
                                       uint32_t cfi, 
                                       uint32_t sf_idx, 
-                                      uint32_t rvidx); 
+                                      int rvidx);
 
 SRSLTE_API int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q,
                                             srslte_ra_dl_grant_t *grant,
                                             uint32_t cfi,
                                             uint32_t sf_idx,
-                                            uint32_t rvidx,
-                                            uint32_t rvidx2,
+                                            int rvidx[SRSLTE_MAX_CODEWORDS],
                                             srslte_mimo_type_t mimo_type,
                                             uint32_t pinfo);
 
diff --git a/lib/include/srslte/phy/utils/debug.h b/lib/include/srslte/phy/utils/debug.h
index e4b4756f8..c88e1a3ce 100644
--- a/lib/include/srslte/phy/utils/debug.h
+++ b/lib/include/srslte/phy/utils/debug.h
@@ -58,10 +58,10 @@ SRSLTE_API extern int srslte_verbose;
 #define PRINT_NONE srslte_verbose=SRSLTE_VERBOSE_NONE
 
 #define DEBUG(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_DEBUG) \
-  fprintf(stdout, "[DEBUG]: " _fmt, __VA_ARGS__)
+  fprintf(stdout, "[DEBUG]: " _fmt, ##__VA_ARGS__)
 
 #define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO) \
-  fprintf(stdout, "[INFO]:  " _fmt, __VA_ARGS__)
+  fprintf(stdout, "[INFO]:  " _fmt, ##__VA_ARGS__)
 
 #if CMAKE_BUILD_TYPE==Debug
 /* In debug mode, it prints out the  */
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 31642751f..35e10fedb 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -358,50 +358,35 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
 /* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg. 
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
-int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
-{
-  if (cfg) {
-    if (grant) {
-      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
-    }
-    if (srslte_cbsegm(&cfg->cb_segm, cfg->grant.mcs.tbs)) {
-      fprintf(stderr, "Error computing Codeblock segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
-      return SRSLTE_ERROR; 
-    }
-    srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, &cfg->nbits);
-    cfg->sf_idx = sf_idx; 
-    cfg->rv = rvidx;  
+int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
+                     uint32_t sf_idx, int rvidx) {
+  int _rvids[SRSLTE_MAX_CODEWORDS] = {1};
+  _rvids[0] = rvidx;
 
-    return SRSLTE_SUCCESS;   
-  } else {
-    return SRSLTE_ERROR_INVALID_INPUTS;
-  }
+  return srslte_pdsch_cfg_multi(cfg, cell, grant, cfi, sf_idx, _rvids, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
 }
 
 /* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg.
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
 int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
-                           uint32_t sf_idx, uint32_t rvidx, uint32_t rvidx2, srslte_mimo_type_t mimo_type, uint32_t pmi)
-{
+                           uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
+                           uint32_t pmi) {
   if (cfg) {
     if (grant) {
       memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
     }
-    if (srslte_cbsegm(&cfg->cb_segm, (uint32_t) cfg->grant.mcs.tbs)) {
-      fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
-      return SRSLTE_ERROR;
-    }
 
-    if (srslte_cbsegm(&cfg->cb_segm2, (uint32_t) cfg->grant.mcs2.tbs)) {
-      fprintf(stderr, "Error computing Codeblock (2) segmentation for TBS=%d\n", cfg->grant.mcs2.tbs);
-      return SRSLTE_ERROR;
+    for (int i = 0; i < grant->nof_tb; i++) {
+      if (srslte_cbsegm(&cfg->cb_segm[i], (uint32_t) cfg->grant.mcs[i].tbs)) {
+        fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[i].tbs);
+        return SRSLTE_ERROR;
+      }
     }
+    srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
 
-    srslte_ra_dl_grant_to_nbits_multi(&cfg->grant, cfi, cell, sf_idx, &cfg->nbits, &cfg->nbits2);
     cfg->sf_idx = sf_idx;
-    cfg->rv = rvidx;
-    cfg->rv2 = rvidx2;
+    memcpy(cfg->rv, rvidx, sizeof(uint32_t) * SRSLTE_MAX_CODEWORDS);
     cfg->mimo_type = mimo_type;
 
     /* Check and configure PDSCH transmission modes */
@@ -428,11 +413,12 @@ int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_r
           cfg->codebook_idx = pmi + 1;
           cfg->nof_layers = 2;
         }
-        INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n", grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+        INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n",
+             grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
         break;
       case SRSLTE_MIMO_TYPE_CDD:
         if (grant->nof_tb != 2) {
-          ERROR("Number of transport blocks is not supported for CDD transmission mode.");
+          ERROR("Number of transport blocks (%d) is not supported for CDD transmission mode.", grant->nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
@@ -472,13 +458,14 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
                                                srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
   srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
-  srslte_ra_nbits_t *nbits = (codeword_idx == 0) ? &cfg->nbits : &cfg->nbits2;
-  srslte_ra_mcs_t *mcs = (codeword_idx == 0) ? &cfg->grant.mcs : &cfg->grant.mcs2;
+  srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
+  srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
+  uint32_t rv = cfg->rv[codeword_idx];
 
   if (nbits->nof_bits) {
     INFO("Encoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
-         nbits->nof_re, nbits->nof_bits, (codeword_idx == 0) ? cfg->rv : cfg->rv2);
+         nbits->nof_re, nbits->nof_bits, rv);
 
     /* Channel coding */
     if (srslte_dlsch_encode2(dl_sch, cfg, softbuffer, data, pdsch->e[codeword_idx], codeword_idx)) {
@@ -516,19 +503,20 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
   srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
-  srslte_ra_nbits_t *nbits = (codeword_idx == 0) ? &cfg->nbits : &cfg->nbits2;
-  srslte_ra_mcs_t *mcs = (codeword_idx == 0) ? &cfg->grant.mcs : &cfg->grant.mcs2;
+  srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
+  srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
+  uint32_t rv = cfg->rv[codeword_idx];
 
   if (nbits->nof_bits) {
     INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
-         nbits->nof_re, nbits->nof_bits, (codeword_idx == 0) ? cfg->rv : cfg->rv2);
+         nbits->nof_re, nbits->nof_bits, rv);
 
     /* 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
      */
-    srslte_demod_soft_demodulate_s(mcs->mod, pdsch->d[codeword_idx], pdsch->e[codeword_idx], cfg->nbits.nof_re);
+    srslte_demod_soft_demodulate_s(mcs->mod, pdsch->d[codeword_idx], pdsch->e[codeword_idx], nbits->nof_re);
 
     if (pdsch->users[rnti] && pdsch->users[rnti]->sequence_generated) {
       srslte_scrambling_s_offset(&pdsch->users[rnti]->seq[codeword_idx][cfg->sf_idx], pdsch->e[codeword_idx],
@@ -596,23 +584,22 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       cfg          != NULL)
   {
 
-    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: [%d %d], C_prb=%d\n",
-        cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re,
-         cfg->nbits.nof_bits, cfg->rv, cfg->rv2, cfg->grant.nof_prb);
+    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d\n",
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb);
 
     for (int j=0;j<q->nof_rx_antennas;j++) {
       /* extract symbols */
-      int n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
-      if (n != cfg->nbits.nof_re) {
-        fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
+      int n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
+      if (n != cfg->nbits[0].nof_re) {
+        fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
         return SRSLTE_ERROR;
       }
 
       /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
-        n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
-        if (n != cfg->nbits.nof_re) {
-          fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
+        n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
+        if (n != cfg->nbits[0].nof_re) {
+          fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
           return SRSLTE_ERROR;
         }
       }
@@ -620,35 +607,31 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
 
     INFO("PDSCH Layer demapper and predecode: mimo_type=%d, nof_layers=%d, nof_tb=%d\n", cfg->mimo_type,
          cfg->nof_layers, cfg->grant.nof_tb);
-    if (q->cell.nof_ports == 1) {
-      /* no need for layer demapping */
-      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d[0], q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
-    } else {
-      int nof_symbols [SRSLTE_MAX_CODEWORDS];
-      nof_symbols[0] = cfg->nbits.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
-      nof_symbols[1] = cfg->nbits2.nof_re * cfg->grant.nof_tb / cfg->nof_layers;
-
-      if (cfg->nof_layers == cfg->grant.nof_tb) {
-        for (i = 0; i < cfg->nof_layers; i++) {
-          x[i] = q->d[i];
-        }
+    int nof_symbols [SRSLTE_MAX_CODEWORDS];
+    nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+    nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+
+    if (cfg->nof_layers == cfg->grant.nof_tb) {
+      /* Skip layer demap */
+      for (i = 0; i < cfg->nof_layers; i++) {
+        x[i] = q->d[i];
+      }
 
-        srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
-                                      cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, noise_estimate);
+      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                    cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
-      } else {
-        /* number of layers equals number of ports */
-        for (i = 0; i < cfg->nof_layers; i++) {
-          x[i] = q->x[i];
-        }
-        memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
+    } else {
+      /* number of layers equals number of ports */
+      for (i = 0; i < cfg->nof_layers; i++) {
+        x[i] = q->x[i];
+      }
+      memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-        srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
-                                      cfg->codebook_idx, cfg->nbits.nof_re, cfg->mimo_type, noise_estimate);
+      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                    cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
-        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
-                               nof_symbols[0], nof_symbols, cfg->mimo_type);
-      }
+      srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+                             nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
@@ -671,7 +654,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
         }
       }
       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));
+      srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits[0].nof_re*sizeof(cf_t));
     }
 
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
@@ -680,7 +663,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
 
     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));
+      srslte_vec_save_file("llr.dat", q->e, cfg->nbits[0].nof_bits*sizeof(int16_t));
     }
 
     return ret;
@@ -733,20 +716,20 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
       }
     }
 
-    if (cfg->grant.mcs.tbs == 0) {
+    if (cfg->grant.mcs[0].tbs == 0) {
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    if (cfg->nbits.nof_re > q->max_re) {
+    if (cfg->nbits[0].nof_re > q->max_re) {
       fprintf(stderr,
           "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
-          cfg->nbits.nof_re, q->max_re, q->cell.nof_prb);
+          cfg->nbits[0].nof_re, q->max_re, q->cell.nof_prb);
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
     INFO("Encoding PDSCH SF: %d, Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs,
-         cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv);
+        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs[0].mod), cfg->grant.mcs[0].tbs,
+         cfg->nbits[0].nof_re, cfg->nbits[0].nof_bits, cfg->rv[0]);
 
     /* number of layers equals number of ports */
     for (i = 0; i < q->cell.nof_ports; i++) {
@@ -761,30 +744,30 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
 
     /* scramble */
     if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[0][cfg->sf_idx], (uint8_t*) q->e[0], cfg->nbits.nof_bits);
+      srslte_scrambling_bytes(&q->users[rnti]->seq[0][cfg->sf_idx], (uint8_t*) q->e[0], cfg->nbits[0].nof_bits);
     } else {
       srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
+      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits[0].nof_bits)) {
         return SRSLTE_ERROR;
       }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->e[0], cfg->nbits.nof_bits);
+      srslte_scrambling_bytes(&seq, (uint8_t*) q->e[0], cfg->nbits[0].nof_bits);
       srslte_sequence_free(&seq);
     }
 
-    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e[0], q->d[0], cfg->nbits.nof_bits);
+    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs[0].mod], (uint8_t*) q->e[0], q->d[0], cfg->nbits[0].nof_bits);
 
     /* TODO: only diversity supported */
     if (q->cell.nof_ports > 1) {
-      srslte_layermap_diversity(q->d[0], x, q->cell.nof_ports, cfg->nbits.nof_re);
+      srslte_layermap_diversity(q->d[0], x, q->cell.nof_ports, cfg->nbits[0].nof_re);
       srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
-          cfg->nbits.nof_re / q->cell.nof_ports);
+          cfg->nbits[0].nof_re / q->cell.nof_ports);
     } else {
-      memcpy(q->symbols[0], q->d, cfg->nbits.nof_re * sizeof(cf_t));
+      memcpy(q->symbols[0], q->d, cfg->nbits[0].nof_re * sizeof(cf_t));
     }
 
     /* mapping to resource elements */
     for (i = 0; i < q->cell.nof_ports; i++) {
-      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
+      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
     }
 
     ret = SRSLTE_SUCCESS;
@@ -810,15 +793,15 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
       }
     }
 
-    /* If both transport block sizes are zero return error */
-    if (cfg->grant.mcs.tbs == 0 && cfg->grant.mcs2.tbs == 0) {
+    /* If both transport block size is zero return error */
+    if (cfg->grant.mcs[0].tbs == 0) {
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    if (cfg->nbits.nof_re > q->max_re) {
+    if (cfg->nbits[0].nof_re > q->max_re) {
       fprintf(stderr,
               "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
-              cfg->nbits.nof_re, q->max_re, q->cell.nof_prb);
+              cfg->nbits[0].nof_re, q->max_re, q->cell.nof_prb);
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
@@ -833,7 +816,7 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
         for (i = 0; i < cfg->nof_layers; i++) {
           x[i] = q->d[i];
         }
-        nof_symbols = cfg->nbits.nof_re;
+        nof_symbols = cfg->nbits[0].nof_re;
       } else {
         /* Initialise layer map pointers */
         for (i = 0; i < cfg->nof_layers; i++) {
@@ -842,20 +825,21 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
         memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
         nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
-                                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits.nof_re, cfg->nbits2.nof_re}, cfg->mimo_type);
+                                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits[0].nof_re, cfg->nbits[1].nof_re},
+                                           cfg->mimo_type);
       }
 
       /* Precode */
       srslte_precoding_type(x, q->symbols, cfg->nof_layers, q->cell.nof_ports, cfg->codebook_idx,
                             nof_symbols, cfg->mimo_type);
     } else {
-      memcpy(q->symbols[0], q->d[0], cfg->nbits.nof_re * sizeof(cf_t));
+      memcpy(q->symbols[0], q->d[0], cfg->nbits[0].nof_re * sizeof(cf_t));
     }
 
     /* mapping to resource elements */
 
     for (i = 0; i < q->cell.nof_ports; i++) {
-      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
+      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
     }
 
     ret = SRSLTE_SUCCESS;
@@ -863,7 +847,7 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
   return ret;
 }
 
-void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, int max_iter) {
+void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter) {
   for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
     srslte_sch_set_max_noi(&q->dl_sch[cw], max_iter);
   }
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 635172762..a7fef1e42 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -492,44 +492,43 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       fprintf(stderr, "Error decoding DCI: P/SI/RA-RNTI supports Format1A/1C only\n");
       return SRSLTE_ERROR; 
     }
-    grant->mcs.mod = SRSLTE_MOD_QPSK;      
-    grant->mcs.tbs = (uint32_t) tbs; 
+    grant->mcs[0].mod = SRSLTE_MOD_QPSK;
+    grant->mcs[0].tbs = (uint32_t) tbs;
   } else {
     n_prb = grant->nof_prb;
     if (dci->tb_en[0]) {
-      grant->mcs.idx = dci->mcs_idx;
-      tbs   = dl_fill_ra_mcs(&grant->mcs, n_prb);
+      grant->mcs[0].idx = dci->mcs_idx;
+      tbs   = dl_fill_ra_mcs(&grant->mcs[0], n_prb);
       if (tbs) {
         last_dl_tbs[dci->harq_process%8] = tbs;
       } else {
         // For mcs>=29, set last TBS received for this PID
-        grant->mcs.tbs = last_dl_tbs[dci->harq_process%8]; 
+        grant->mcs[0].tbs = last_dl_tbs[dci->harq_process%8];
       }
     } else {
-      grant->mcs.tbs = 0; 
+      grant->mcs[0].tbs = 0;
     }
     if (dci->tb_en[1]) {
-      grant->mcs2.idx = dci->mcs_idx_1;
-      tbs = dl_fill_ra_mcs(&grant->mcs2, n_prb);
+      grant->mcs[1].idx = dci->mcs_idx_1;
+      tbs = dl_fill_ra_mcs(&grant->mcs[1], n_prb);
       if (tbs) {
         last_dl_tbs2[dci->harq_process%8] = tbs;
       } else {
         // For mcs>=29, set last TBS received for this PID
-        grant->mcs2.tbs = last_dl_tbs2[dci->harq_process%8]; 
+        grant->mcs[1].tbs = last_dl_tbs2[dci->harq_process%8];
       }
     } else {
-      grant->mcs2.tbs = 0; 
+      grant->mcs[1].tbs = 0;
     }
   }
   grant->nof_tb = 0;
-  if (dci->tb_en[0]) {
-    grant->Qm = srslte_mod_bits_x_symbol(grant->mcs.mod);
-    grant->nof_tb++;
-  }
-  if (dci->tb_en[1]) {
-    grant->Qm2 = srslte_mod_bits_x_symbol(grant->mcs2.mod);
-    grant->nof_tb++;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (dci->tb_en[tb]) {
+      grant->Qm[tb] = srslte_mod_bits_x_symbol(grant->mcs[tb].mod);
+      grant->nof_tb++;
+    }
   }
+
   if (tbs < 0) {
     return SRSLTE_ERROR; 
   } else {    
@@ -537,29 +536,16 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
   }
 }
 
-void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx, srslte_ra_nbits_t *nbits) 
+void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx,
+                                 srslte_ra_nbits_t nbits [SRSLTE_MAX_CODEWORDS])
 {
   // Compute number of RE 
-  nbits->nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb<10?(cfi+1):cfi);
-  nbits->lstart = cell.nof_prb<10?(cfi+1):cfi;
-  nbits->nof_symb = 2*SRSLTE_CP_NSYMB(cell.cp)-nbits->lstart;
-  nbits->nof_bits = nbits->nof_re * grant->Qm;      
-}
-
-void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx,
-                                       srslte_ra_nbits_t *nbits, srslte_ra_nbits_t *nbits2) {
-  /* Compute number of RE for first transport block */
-  nbits->nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb<10?(cfi+1):cfi);
-  nbits->lstart = cell.nof_prb<10?(cfi+1):cfi;
-  nbits->nof_symb = 2*SRSLTE_CP_NSYMB(cell.cp)-nbits->lstart;
-  nbits->nof_bits = nbits->nof_re * grant->Qm;
-
-  /*/ Compute number of RE for second transport block */
-  if (grant->nof_tb > 1) {
-    nbits2->nof_re = nbits->nof_re;
-    nbits2->lstart = nbits->lstart;
-    nbits2->nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits2->lstart;
-    nbits2->nof_bits = nbits2->nof_re * grant->Qm2;
+  for (int i = 0; i < grant->nof_tb; i++) {
+    /* Compute number of RE for first transport block */
+    nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
+    nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
+    nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+    nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
   }
 }
 
@@ -834,10 +820,11 @@ void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
   fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
-  fprintf(f, " - Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs.mod));
-  fprintf(f, " - Transport block size:\t\t%d\n", grant->mcs.tbs);
-  fprintf(f, " - Modulation type (TB2):\t\t%s\n", srslte_mod_string(grant->mcs2.mod));
-  fprintf(f, " - Transport block size (TB2):\t\t%d\n", grant->mcs2.tbs);
+  for (int i = 0; i < grant->nof_tb; i++) {
+    fprintf(f, "  - Transport block:\t\t\t%d\n", i);
+    fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
+    fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+  }
 }
 
 void srslte_ra_prb_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 4b78f6228..e0002429e 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -514,39 +514,23 @@ static int decode_tb(srslte_sch_t *q,
   }
 }
 
-int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer, 
-                        int16_t *e_bits, uint8_t *data) 
-{
-  return decode_tb(q,                    
-                   softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits, 
-                   e_bits, data);
+int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                        int16_t *e_bits, uint8_t *data) {
+  return srslte_dlsch_decode2(q, cfg, softbuffer, e_bits, data, 0);
 }
 
 
 int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
-                        int16_t *e_bits, uint8_t *data, int codeword_idx)
-{
+                         int16_t *e_bits, uint8_t *data, int codeword_idx) {
   uint32_t Nl = 1;
-  int ret = SRSLTE_ERROR;
 
   if (cfg->nof_layers != cfg->grant.nof_tb) {
     Nl = 2;
   }
 
-  if (codeword_idx == 0) {
-    ret = decode_tb(q, softbuffer, &cfg->cb_segm,
-                     cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
-                     e_bits, data);
-  } else if (codeword_idx == 1) {
-    ret = decode_tb(q, softbuffer, &cfg->cb_segm2,
-                     cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
-                     e_bits, data);
-  } else {
-    ERROR("Not implemented");
-  }
-
-  return ret;
+  return decode_tb(q, softbuffer, &cfg->cb_segm[codeword_idx],
+                   cfg->grant.Qm[codeword_idx] * Nl, cfg->rv[codeword_idx], cfg->nbits[codeword_idx].nof_bits,
+                   e_bits, data);
 }
 
 /**
@@ -562,64 +546,22 @@ int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
 int srslte_dlsch_encode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
                         uint8_t *data, uint8_t *e_bits) 
 {
-  return encode_tb(q, 
-                   softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits, 
-                   data, e_bits);
+  return srslte_dlsch_encode2(q, cfg, softbuffer, data, e_bits, 0);
 }
 
 int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
                               uint8_t *data, uint8_t *e_bits, int codeword_idx) {
-  int ret = SRSLTE_ERROR;
   uint32_t Nl = 1;
 
   if (cfg->nof_layers != cfg->grant.nof_tb) {
     Nl = 2;
   }
 
-  if(codeword_idx == 0) {
-    /* Codeword 1 shall be encoded */
-    ret = encode_tb(q, softbuffer, &cfg->cb_segm, cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits, data, e_bits);
-  } else if(codeword_idx == 1) {
-    /* Codeword 2 shall be encoded */
-    ret = encode_tb(q, softbuffer, &cfg->cb_segm2, cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits, data, e_bits);
-  } else {
-    ERROR("Not implemented");
-  }
-
-  return ret;
-}
-
-int srslte_dlsch_encode_multi(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                        uint8_t *data[SRSLTE_MAX_CODEWORDS], uint8_t *e_bits[SRSLTE_MAX_CODEWORDS])
-{
-  int ret = 0;
-  uint32_t Nl = 1;
-
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
-    Nl = 2;
-  }
-
-  /* Check if codeword 1 shall be encoded */
-  if(cfg->nbits.nof_bits) {
-    ret |= encode_tb(q,
-                     &softbuffers[0], &cfg->cb_segm,
-                     cfg->grant.Qm*Nl, cfg->rv, cfg->nbits.nof_bits,
-                     data[0], e_bits[0]);
-  }
-
-  /* Check if codeword 2 shall be encoded */
-  if(cfg->nbits2.nof_bits) {
-    ret |= encode_tb(q,
-                     &softbuffers[1], &cfg->cb_segm2,
-                     cfg->grant.Qm2*Nl, cfg->rv2, cfg->nbits2.nof_bits,
-                     data[1], e_bits[1]);
-  }
-
-  return ret;
+  return encode_tb(q, softbuffer, &cfg->cb_segm[codeword_idx], cfg->grant.Qm[codeword_idx]*Nl, cfg->rv[codeword_idx],
+                   cfg->nbits[codeword_idx].nof_bits, data, e_bits);
 }
 
-/* Compute the interleaving function on-the-fly, because it depends on number of RI bits 
+/* Compute the interleaving function on-the-fly, because it depends on number of RI bits
  * Profiling show that the computation of this matrix is neglegible. 
  */
 static void ulsch_interleave_gen(uint32_t H_prime_total, uint32_t N_pusch_symbs, uint32_t Qm,
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 180e689d6..777481d6a 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -27,7 +27,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
 #include <sys/time.h>
 #include <srslte/phy/phch/ra.h>
@@ -55,11 +54,9 @@ srslte_cell_t cell = {
 char mimo_type_str [32] = "single";
 srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
 uint32_t cfi = 2;
-uint32_t mcs = 0;
-uint32_t mcs2 = 0;
+uint32_t mcs[SRSLTE_MAX_CODEWORDS] = {0, 0};
 uint32_t subframe = 1;
-uint32_t rv_idx = 0;
-uint32_t rv_idx2 = 1;
+int rv_idx[SRSLTE_MAX_CODEWORDS] = {0, 1};
 uint16_t rnti = 1234;
 uint32_t nof_rx_antennas = 1;
 uint32_t pmi = 0;
@@ -68,12 +65,12 @@ char *input_file = NULL;
 void usage(char *prog) {
   printf("Usage: %s [fmMcsrtRFpnwav] \n", prog);
   printf("\t-f read signal from file [Default generate it with pdsch_encode()]\n");
-  printf("\t-m MCS [Default %d]\n", mcs);
-  printf("\t-M MCS2 [Default %d]\n", mcs2);
+  printf("\t-m MCS [Default %d]\n", mcs[0]);
+  printf("\t-M MCS2 [Default %d]\n", mcs[1]);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-s subframe [Default %d]\n", subframe);
-  printf("\t-r rv_idx [Default %d]\n", rv_idx);
-  printf("\t-t rv_idx2 [Default %d]\n", rv_idx2);
+  printf("\t-r rv_idx [Default %d]\n", rv_idx[0]);
+  printf("\t-t rv_idx2 [Default %d]\n", rv_idx[1]);
   printf("\t-R rnti [Default %d]\n", rnti);
   printf("\t-F cfi [Default %d]\n", cfi);
   printf("\t-x Transmission mode [single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
@@ -91,19 +88,19 @@ void parse_args(int argc, char **argv) {
       input_file = argv[optind];
       break;
     case 'm':
-      mcs = atoi(argv[optind]);
+      mcs[0] = (uint32_t) atoi(argv[optind]);
       break;
     case 'M':
-      mcs2 = (uint32_t) atoi(argv[optind]);
+      mcs[1] = (uint32_t) atoi(argv[optind]);
       break;
     case 's':
       subframe = atoi(argv[optind]);
       break;
     case 'r':
-      rv_idx = atoi(argv[optind]);
+      rv_idx[0] = (uint32_t) atoi(argv[optind]);
       break;
     case 't':
-      rv_idx2 = (uint32_t) atoi(argv[optind]);
+      rv_idx[1] = (uint32_t) atoi(argv[optind]);
       break;
     case 'R':
       rnti = atoi(argv[optind]);
@@ -197,16 +194,16 @@ int main(int argc, char **argv) {
   dci.type0_alloc.rbg_bitmask = 0xffffffff;
 
   /* If transport block 0 is enabled */
-  if (mcs != 0 || rv_idx != 1) {
-    dci.mcs_idx = mcs;
-    dci.rv_idx = rv_idx;
+  if (mcs[0] != 0 || rv_idx[0] != 1) {
+    dci.mcs_idx = mcs[0];
+    dci.rv_idx = rv_idx[0];
     dci.tb_en[0] = true;
   }
 
   /* If transport block 0 is disabled */
-  if (mcs2 != 0 || rv_idx2 != 1) {
-    dci.mcs_idx_1 = mcs2;
-    dci.rv_idx_1 = rv_idx2;
+  if (mcs[1] != 0 || rv_idx[1] != 1) {
+    dci.mcs_idx_1 = mcs[1];
+    dci.rv_idx_1 = rv_idx[1];
     dci.tb_en[1] = true;
   }
 
@@ -235,7 +232,7 @@ int main(int argc, char **argv) {
 #endif /* DO_OFDM */
 
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, rv_idx2, mimo_type, pmi)) {
+  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     goto quit;
   }
@@ -260,22 +257,15 @@ int main(int argc, char **argv) {
   }
 
 
-  if (grant.mcs.tbs) {
-    data[0] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs);
-    if (!data[0]) {
-      perror("srslte_vec_malloc");
-      goto quit;
-    }
-    bzero(data[0], sizeof(uint8_t) * grant.mcs.tbs);
-  }
-
-  if (grant.mcs2.tbs) {
-    data[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs2.tbs);
-    if (!data[1]) {
-      perror("srslte_vec_malloc");
-      goto quit;
+  for (int i = 0; i < grant.nof_tb; i++) {
+    if (grant.mcs[i].tbs) {
+      data[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data[i], sizeof(uint8_t) * grant.mcs[i].tbs);
     }
-    bzero(data[1], sizeof(uint8_t) * grant.mcs2.tbs);
   }
 
   if (srslte_pdsch_init_rx_multi(&pdsch_rx, cell, nof_rx_antennas)) {
@@ -292,6 +282,7 @@ int main(int argc, char **argv) {
     }
   }
 
+  INFO(" Global:\n");
   INFO("         nof_prb=%d\n", cell.nof_prb);
   INFO("       nof_ports=%d\n", cell.nof_ports);
   INFO("              id=%d\n", cell.id);
@@ -300,28 +291,22 @@ int main(int argc, char **argv) {
   INFO(" phich_resources=%d\n", (int) cell.phich_resources);
   INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
   INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
-  INFO("              Qm=%d\n", pdsch_cfg.grant.Qm);
-  INFO("             Qm2=%d\n", pdsch_cfg.grant.Qm2);
-  INFO("         mcs.idx=0x%X\n", pdsch_cfg.grant.mcs.idx);
-  INFO("         mcs.tbs=%d\n", pdsch_cfg.grant.mcs.tbs);
-  INFO("         mcs.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs.mod));
-  INFO("        mcs2.idx=0x%X\n", pdsch_cfg.grant.mcs2.idx);
-  INFO("        mcs2.tbs=%d\n", pdsch_cfg.grant.mcs2.tbs);
-  INFO("        mcs2.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs2.mod));
-  INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
-  INFO("              rv=%d\n", pdsch_cfg.rv);
-  INFO("             rv2=%d\n", pdsch_cfg.rv2);
   INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
-  INFO("       mimo_type=%d\n", (int) pdsch_cfg.mimo_type);
+  INFO("       mimo_type=%s\n", srslte_mimotype2str(pdsch_cfg.mimo_type));
+  INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
   INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
-  INFO("          lstart=%d\n", pdsch_cfg.nbits.lstart);
-  INFO("        nof_bits=%d\n", pdsch_cfg.nbits.nof_bits);
-  INFO("          nof_re=%d\n", pdsch_cfg.nbits.nof_re);
-  INFO("        nof_symb=%d\n", pdsch_cfg.nbits.nof_symb);
-  INFO("          lstart=%d\n", pdsch_cfg.nbits2.lstart);
-  INFO("        nof_bits=%d\n", pdsch_cfg.nbits2.nof_bits);
-  INFO("          nof_re=%d\n", pdsch_cfg.nbits2.nof_re);
-  INFO("        nof_symb=%d\n", pdsch_cfg.nbits2.nof_symb);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    INFO(" Tranport block index %d:\n", i);
+    INFO("              Qm=%d\n", pdsch_cfg.grant.Qm[i]);
+    INFO("         mcs.idx=0x%X\n", pdsch_cfg.grant.mcs[i].idx);
+    INFO("         mcs.tbs=%d\n", pdsch_cfg.grant.mcs[i].tbs);
+    INFO("         mcs.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs[i].mod));
+    INFO("              rv=%d\n", pdsch_cfg.rv[i]);
+    INFO("          lstart=%d\n", pdsch_cfg.nbits[i].lstart);
+    INFO("        nof_bits=%d\n", pdsch_cfg.nbits[i].nof_bits);
+    INFO("          nof_re=%d\n", pdsch_cfg.nbits[i].nof_re);
+    INFO("        nof_symb=%d\n", pdsch_cfg.nbits[i].nof_symb);
+  }
 
   if (input_file) {
     srslte_filesource_t fsrc;
@@ -368,29 +353,25 @@ int main(int argc, char **argv) {
       }
     }
 
-    for (i = 0; i < grant.mcs.tbs / 8; i++) {
-      data[0][i] = (uint8_t) (rand() % 256);
-    }
-
-    for (i = 0; i < grant.mcs2.tbs / 8; i++) {
-      data[1][i] = (uint8_t) (rand() % 256);
+    for (i = 0; i< grant.nof_tb; i++) {
+      for (i = 0; i < grant.mcs[i].tbs / 8; i++) {
+        data[i][i] = (uint8_t) (rand() % 256);
+      }
     }
 
     /*uint8_t databit[100000];
     srslte_bit_unpack_vector(data, databit, grant.mcs.tbs);
     srslte_vec_save_file("data_in", databit, grant.mcs.tbs);*/
     
-    if (rv_idx) {
+    if (rv_idx[0] != 0 || rv_idx[1] != 0) {
       /* Do 1st transmission for rv_idx!=0 */
-      pdsch_cfg.rv = 0;
-      pdsch_cfg.rv2 = 0;
+      bzero(pdsch_cfg.rv, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
       if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
         fprintf(stderr, "Error encoding PDSCH\n");
         goto quit;
       }
     }
-    pdsch_cfg.rv = rv_idx;
-    pdsch_cfg.rv2 = rv_idx2;
+    memcpy(pdsch_cfg.rv, rv_idx, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
     gettimeofday(&t[1], NULL);
     for (k = 0; k < M; k++) {
       if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
@@ -401,8 +382,8 @@ int main(int argc, char **argv) {
     gettimeofday(&t[2], NULL);
     get_time_interval(t);
     printf("ENCODED in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n",
-           (float) t[0].tv_usec/M, (float) (grant.mcs.tbs + grant.mcs2.tbs)/1000.0f,
-           (float) (grant.mcs.tbs + grant.mcs2.tbs)*M/t[0].tv_usec);
+           (float) t[0].tv_usec/M, (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)/1000.0f,
+           (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
 
   #ifdef DO_OFDM
     for (i = 0; i < cell.nof_ports; i++) {
@@ -444,19 +425,18 @@ int main(int argc, char **argv) {
       srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
     }
 #endif
-    if (grant.mcs.tbs) {
-      srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[0], (uint32_t) grant.mcs.tbs);
-    }
-    if (grant.mcs2.tbs) {
-      srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[1], (uint32_t) grant.mcs2.tbs);
+    for (i = 0; i < grant.nof_tb; i++) {
+      if (grant.mcs[i].tbs) {
+        srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
+      }
     }
     r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data);
   }
   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 + grant.mcs2.tbs)/1000.0f,
-         (float) (grant.mcs.tbs + grant.mcs2.tbs)*M/t[0].tv_usec);
+         (float) t[0].tv_usec/M, (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)/1000.0f,
+         (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
   if (r) {
     ret = -1;
     goto quit;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 3f51a3036..47337a109 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -282,14 +282,16 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 }
 
 
-int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
-{
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, 0, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
+int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
+                           int rvidx) {
+  int _rvidx [SRSLTE_MAX_CODEWORDS] = {1};
+  _rvidx[0] = rvidx;
+
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, _rvidx, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
 }
 
 int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
-                                 uint32_t rvidx, uint32_t rvidx2, srslte_mimo_type_t mimo_type, uint32_t pinfo)
-{
+                                 int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type, uint32_t pinfo) {
   uint32_t pmi = 0;
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
@@ -310,7 +312,7 @@ int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant,
       }
     }
   }
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, rvidx2, mimo_type, pmi);
+  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
 }
 
 int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti)
@@ -357,23 +359,28 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     /* ===== These lines of code are supposed to be MAC functionality === */
 
     
-    uint32_t rvidx = 0; 
-    uint32_t rvidx2 = 0;
+    int rvidx[SRSLTE_MAX_CODEWORDS] = {1};
     if (dci_unpacked.rv_idx < 0) {
       uint32_t sfn = tti/10; 
-      uint32_t k   = (sfn/2)%4; 
-      rvidx        = ((uint32_t) ceilf((float)1.5*k))%4;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffers[0], (uint32_t) grant.mcs.tbs);
-      if (grant.nof_tb > 1) {
-        rvidx2     = ((uint32_t) ceilf((float)1.5*k))%4;
-        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[1], (uint32_t) grant.mcs2.tbs);
+      uint32_t k   = (sfn/2)%4;
+      for (int i = 0; i < grant.nof_tb; i++) {
+        rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     } else {
-      rvidx = (uint32_t) dci_unpacked.rv_idx;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffers[0], (uint32_t) grant.mcs.tbs);
-      if (grant.nof_tb > 1) {
-        rvidx2 = (uint32_t) dci_unpacked.rv_idx_1;
-        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[1], (uint32_t) grant.mcs2.tbs);
+      for (int i = 0; i < grant.nof_tb; i++) {
+        switch(i) {
+          case 0:
+            rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
+            break;
+          case 1:
+            rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
+            break;
+          default:
+            ERROR("Wrong number of transport blocks");
+            return SRSLTE_ERROR;
+        }
+        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
 
@@ -408,7 +415,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
         return SRSLTE_ERROR;
     }
 
-    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, rvidx2, mimo_type, dci_unpacked.pinfo)) {
+    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, mimo_type, dci_unpacked.pinfo)) {
       ERROR("Configuing PDSCH");
       return SRSLTE_ERROR; 
     }
@@ -418,7 +425,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     q->nof_detected++;
   
     
-    if (q->pdsch_cfg.grant.mcs.mod > 0 && q->pdsch_cfg.grant.mcs.tbs >= 0) {
+    if (q->pdsch_cfg.grant.mcs[0].mod > 0 && q->pdsch_cfg.grant.mcs[0].tbs >= 0) {
       ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
                                     q->sf_symbols_m, q->ce_m, 
                                     noise_estimate, 
@@ -446,7 +453,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   q->pkts_total++;
 
   if (found_dci == 1 && ret == SRSLTE_SUCCESS) { 
-    return q->pdsch_cfg.grant.mcs.tbs;    
+    return q->pdsch_cfg.grant.mcs[0].tbs;
   } else {
     return 0;
   }
@@ -723,16 +730,16 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
   
   
-  srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits.nof_bits*sizeof(cf_t));
-  int cb_len = q->pdsch_cfg.cb_segm.K1; 
-  for (int i=0;i<q->pdsch_cfg.cb_segm.C;i++) {
+  srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits[0].nof_bits*sizeof(cf_t));
+  int cb_len = q->pdsch_cfg.cb_segm[0].K1;
+  for (int i=0;i<q->pdsch_cfg.cb_segm[0].C;i++) {
     char tmpstr[64]; 
     snprintf(tmpstr,64,"rmout_%d.dat",i);
     srslte_vec_save_file(tmpstr, softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));  
   }
   printf("Saved files for tti=%d, sf=%d, cfi=%d, mcs=%d, rv=%d, rnti=0x%x\n", tti, tti%10, cfi, 
-         q->pdsch_cfg.grant.mcs.idx, rv_idx, rnti);
+         q->pdsch_cfg.grant.mcs[0].idx, rv_idx, rnti);
 }
 
 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index eca637635..40fbe4b1f 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -632,7 +632,7 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
       if (LOG_THIS(rnti)) { 
         uint8_t x = 0;
         uint8_t *ptr = grants[i].data;
-        uint32_t len = phy_grant.mcs.tbs/8;
+        uint32_t len = phy_grant.mcs[0].tbs / (uint32_t) 8;
         if (!ptr) {          
           ptr = &x;
           len = 1; 
@@ -640,7 +640,7 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
         log_h->info_hex(ptr, len,
                              "PDSCH: rnti=0x%x, l_crb=%2d, %s, harq=%d, tbs=%d, mcs=%d, rv=%d, tti_tx=%d\n", 
                              rnti, phy_grant.nof_prb, grant_str, grants[i].grant.harq_process, 
-                             phy_grant.mcs.tbs/8, phy_grant.mcs.idx, grants[i].grant.rv_idx, tti_tx);
+                             phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
       if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, grants[i].softbuffer, rnti, grants[i].grant.rv_idx, sf_idx, 
                                   grants[i].data)) 
@@ -650,7 +650,7 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
       }
 
       // Save metrics stats 
-      ue_db[rnti].metrics_dl(phy_grant.mcs.idx);
+      ue_db[rnti].metrics_dl(phy_grant.mcs[0].idx);
     }
   }
   return SRSLTE_SUCCESS; 
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 8cdec22c2..ed1d7af26 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -76,9 +76,20 @@ private:
   /* ... for DL */
   bool decode_pdcch_ul(mac_interface_phy::mac_grant_t *grant);
   bool decode_pdcch_dl(mac_interface_phy::mac_grant_t *grant);
-  bool decode_phich(bool *ack); 
-  bool decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload, srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t pid);
-  bool decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS], srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS], int rv, uint16_t rnti, uint32_t pid);
+  bool decode_phich(bool *ack);
+
+  bool decode_pdsch(srslte_ra_dl_grant_t *grant,
+                    uint8_t *payload, srslte_softbuffer_rx_t *softbuffer,
+                    int rv,
+                    uint16_t rnti,
+                    uint32_t pid);
+
+  bool decode_pdsch_multi(srslte_ra_dl_grant_t *grant,
+                          uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                          srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                          int rv[SRSLTE_MAX_CODEWORDS],
+                          uint16_t rnti,
+                          uint32_t pid);
 
   /* ... for UL */
   void encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, srslte_softbuffer_tx_t *softbuffer, 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index fce931837..231190fb1 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -204,9 +204,9 @@ void phch_worker::work_imp()
       /* Decode PDSCH if instructed to do so */
       dl_ack = dl_action.default_ack; 
       if (dl_action.decode_enabled) {
-        dl_ack = decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr, 
+        dl_ack = decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr,
                               dl_action.softbuffer, dl_action.rv, dl_action.rnti, 
-                              dl_mac_grant.pid);              
+                              dl_mac_grant.pid);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
         phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
@@ -382,7 +382,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     /* Fill MAC grant structure */
     grant->ndi = dci_unpacked.ndi;
     grant->pid = dci_unpacked.harq_process;
-    grant->n_bytes = grant->phy_grant.dl.mcs.tbs/8;
+    grant->n_bytes = grant->phy_grant.dl.mcs[0].tbs/8;
     grant->tti = tti; 
     grant->rv  = dci_unpacked.rv_idx;
     grant->rnti = dl_rnti; 
@@ -406,23 +406,70 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
 }
 
 bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
-                               srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t harq_pid)
-{
-  return decode_pdsch_multi(grant, &payload, softbuffer, rv, rnti, harq_pid);
+                               srslte_softbuffer_rx_t *softbuffer, int rv,
+                               uint16_t rnti, uint32_t harq_pid) {
+  int _rv [SRSLTE_MAX_CODEWORDS] = {1};
+  _rv[0] = rv;
+
+  return decode_pdsch_multi(grant, &payload, softbuffer, _rv, rnti, harq_pid);
 }
 
 bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
-                               srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS], int rv, uint16_t rnti, uint32_t harq_pid)
-{
+                                     srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                     int rv[SRSLTE_MAX_CODEWORDS],
+                                     uint16_t rnti, uint32_t harq_pid) {
   char timestr[64];
+  bool valid_config = true;
   timestr[0]='\0';
-  
+  srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+
+  for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
+    if (rv[tb] < 0 || rv[tb] > 3) {
+      valid_config = false;
+      Error("Wrong RV (%d) for TB index %d", rv[tb], tb);
+    }
+  }
+
+  switch(phy->config->dedicated.antenna_info_explicit_value.tx_mode) {
+    case LIBLTE_RRC_TRANSMISSION_MODE_1:
+      mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_2:
+      mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_3:
+      if (grant->nof_tb == 1) {
+        mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      } else if (grant->nof_tb == 2) {
+        mimo_type = SRSLTE_MIMO_TYPE_CDD;
+      } else {
+        Error("Wrong number of transport blocks (%d) for TM3\n", grant->nof_tb);
+        valid_config = false;
+      }
+      break;
+
+    /* Not implemented cases */
+    case LIBLTE_RRC_TRANSMISSION_MODE_4:
+    case LIBLTE_RRC_TRANSMISSION_MODE_5:
+    case LIBLTE_RRC_TRANSMISSION_MODE_6:
+    case LIBLTE_RRC_TRANSMISSION_MODE_7:
+    case LIBLTE_RRC_TRANSMISSION_MODE_8:
+      Error("Not implemented Tx mode (%d)\n", phy->config->dedicated.antenna_info_explicit_value.tx_mode);
+      break;
+
+    /* Error cases */
+    case LIBLTE_RRC_TRANSMISSION_MODE_N_ITEMS:
+    default:
+      Error("Wrong Tx mode (%d)\n", phy->config->dedicated.antenna_info_explicit_value.tx_mode);
+      valid_config = false;
+  }
+
   Debug("DL Buffer TTI %d: Decoding PDSCH\n", tti);
 
   /* Setup PDSCH configuration for this CFI, SFIDX and RVIDX */
-  if (rv >= 0 && rv <= 3) {
-    if (!srslte_ue_dl_cfg_grant(&ue_dl, grant, cfi, tti%10, rv)) {
-      if (ue_dl.pdsch_cfg.grant.mcs.mod > 0 && ue_dl.pdsch_cfg.grant.mcs.tbs >= 0) {
+  if (valid_config) {
+    if (!srslte_ue_dl_cfg_grant_multi(&ue_dl, grant, cfi, tti%10, rv, mimo_type, 0)) {
+      if (ue_dl.pdsch_cfg.grant.mcs[0].mod > 0 && ue_dl.pdsch_cfg.grant.mcs[0].tbs >= 0) {
         
         float noise_estimate = srslte_chest_dl_get_noise_estimate(&ue_dl.chest);
         
@@ -451,7 +498,7 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
         
         Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n", 
               grant->nof_prb, harq_pid, 
-              grant->mcs.tbs/8, grant->mcs.idx, rv, 
+              grant->mcs[0].tbs/8, grant->mcs[0].idx, rv,
               ack?"OK":"KO", 
               10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), 
               srslte_pdsch_last_noi(&ue_dl.pdsch),
@@ -461,7 +508,7 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
         //srslte_vec_save_file("pdsch", signal_buffer, sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
         
         // Store metrics
-        dl_metrics.mcs    = grant->mcs.idx;
+        dl_metrics.mcs    = grant->mcs[0].idx;
         
         return ack; 
       } else {
@@ -470,8 +517,6 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
     } else {
       Error("Error configuring DL grant\n"); 
     }
-  } else {
-    Error("Error RV is not set or is invalid (%d)\n", rv);
   }
   return true; 
 }
@@ -973,8 +1018,9 @@ int phch_worker::read_ce_abs(float *ce_abs) {
 
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
 {
-  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  return ue_dl.pdsch_cfg.nbits.nof_re; 
+
+  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  return ue_dl.pdsch_cfg.nbits[0].nof_re;
 }
 
 

From 7e9e9a6a7d76da472ac193debb624ad9ebe0a771 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 21 Aug 2017 17:36:44 +0200
Subject: [PATCH 078/269] Included precoding information (pinfo) in grant and
 interfaced TM4

---
 lib/include/srslte/phy/phch/ra.h  |  1 +
 lib/include/srslte/phy/ue/ue_dl.h | 19 ++++++-------------
 lib/src/phy/phch/ra.c             |  1 +
 lib/src/phy/ue/ue_dl.c            | 24 ++++++++----------------
 srsue/src/phy/phch_worker.cc      | 14 ++++++++++++--
 5 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index c99dba1a1..58e9585a5 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -105,6 +105,7 @@ typedef struct SRSLTE_API {
   uint32_t Qm[SRSLTE_MAX_CODEWORDS];
   srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
   uint32_t nof_tb;
+  uint32_t pinfo;
 } srslte_ra_dl_grant_t;
 
 /** Unpacked DCI message for DL grant */
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 268a8c4d9..4101fadd9 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -141,19 +141,12 @@ SRSLTE_API int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q,
                                             uint32_t sf_idx, 
                                             uint32_t *cfi); 
 
-SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, 
-                                      srslte_ra_dl_grant_t *grant, 
-                                      uint32_t cfi, 
-                                      uint32_t sf_idx, 
-                                      int rvidx);
-
-SRSLTE_API int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q,
-                                            srslte_ra_dl_grant_t *grant,
-                                            uint32_t cfi,
-                                            uint32_t sf_idx,
-                                            int rvidx[SRSLTE_MAX_CODEWORDS],
-                                            srslte_mimo_type_t mimo_type,
-                                            uint32_t pinfo);
+SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q,
+                                      srslte_ra_dl_grant_t *grant,
+                                      uint32_t cfi,
+                                      uint32_t sf_idx,
+                                      int rvidx[SRSLTE_MAX_CODEWORDS],
+                                      srslte_mimo_type_t mimo_type);
 
 SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, 
                                         uint32_t cfi, 
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index a7fef1e42..00713e6c1 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -528,6 +528,7 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       grant->nof_tb++;
     }
   }
+  grant->pinfo = dci->pinfo;
 
   if (tbs < 0) {
     return SRSLTE_ERROR; 
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 47337a109..c9a5dea1a 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -283,31 +283,23 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 
 
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
-                           int rvidx) {
-  int _rvidx [SRSLTE_MAX_CODEWORDS] = {1};
-  _rvidx[0] = rvidx;
-
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, _rvidx, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
-}
-
-int srslte_ue_dl_cfg_grant_multi(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
-                                 int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type, uint32_t pinfo) {
+                                 int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type) {
   uint32_t pmi = 0;
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
   if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
     if (grant->nof_tb == 1) {
-      if (pinfo > 0 && pinfo < 5) {
-        pmi = pinfo - 1;
+      if (grant->pinfo > 0 && grant->pinfo < 5) {
+        pmi = grant->pinfo - 1;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     } else {
-      if (pinfo < 2) {
-        pmi = pinfo;
+      if (grant->pinfo < 2) {
+        pmi = grant->pinfo;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     }
@@ -415,7 +407,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
         return SRSLTE_ERROR;
     }
 
-    if (srslte_ue_dl_cfg_grant_multi(q, &grant, cfi, sf_idx, rvidx, mimo_type, dci_unpacked.pinfo)) {
+    if (srslte_ue_dl_cfg_grant(q, &grant, cfi, sf_idx, rvidx, mimo_type)) {
       ERROR("Configuing PDSCH");
       return SRSLTE_ERROR; 
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 231190fb1..6dabbbf01 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -431,6 +431,7 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
   }
 
   switch(phy->config->dedicated.antenna_info_explicit_value.tx_mode) {
+    /* Implemented Tx Modes */
     case LIBLTE_RRC_TRANSMISSION_MODE_1:
       mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
       break;
@@ -447,9 +448,18 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
         valid_config = false;
       }
       break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_4:
+      if (grant->nof_tb == 1) {
+        mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+      } else if (grant->nof_tb == 2) {
+        mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+      } else {
+        Error("Wrong number of transport blocks (%d) for TM4\n", grant->nof_tb);
+        valid_config = false;
+      }
+    break;
 
     /* Not implemented cases */
-    case LIBLTE_RRC_TRANSMISSION_MODE_4:
     case LIBLTE_RRC_TRANSMISSION_MODE_5:
     case LIBLTE_RRC_TRANSMISSION_MODE_6:
     case LIBLTE_RRC_TRANSMISSION_MODE_7:
@@ -468,7 +478,7 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
 
   /* Setup PDSCH configuration for this CFI, SFIDX and RVIDX */
   if (valid_config) {
-    if (!srslte_ue_dl_cfg_grant_multi(&ue_dl, grant, cfi, tti%10, rv, mimo_type, 0)) {
+    if (!srslte_ue_dl_cfg_grant(&ue_dl, grant, cfi, tti%10, rv, mimo_type)) {
       if (ue_dl.pdsch_cfg.grant.mcs[0].mod > 0 && ue_dl.pdsch_cfg.grant.mcs[0].tbs >= 0) {
         
         float noise_estimate = srslte_chest_dl_get_noise_estimate(&ue_dl.chest);

From 0f9f76ff00466f578fdd6e31f761bd24ea480847 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 22 Aug 2017 15:06:32 +0200
Subject: [PATCH 079/269] calibrated configuration values

---
 srsenb/enb.conf.example |  8 ++++----
 srsenb/rr.conf.example  |  2 +-
 srsenb/sib.conf.example | 28 ++++++++++++++--------------
 srsue/ue.conf.example   |  4 ++--
 4 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index cfba64a30..8cc063d41 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -57,8 +57,8 @@ drb_config = drb.conf
 #####################################################################
 [rf]
 dl_earfcn = 3400
-tx_gain = 70
-rx_gain = 50
+tx_gain = 80
+rx_gain = 60
 
 #device_name = auto
 #device_args = auto
@@ -141,9 +141,9 @@ nof_ctrl_symbols = 2
 #pdsch_max_its        = 4
 #nof_phy_threads      = 2
 #pregenerate_signals  = false
-#tx_amplitude         = 0.8
+#tx_amplitude         = 0.6
 #link_failure_nof_err = 50
-#rrc_inactivity_timer = 30000
+#rrc_inactivity_timer = 10000
 #max_prach_offset_us  = 30
 
 #####################################################################
diff --git a/srsenb/rr.conf.example b/srsenb/rr.conf.example
index 33a625359..830dd3bc6 100644
--- a/srsenb/rr.conf.example
+++ b/srsenb/rr.conf.example
@@ -28,7 +28,7 @@ phy_cnfg =
   {
     beta_offset_ack_idx = 10;
     beta_offset_ri_idx  = 5;
-    beta_offset_cqi_idx = 5;
+    beta_offset_cqi_idx = 10;
   };
   
   // PUCCH-SR resources are scheduled on time-frequeny domain first, then multiplexed in the same resource. 
diff --git a/srsenb/sib.conf.example b/srsenb/sib.conf.example
index 1ccfe5465..7002f4a7f 100644
--- a/srsenb/sib.conf.example
+++ b/srsenb/sib.conf.example
@@ -23,9 +23,9 @@ sib2 =
         rach_cnfg = 
         {
             num_ra_preambles = 52;
-            preamble_init_rx_target_pwr = -108;
+            preamble_init_rx_target_pwr = -104;
             pwr_ramping_step = 6;  // in dB
-            preamble_trans_max = 7;
+            preamble_trans_max = 10;
             ra_resp_win_size = 10;  // in ms
             mac_con_res_timer = 64; // in ms
             max_harq_msg3_tx = 4;
@@ -53,7 +53,7 @@ sib2 =
         pdsch_cnfg = 
         {
             p_b = 0; 
-            rs_power = -4;
+            rs_power = 20;
         };
         pusch_cnfg = 
         {
@@ -71,25 +71,25 @@ sib2 =
         };
         pucch_cnfg =
         {
-            delta_pucch_shift = 1;
-            n_rb_cqi = 1;
+            delta_pucch_shift = 2;
+            n_rb_cqi = 2;
             n_cs_an = 0;
-            n1_pucch_an = 2;
+            n1_pucch_an = 12;
         };
         ul_pwr_ctrl =
         {
-            p0_nominal_pusch = -108; 
-            alpha = 1.0; 
-            p0_nominal_pucch = -88; 
+            p0_nominal_pusch = -85;
+            alpha = 0.7;
+            p0_nominal_pucch = -107;
             delta_flist_pucch =
             {
-                format_1  = 2; 
+                format_1  = 0;
                 format_1b = 3; 
-                format_2  = 0; 
-                format_2a = 0; 
-                format_2b = 0; 
+                format_2  = 1;
+                format_2a = 2;
+                format_2b = 2;
             };
-            delta_preamble_msg3 = 4;
+            delta_preamble_msg3 = 8;
         };
         ul_cp_length = "Normal";
     };
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 90d974859..8c9ba7d3d 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -23,8 +23,8 @@
 [rf]
 dl_freq = 2685000000
 ul_freq = 2565000000
-tx_gain = 70
-rx_gain = 50
+tx_gain = 80
+rx_gain = 60
 
 #nof_rx_ant = 1
 #device_name = auto

From 5fed7668065872c6077639302d06e586f1a28303 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 22 Aug 2017 15:06:51 +0200
Subject: [PATCH 080/269] fixed UE not reattaching after paging

---
 lib/include/srslte/upper/rlc.h        | 1 +
 lib/include/srslte/upper/rlc_entity.h | 1 +
 lib/src/phy/phch/uci.c                | 2 +-
 lib/src/upper/rlc.cc                  | 8 ++++++++
 lib/src/upper/rlc_entity.cc           | 6 ++++++
 srsenb/src/mac/mac.cc                 | 2 +-
 srsenb/src/main.cc                    | 4 ++--
 srsenb/src/phy/phch_worker.cc         | 5 ++---
 srsenb/src/upper/rlc.cc               | 4 ++--
 srsenb/src/upper/rrc.cc               | 6 +++---
 10 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/lib/include/srslte/upper/rlc.h b/lib/include/srslte/upper/rlc.h
index 11ce507d5..349a7952d 100644
--- a/lib/include/srslte/upper/rlc.h
+++ b/lib/include/srslte/upper/rlc.h
@@ -77,6 +77,7 @@ public:
 
   // RRC interface
   void reset();
+  void empty_queue();
   void add_bearer(uint32_t lcid);
   void add_bearer(uint32_t lcid, srslte_rlc_config_t cnfg);
 
diff --git a/lib/include/srslte/upper/rlc_entity.h b/lib/include/srslte/upper/rlc_entity.h
index 3747f9410..651af72fb 100644
--- a/lib/include/srslte/upper/rlc_entity.h
+++ b/lib/include/srslte/upper/rlc_entity.h
@@ -56,6 +56,7 @@ public:
 
   void configure(srslte_rlc_config_t cnfg);
   void reset();
+  void empty_queue();
   bool active();
 
   rlc_mode_t    get_mode();
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 821e5be5f..5bf40cc17 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -380,7 +380,7 @@ int decode_cqi_long(srslte_uci_cqi_pusch_t *q, int16_t *q_bits, uint32_t Q,
     ret = srslte_crc_checksum(&q->crc, q->tmp_cqi, nof_bits + 8);
    if (ret == 0) {
       memcpy(data, q->tmp_cqi, nof_bits*sizeof(uint8_t));
-      ret = 1; 
+      ret = 1;
     } else {
       ret = 0; 
     }
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index 9a7b50448..0cc551a1f 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -102,6 +102,14 @@ void rlc::reset()
   rlc_array[0].init(RLC_MODE_TM, rlc_log, default_lcid, pdcp, rrc, mac_timers); // SRB0
 }
 
+void rlc::empty_queue()
+{
+  for(uint32_t i=0; i<SRSLTE_N_RADIO_BEARERS; i++) {
+    if(rlc_array[i].active())
+      rlc_array[i].empty_queue();
+  }
+}
+
 /*******************************************************************************
   PDCP interface
 *******************************************************************************/
diff --git a/lib/src/upper/rlc_entity.cc b/lib/src/upper/rlc_entity.cc
index 541649897..247bf4409 100644
--- a/lib/src/upper/rlc_entity.cc
+++ b/lib/src/upper/rlc_entity.cc
@@ -76,6 +76,12 @@ void rlc_entity::reset()
   rlc = NULL;
 }
 
+
+void rlc_entity::empty_queue()
+{
+  rlc->empty_queue();
+}
+
 bool rlc_entity::active()
 {
   return (rlc != NULL);
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index d45330863..5711ccce0 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -259,7 +259,7 @@ void mac::rl_failure(uint16_t rnti)
   if (ue_db.count(rnti)) {         
     uint32_t nof_fails = ue_db[rnti]->rl_failure();  
     if (nof_fails >= (uint32_t) args.link_failure_nof_err && args.link_failure_nof_err > 0) {    
-      Info("Detected PUSCH failure for rnti=0x%x\n", rnti);
+      Info("Detected Uplink failure for rnti=0x%x\n", rnti);
       rrc_h->rl_failure(rnti);
       ue_db[rnti]->rl_failure_reset();
     }
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 92669ba69..bc6aedc16 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -152,7 +152,7 @@ void parse_args(all_args_t *args, int argc, char* argv[]) {
         "Maximum number of turbo decoder iterations")
 
     ("expert.tx_amplitude",
-        bpo::value<float>(&args->expert.phy.tx_amplitude)->default_value(0.8),
+        bpo::value<float>(&args->expert.phy.tx_amplitude)->default_value(0.6),
         "Transmit amplitude factor")
 
     ("expert.nof_phy_threads",
@@ -176,7 +176,7 @@ void parse_args(all_args_t *args, int argc, char* argv[]) {
         "Chooses the coefficients for the 3-tap channel estimator centered filter.")
 
     ("expert.rrc_inactivity_timer",
-        bpo::value<uint32_t>(&args->expert.rrc_inactivity_timer)->default_value(30000),
+        bpo::value<uint32_t>(&args->expert.rrc_inactivity_timer)->default_value(10000),
         "Inactivity timer in ms")
 
 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index eca637635..187d8b49f 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -109,7 +109,7 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     return;
   }
   
-  srslte_pucch_set_threshold(&enb_ul.pucch, 0.8, 0.5); 
+  srslte_pucch_set_threshold(&enb_ul.pucch, 0.5, 0.5);
   srslte_sch_set_max_noi(&enb_ul.pusch.ul_sch, phy->params.pusch_max_its);
   srslte_enb_dl_set_amp(&enb_dl, phy->params.tx_amplitude);
   
@@ -355,7 +355,6 @@ int phch_worker::decode_pusch(srslte_enb_ul_pusch_t *grants, uint32_t nof_pusch,
       }
       if (cqi_enabled) {
         uci_data.uci_cqi_len = srslte_cqi_size(&cqi_value);
-        Info("cqi enabled len=%d\n", uci_data.uci_cqi_len);
       }
       
       // mark this tti as having an ul grant to avoid pucch 
@@ -415,7 +414,7 @@ int phch_worker::decode_pusch(srslte_enb_ul_pusch_t *grants, uint32_t nof_pusch,
       }
       */
       log_h->info_hex(grants[i].data, phy_grant.mcs.tbs/8,
-          "PUSCH: rnti=0x%x, prb=(%d,%d), tbs=%d, mcs=%d, rv=%d, snr=%.1f dB, n_iter=%d, crc=%s%s%s%s\n", 
+          "PUSCH: rnti=0x%x, prb=(%d,%d), tbs=%d, mcs=%d, rv=%d, snr=%.1f dB, n_iter=%d, crc=%s%s%s%s\n",
           rnti, phy_grant.n_prb[0], phy_grant.n_prb[0]+phy_grant.L_prb,
           phy_grant.mcs.tbs/8, phy_grant.mcs.idx, grants[i].grant.rv_idx,
           snr_db, 
diff --git a/srsenb/src/upper/rlc.cc b/srsenb/src/upper/rlc.cc
index 758a3b5b3..6147597e3 100644
--- a/srsenb/src/upper/rlc.cc
+++ b/srsenb/src/upper/rlc.cc
@@ -83,11 +83,11 @@ void rlc::reset(uint16_t rnti)
 void rlc::clear_buffer(uint16_t rnti)
 {
   if (users.count(rnti)) {
-    log_h->info("Clearing buffer rnti=0x%x\n", rnti);
-    users[rnti].rlc->reset();
+    users[rnti].rlc->empty_queue();
     for (int i=0;i<SRSLTE_N_RADIO_BEARERS;i++) {
       mac->rlc_buffer_state(rnti, i, 0, 0);      
     }
+    log_h->info("Cleared buffer rnti=0x%x\n", rnti);
   }
 }
 
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 331efdbd5..7c06ebab7 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -313,7 +313,7 @@ void rrc::release_complete(uint16_t rnti)
       rlc->clear_buffer(rnti); 
       users[rnti].send_connection_release();
       // There is no RRCReleaseComplete message from UE thus sleep to enable all retx in PHY +50%
-      usleep(1.5*8*1e3*cfg.mac_cnfg.ulsch_cnfg.max_harq_tx);
+      usleep(1500*8*cfg.mac_cnfg.ulsch_cnfg.max_harq_tx);
     }
     rem_user(rnti);
   } else {
@@ -437,7 +437,7 @@ void rrc::add_paging_id(uint32_t ueid, LIBLTE_S1AP_UEPAGINGID_STRUCT UEPagingID)
 // Described in Section 7 of 36.304
 bool rrc::is_paging_opportunity(uint32_t tti, uint32_t *payload_len)
 {
-  int sf_pattern[4][3] = {{9, 4, 0}, {-1, 9, 4}, {-1, -1, 5}, {-1, -1, 9}}; 
+  int sf_pattern[4][4] = {{9, 4, -1, 0}, {-1, 9, -1, 4}, {-1, -1, -1, 5}, {-1, -1, -1, 9}};
   
   if (pending_paging.empty()) {
     return false; 
@@ -466,7 +466,7 @@ bool rrc::is_paging_opportunity(uint32_t tti, uint32_t *payload_len)
     
     if ((sfn % T) == (T/N) * (ueid % N)) {
             
-      int sf_idx = sf_pattern[i_s%4][(Ns-1)%3]; 
+      int sf_idx = sf_pattern[i_s%4][(Ns-1)%4];
       if (sf_idx < 0) {
         rrc_log->error("SF pattern is N/A for Ns=%d, i_s=%d, imsi_decimal=%d\n", Ns, i_s, ueid);
       } else if ((uint32_t) sf_idx == (tti%10)) {

From 49e8a2c5941bbf08fd0ed524460361c20c668cf5 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 23 Aug 2017 12:33:33 +0200
Subject: [PATCH 081/269] fixed memory leak in mac/ue

---
 srsenb/hdr/mac/ue.h  | 4 ++++
 srsenb/src/mac/ue.cc | 2 ++
 2 files changed, 6 insertions(+)

diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index bb3597a04..ef2df2f22 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -58,6 +58,10 @@ public:
   }
   
   virtual ~ue() {
+    for (int i=0;i<NOF_HARQ_PROCESSES;i++) {
+      srslte_softbuffer_rx_free(&softbuffer_rx[i]);
+      srslte_softbuffer_tx_free(&softbuffer_tx[i]);
+    }
     pthread_mutex_destroy(&mutex);
   }
   
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index cae9b6899..cf82b5353 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -60,6 +60,8 @@ void ue::config(uint16_t rnti_, uint32_t nof_prb, sched_interface *sched_, rrc_i
   }
 }  
 
+
+
 void ue::reset()
 {
   bzero(&metrics, sizeof(mac_metrics_t));  

From dea557f345ec8d81e9f7dc05f8bdfbc4880f5310 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 23 Aug 2017 12:43:20 +0200
Subject: [PATCH 082/269] Fixes #89. Removed sampling rate warnings

---
 lib/src/phy/rf/rf_uhd_imp.c | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 319093994..3a047ceba 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -328,7 +328,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
     if (args[0]=='\0') {
       if (find_string(devices_str, "type=b200") && !strstr(args, "recv_frame_size")) {
         // If B200 is available, use it
-        args = "type=b200";        
+        args = "type=b200,master_clock_rate=30.72e6";
         handler->devname = DEVNAME_B200;
       } else if (find_string(devices_str, "type=x300")) {
         // Else if X300 is available, set master clock rate now (can't be changed later)
@@ -344,6 +344,8 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
         handler->dynamic_rate = false; 
         handler->devname = DEVNAME_X300;
       } else if (strstr(args, "type=b200")) {
+        snprintf(args2, sizeof(args2), "%s,master_clock_rate=30.72e6", args);
+        args = args2;
         handler->devname = DEVNAME_B200;
       }
     }        
@@ -399,7 +401,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
       };
       
     handler->nof_rx_channels = nof_rx_antennas; 
-    handler->nof_tx_channels = 1; 
+    handler->nof_tx_channels = 1;
+
+    /* Set default rate to avoid decimation warnings */
+    uhd_usrp_set_rx_rate(handler->usrp, 1.92e6, 0);
+    uhd_usrp_set_tx_rate(handler->usrp, 1.92e6, 0);
     
     /* Initialize rx and tx stremers */
     uhd_rx_streamer_make(&handler->rx_stream);

From 8e814003296b5b217d0ed18ff8d71cd52df3e23e Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 23 Aug 2017 15:34:40 +0200
Subject: [PATCH 083/269] Solved bug: UE DL was not getting into TxDiversity on
 DCI formats 1 and 1A

---
 lib/src/phy/ue/ue_dl.c | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index c9a5dea1a..26dc0b908 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -379,7 +379,11 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     switch(dci_msg.format) {
       case SRSLTE_DCI_FORMAT1:
       case SRSLTE_DCI_FORMAT1A:
-        mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+        if (q->cell.nof_ports == 1) {
+          mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        }
         break;
       case SRSLTE_DCI_FORMAT2:
         if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
@@ -456,7 +460,10 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
   float best_sinr = -INFINITY;
   uint32_t best_pmi = 0, best_ri = 0;
 
-  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+  if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
+    /* Do nothing */
+    return SRSLTE_SUCCESS;
+  } else if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
     if (srslte_pdsch_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
                                   SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), q->pmi, q->sinr)) {
       ERROR("SINR calculation error");
@@ -489,7 +496,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
       } else if (q->pdsch_cfg.nof_layers == 2) {
         *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
       } else {
-        ERROR("Not implemented number of layers");
+        ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
         return SRSLTE_ERROR;
       }
     }

From 02a4bb6bddca224d5d17c6eb606e5e24d208ac8e Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 23 Aug 2017 15:35:48 +0200
Subject: [PATCH 084/269] Improved PDSCH UE debug

---
 lib/examples/pdsch_ue.c | 45 ++++++++++++++++++++++++++++++++---------
 1 file changed, 36 insertions(+), 9 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 60cc0a4db..8c4d57484 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -101,6 +101,7 @@ typedef struct {
   int net_port_signal; 
   char *net_address_signal;
   int decimate;
+  int verbose;
 }prog_args_t;
 
 void args_default(prog_args_t *args) {
@@ -131,6 +132,7 @@ void args_default(prog_args_t *args) {
   args->net_address_signal = "127.0.0.1";
   args->decimate = 0;
   args->cpu_affinity = -1;
+  args->verbose = 0;
 }
 
 void usage(prog_args_t *args, char *prog) {
@@ -187,6 +189,8 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
       break;
     case 'o':
       args->file_offset_freq = atof(argv[optind]);
+        argv++;
+        argc--;
       break;
     case 'O':
       args->file_offset_time = atoi(argv[optind]);
@@ -240,7 +244,8 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
       args->disable_plots_except_constellation = true;
       break;
     case 'v':
-      srslte_verbose++;
+      args->verbose++;
+      srslte_verbose = args->verbose;
       break;
     case 'Z':
       args->decimate = atoi(argv[optind]);
@@ -392,8 +397,8 @@ int main(int argc, char **argv) {
     srslte_rf_set_master_clock_rate(&rf, 30.72e6);        
 
     /* set receiver frequency */
-    printf("Tunning receiver to %.3f MHz\n", prog_args.rf_freq/1000000);
-    srslte_rf_set_rx_freq(&rf, prog_args.rf_freq);
+    printf("Tunning receiver to %.3f MHz\n", (prog_args.rf_freq + prog_args.file_offset_freq)/1000000);
+    srslte_rf_set_rx_freq(&rf, prog_args.rf_freq + prog_args.file_offset_freq);
     srslte_rf_rx_wait_lo_locked(&rf);
 
     uint32_t ntrial=0; 
@@ -416,7 +421,7 @@ int main(int argc, char **argv) {
     srslte_rf_flush_buffer(&rf);    
 
     /* set sampling frequency */
-    int srate = srslte_sampling_freq_hz(cell.nof_prb);    
+    int srate = srslte_sampling_freq_hz(cell.nof_prb)*(1 + prog_args.file_offset_freq/prog_args.rf_freq);
     if (srate != -1) {  
       if (srate < 10e6) {          
         srslte_rf_set_master_clock_rate(&rf, 4*srate);        
@@ -540,7 +545,27 @@ int main(int argc, char **argv) {
   INFO("\nEntering main loop...\n\n", 0);
   /* Main loop */
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
-    
+    char input[128];
+
+    fd_set set;
+    FD_ZERO(&set);
+    FD_SET(0, &set);
+
+    struct timeval to;
+    to.tv_sec = 0;
+    to.tv_usec = 0;
+
+    /* Set default verbose level */
+    srslte_verbose = prog_args.verbose;
+    int n = select(1, &set, NULL, NULL, &to);
+    if (n == 1) {
+      /* If a new line is detected set verbose level to Debug */
+      if (fgets(input, sizeof(input), stdin)) {
+        srslte_verbose = SRSLTE_VERBOSE_DEBUG;
+      }
+    }
+
+
     ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
     if (ret < 0) {
       fprintf(stderr, "Error calling srslte_ue_sync_work()\n");
@@ -673,7 +698,9 @@ int main(int argc, char **argv) {
               printf("\033[K           Rb: %6.2f / %6.2f Mbps (net/maximum)\n", uerate, enodebrate);
               printf("\033[K   PDCCH-Miss: %5.2f%%\n", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
               printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
-              printf("\033[K   PDSCH-BLER: %5.2f%%\n\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+              printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+              printf("\033[K         TB 0: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[0].idx, ue_dl.pdsch_cfg.grant.mcs[0].tbs);
+              printf("\033[K         TB 1: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[1].idx, ue_dl.pdsch_cfg.grant.mcs[1].tbs);
               printf("\033[K\n");
               printf("\033[KSINR (dB) Vs RI and PMI:\n");
               printf("\033[K   | RI |   1   |   2   |\n");
@@ -683,7 +710,7 @@ int main(int argc, char **argv) {
               printf("\033[K I |  2 | %5.2f%c|-------+ \n", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2)?'*':' ');
               printf("\033[K   |  3 | %5.2f%c|         \n", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3)?'*':' ');
               printf("\033[K\n\n");
-              printf("\033[20A");
+              printf("\033[21A");
             }
           }
           break;
@@ -692,7 +719,7 @@ int main(int argc, char **argv) {
         sfn++; 
         if (sfn == 1024) {
           sfn = 0; 
-          printf("\033[20B");
+          printf("\033[21B");
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
@@ -724,7 +751,7 @@ int main(int argc, char **argv) {
         
     sf_cnt++;                  
   } // Main loop
-  printf("\033[20B");
+  printf("\033[21B\n");
 
 #ifndef DISABLE_GRAPHICS
   if (!prog_args.disable_plots) {

From ecaf84de5193f4f453c0ec5fafa27e55f4b03c1c Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 23 Aug 2017 15:30:13 +0200
Subject: [PATCH 085/269] pdsch_test can load MIMO files

---
 lib/src/phy/phch/test/pdsch_test.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 777481d6a..41b05f229 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -317,7 +317,7 @@ int main(int argc, char **argv) {
 #ifdef DO_OFDM
     srslte_filesource_read(&fsrc, rx_slot_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
 #else
-    srslte_filesource_read(&fsrc, rx_slot_symbols[0], SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
+    srslte_filesource_read_multi(&fsrc, (void*) rx_slot_symbols, SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp), pdsch_cfg.nof_layers);
 #endif
     
     srslte_chest_dl_t chest; 
@@ -325,7 +325,7 @@ int main(int argc, char **argv) {
       fprintf(stderr, "Error initializing equalizer\n");
       exit(-1);
     }
-    srslte_chest_dl_estimate(&chest, rx_slot_symbols[0], ce[0], subframe);
+    srslte_chest_dl_estimate_multi(&chest, rx_slot_symbols, ce, subframe, nof_rx_antennas);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);

From a1060c067af960cab53b969170cfbdc51ad8f559 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 23 Aug 2017 15:35:48 +0200
Subject: [PATCH 087/269] Improved PDSCH UE debug

(cherry picked from commit cef312c)
---
 lib/examples/pdsch_ue.c | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 8c4d57484..4b7a3f24b 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -318,7 +318,6 @@ int main(int argc, char **argv) {
   srslte_rf_t rf; 
 #endif
   uint32_t nof_trials = 0; 
-  int n; 
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
   int sfn_offset;
   float cfo = 0; 
@@ -690,7 +689,12 @@ int main(int argc, char **argv) {
               }
 
               /* Print Results */
-              printf("\033[K    Tx scheme: %-10s\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
+              if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+                printf("\033[K    Tx scheme: %s (codebook_idx=%d)\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type),
+                       ue_dl.pdsch_cfg.codebook_idx);
+              } else {
+                printf("\033[K    Tx scheme: %s\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
+              }
               printf("\033[K   nof layers: %d \n", ue_dl.pdsch_cfg.nof_layers);
               printf("\033[Knof codewords: %d \n", ue_dl.pdsch_cfg.grant.nof_tb);
               printf("\033[K          CFO: %+5.2f kHz\n", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
@@ -702,7 +706,7 @@ int main(int argc, char **argv) {
               printf("\033[K         TB 0: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[0].idx, ue_dl.pdsch_cfg.grant.mcs[0].tbs);
               printf("\033[K         TB 1: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[1].idx, ue_dl.pdsch_cfg.grant.mcs[1].tbs);
               printf("\033[K\n");
-              printf("\033[KSINR (dB) Vs RI and PMI:\n");
+              printf("\033[KSINR (dB) Vs RI and PMI (for TM4, close loop MIMO only):\n");
               printf("\033[K   | RI |   1   |   2   |\n");
               printf("\033[K -------+-------+-------+\n");
               printf("\033[K P |  0 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][0]), (ri == 1 && pmi == 0)?'*':' ', 10 * log10(sinr[1][0]), (ri == 2 && pmi == 0)?'*':' ');

From 616e18c5707e55197dd5b39123d1ebc4a4740e4d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 24 Aug 2017 15:16:13 +0200
Subject: [PATCH 088/269] fixed PUCCH correlation estimator

---
 lib/include/srslte/phy/phch/pucch.h   |  6 ++---
 lib/include/srslte/phy/utils/vector.h |  3 +++
 lib/src/phy/enb/enb_ul.c              |  2 +-
 lib/src/phy/phch/pucch.c              | 35 ++++++++++++++++++---------
 lib/src/phy/utils/vector.c            |  9 +++++++
 srsenb/src/mac/ue.cc                  |  4 +--
 srsenb/src/phy/phch_worker.cc         |  6 ++---
 7 files changed, 43 insertions(+), 22 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index 56d512418..da0b5b81e 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -108,7 +108,6 @@ typedef struct SRSLTE_API {
   bool group_hopping_en;
 
   float threshold_format1;
-  float threshold_format1a;
   float last_corr;
   uint32_t last_n_prb;
   uint32_t last_n_pucch;
@@ -125,9 +124,8 @@ SRSLTE_API bool srslte_pucch_set_cfg(srslte_pucch_t* q,
                                      srslte_pucch_cfg_t* cfg, 
                                      bool group_hopping_en); 
 
-SRSLTE_API void srslte_pucch_set_threshold(srslte_pucch_t *q, 
-                                           float format1, 
-                                           float format1a); 
+SRSLTE_API void srslte_pucch_set_threshold(srslte_pucch_t *q,
+                                           float format1_threshold);
 
 SRSLTE_API int srslte_pucch_set_crnti(srslte_pucch_t *q, 
                                       uint16_t c_rnti); 
diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index 74cd60172..d203ff390 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -152,6 +152,9 @@ SRSLTE_API void srslte_vec_conj_cc(cf_t *x, cf_t *y, uint32_t len);
 /* average vector power */
 SRSLTE_API float srslte_vec_avg_power_cf(cf_t *x, uint32_t len);
 
+/* Correlation between complex vectors x and y */
+SRSLTE_API float srslte_vec_corr_ccc(cf_t *x, cf_t *y, uint32_t len);
+
 /* return the index of the maximum value in the vector */
 SRSLTE_API uint32_t srslte_vec_max_fi(float *x, uint32_t len);
 SRSLTE_API uint32_t srslte_vec_max_abs_ci(cf_t *x, uint32_t len);
diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index 9c294ac43..9d773d6d5 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -91,7 +91,7 @@ int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell,
       srslte_prach_set_detect_factor(&q->prach, 60);    
     }
     
-    srslte_pucch_set_threshold(&q->pucch, 0.5, 0.5);
+    srslte_pucch_set_threshold(&q->pucch, 0.8);
     
     if (srslte_chest_ul_init(&q->chest, cell)) {
       fprintf(stderr, "Error initiating channel estimator\n");
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 6a889b89c..928a51019 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -214,15 +214,15 @@ uint32_t srslte_pucch_get_npucch(uint32_t n_cce, srslte_pucch_format_t format, b
 {
   uint32_t n_pucch = 0; 
   if (has_scheduling_request) {
-    n_pucch = pucch_sched->n_pucch_sr; 
+    n_pucch = pucch_sched->n_pucch_sr;
   } else if (format < SRSLTE_PUCCH_FORMAT_2) {
     if (pucch_sched->sps_enabled) {
       n_pucch = pucch_sched->n_pucch_1[pucch_sched->tpc_for_pucch%4];
     } else {
-      n_pucch = n_cce + pucch_sched->N_pucch_1; 
+      n_pucch = n_cce + pucch_sched->N_pucch_1;
     }
   } else {
-    n_pucch = pucch_sched->n_pucch_2; 
+    n_pucch = pucch_sched->n_pucch_2;
   }
   return n_pucch;
 }
@@ -411,9 +411,8 @@ static int pucch_get(srslte_pucch_t *q, srslte_pucch_format_t format, uint32_t n
   return pucch_cp(q, format, n_pucch, input, z, true);
 }
 
-void srslte_pucch_set_threshold(srslte_pucch_t *q, float format1, float format1a) {
-  q->threshold_format1  = format1; 
-  q->threshold_format1a = format1a; 
+void srslte_pucch_set_threshold(srslte_pucch_t *q, float format1_threshold) {
+  q->threshold_format1  = format1_threshold;
 }
 
 /** Initializes the PDCCH transmitter and receiver */
@@ -452,6 +451,8 @@ int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
     q->z_tmp = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
     q->ce = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
 
+    q->threshold_format1  = 0.8;
+
     ret = SRSLTE_SUCCESS;
   }
   return ret;
@@ -612,6 +613,10 @@ static int uci_mod_bits(srslte_pucch_t *q, srslte_pucch_format_t format, uint8_t
 // Declare this here, since we can not include refsignal_ul.h
 void srslte_refsignal_r_uv_arg_1prb(float *arg, uint32_t u);
 
+
+float tmp_alpha;
+uint32_t tmp_noc, tmp_nprime, tmp_woc;
+
 static int pucch_encode_(srslte_pucch_t* q, srslte_pucch_format_t format, 
                           uint32_t n_pucch, uint32_t sf_idx, uint16_t rnti,
                           uint8_t bits[SRSLTE_PUCCH_MAX_BITS], cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS], bool signal_only) 
@@ -655,8 +660,14 @@ static int pucch_encode_(srslte_pucch_t* q, srslte_pucch_format_t format,
         if (n_prime_ns%2) {
           S_ns = M_PI/2;
         }
-        DEBUG("PUCCH d_0: %.1f+%.1fi, alpha: %.1f, n_oc: %d, n_prime_ns: %d, n_rb_2=%d\n", 
+        DEBUG("PUCCH d_0: %.1f+%.1fi, alpha: %.1f, n_oc: %d, n_prime_ns: %d, n_rb_2=%d\n",
               __real__ q->d[0], __imag__ q->d[0], alpha, n_oc, n_prime_ns, q->pucch_cfg.n_rb_2);
+
+        tmp_alpha = alpha;
+        tmp_noc   = n_oc;
+        tmp_nprime = n_prime_ns;
+        tmp_woc   = w_n_oc[N_sf_widx][n_oc%3][m];
+
         for (uint32_t n=0;n<SRSLTE_PUCCH_N_SEQ;n++) {
           z[(ns%2)*N_sf_0*SRSLTE_PUCCH_N_SEQ+m*SRSLTE_PUCCH_N_SEQ+n] = 
             q->d[0]*cexpf(I*(w_n_oc[N_sf_widx][n_oc%3][m]+q->tmp_arg[n]+alpha*n+S_ns));
@@ -767,7 +778,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
       case SRSLTE_PUCCH_FORMAT_1:
         bzero(bits, SRSLTE_PUCCH_MAX_BITS*sizeof(uint8_t));
         pucch_encode(q, format, n_pucch, sf_idx, rnti, bits, q->z_tmp);
-        corr = crealf(srslte_vec_dot_prod_conj_ccc(q->z, q->z_tmp, nof_re))/nof_re;
+        corr = srslte_vec_corr_ccc(q->z, q->z_tmp, nof_re);
         if (corr >= q->threshold_format1) {
           ret = 1; 
         } else {
@@ -778,11 +789,11 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
         break;
       case SRSLTE_PUCCH_FORMAT_1A:
         bzero(bits, SRSLTE_PUCCH_MAX_BITS*sizeof(uint8_t));
-        ret = 0; 
+        ret = 0;
         for (int b=0;b<2;b++) {
-          bits[0] = b; 
+          bits[0] = b;
           pucch_encode(q, format, n_pucch, sf_idx, rnti, bits, q->z_tmp);
-          corr = crealf(srslte_vec_dot_prod_conj_ccc(q->z, q->z_tmp, nof_re))/nof_re;          
+          corr = srslte_vec_corr_ccc(q->z, q->z_tmp, nof_re);
           if (corr > corr_max) {
             corr_max = corr; 
             b_max = b; 
@@ -790,7 +801,7 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
           if (corr_max > q->threshold_format1) { // check with format1 in case ack+sr because ack only is binary
             ret = 1; 
           }
-          DEBUG("format1a b=%d, corr=%f, nof_re=%d, th=%f\n", b, corr, nof_re, q->threshold_format1a);
+          DEBUG("format1a b=%d, corr=%f, nof_re=%d\n", b, corr, nof_re);
         }
         q->last_corr = corr_max; 
         bits[0] = b_max; 
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 30527510d..b76c87645 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -681,6 +681,15 @@ float srslte_vec_avg_power_cf(cf_t *x, uint32_t len) {
   return crealf(srslte_vec_dot_prod_conj_ccc(x,x,len)) / len;
 }
 
+// Correlation assumes zero-mean x and y
+float srslte_vec_corr_ccc(cf_t *x, cf_t *y, uint32_t len) {
+//  return crealf(srslte_vec_dot_prod_conj_ccc(x,y,len)) / len;
+  float s_x = crealf(srslte_vec_dot_prod_conj_ccc(x, x, len))/len;
+  float s_y = crealf(srslte_vec_dot_prod_conj_ccc(y, y, len))/len;
+  float cov = crealf(srslte_vec_dot_prod_conj_ccc(x, y, len))/len;
+  return cov/(sqrt(s_x*s_y));
+}
+
 void srslte_vec_abs_cf(cf_t *x, float *abs, uint32_t len) {
 #ifndef HAVE_VOLK_MAG_FUNCTION
   int i;
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index cf82b5353..123f66e46 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -248,12 +248,12 @@ bool ue::process_ce(srslte::sch_subh *subh) {
       idx = subh->get_bsr(buff_size);
       if (idx > 0) {
         // Indicate BSR to scheduler 
-        sched->ul_bsr(rnti, idx, buff_size[idx]);
+        sched->ul_bsr(rnti, idx, 2*buff_size[idx]);
         Info("CE:    Received BSR rnti=0x%x, lcid=%d, value=%d\n", rnti, idx, buff_size[idx]);        
       } else if (idx == 0) {
         // TODO: map lcid group to lcid
         for (int i=0;i<4;i++) {
-          sched->ul_bsr(rnti, i, buff_size[i]);
+          sched->ul_bsr(rnti, i, 2*buff_size[i]);
         }
         Info("CE:    Received Long BSR rnti=0x%x, value=%d,%d,%d,%d\n", rnti, 
              buff_size[0], buff_size[1], buff_size[2], buff_size[3]);        
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 187d8b49f..095a4643c 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -109,7 +109,7 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     return;
   }
   
-  srslte_pucch_set_threshold(&enb_ul.pucch, 0.5, 0.5);
+  srslte_pucch_set_threshold(&enb_ul.pucch, 0.8);
   srslte_sch_set_max_noi(&enb_ul.pusch.ul_sch, phy->params.pusch_max_its);
   srslte_enb_dl_set_amp(&enb_dl, phy->params.tx_amplitude);
   
@@ -178,8 +178,8 @@ void phch_worker::set_config_dedicated(uint16_t rnti,
     srslte_enb_ul_cfg_ue(&enb_ul, rnti, uci_cfg, pucch_sched, srs_cfg);
         
     ue_db[rnti].I_sr    = I_sr; 
-    ue_db[rnti].I_sr_en = true; 
-    
+    ue_db[rnti].I_sr_en = true;
+
     if (pucch_cqi) {
       ue_db[rnti].pmi_idx = pmi_idx; 
       ue_db[rnti].cqi_en  = true;       

From 955170bd52dbf15b1c8e6fb2c0041b7ffe5cb9b7 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 28 Aug 2017 10:06:12 +0200
Subject: [PATCH 089/269] Improved DL TCP performance by tweeking BSR reports
 and UL scheduler

---
 .../srslte/interfaces/sched_interface.h       |   5 +-
 lib/src/common/pdu.cc                         |  10 +-
 srsenb/drb.conf.example                       |   6 +-
 srsenb/hdr/mac/scheduler.h                    |   2 +-
 srsenb/hdr/mac/scheduler_harq.h               |   3 +-
 srsenb/hdr/mac/scheduler_ue.h                 |  11 +-
 srsenb/hdr/mac/ue.h                           |   8 +-
 srsenb/rr.conf.example                        |   2 +-
 srsenb/src/mac/mac.cc                         |   5 +-
 srsenb/src/mac/scheduler.cc                   |  20 ++-
 srsenb/src/mac/scheduler_harq.cc              |  17 ++-
 srsenb/src/mac/scheduler_ue.cc                |  18 ++-
 srsenb/src/mac/ue.cc                          | 133 +++++++++++-------
 srsenb/src/upper/rrc.cc                       |   4 +
 srsue/ue.conf.example                         |   2 +-
 15 files changed, 165 insertions(+), 81 deletions(-)

diff --git a/lib/include/srslte/interfaces/sched_interface.h b/lib/include/srslte/interfaces/sched_interface.h
index 63ab92121..a717b4293 100644
--- a/lib/include/srslte/interfaces/sched_interface.h
+++ b/lib/include/srslte/interfaces/sched_interface.h
@@ -94,7 +94,8 @@ public:
   typedef struct {
     int priority; 
     int bsd; 
-    int pbr; 
+    int pbr;
+    int group;
     enum {IDLE = 0, UL, DL, BOTH} direction; 
   } ue_bearer_cfg_t; 
   
@@ -231,7 +232,7 @@ public:
   /* UL information */
   virtual int ul_crc_info(uint32_t tti, uint16_t rnti, bool crc) = 0; 
   virtual int ul_sr_info(uint32_t tti, uint16_t rnti) = 0; 
-  virtual int ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr) = 0; 
+  virtual int ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr, bool set_value = true) = 0;
   virtual int ul_recv_len(uint16_t rnti, uint32_t lcid, uint32_t len) = 0; 
   virtual int ul_phr(uint16_t rnti, int phr) = 0; 
   virtual int ul_cqi_info(uint32_t tti, uint16_t rnti, uint32_t cqi, uint32_t ul_ch_code) = 0; 
diff --git a/lib/src/common/pdu.cc b/lib/src/common/pdu.cc
index c54636fcd..57bc07043 100644
--- a/lib/src/common/pdu.cc
+++ b/lib/src/common/pdu.cc
@@ -33,7 +33,7 @@
 
 // Table 6.1.3.1-1 Buffer size levels for BSR 
 static uint32_t btable[64] = {
-  0, 5, 10, 12, 14, 17, 19, 22, 26, 31, 36, 42, 49, 57, 67, 78, 91, 107, 125, 146, 171, 200, 234, 274, 321, 376, 440, 515, 603, 706, 826, 967, 1132, 
+  0, 1, 10, 12, 14, 17, 19, 22, 26, 31, 36, 42, 49, 57, 67, 78, 91, 107, 125, 146, 171, 200, 234, 274, 321, 376, 440, 515, 603, 706, 826, 967, 1132,
   1326, 1552, 1817, 2127, 2490, 2915, 3413, 3995, 4667, 5476, 6411, 7505, 8787, 10287, 12043, 14099, 16507, 19325, 22624, 26487, 31009, 36304, 
   42502, 49759, 58255, 68201, 79846, 93479, 109439, 128125, 150000};
 
@@ -428,12 +428,16 @@ int sch_subh::get_bsr(uint32_t buff_size[4])
       buff_size[2] = (payload[1]&0x0F) << 4 | (payload[1]&0xC0) >> 6;
       buff_size[3] = (payload[2]&0x3F); 
     } else {
-      uint32_t nonzero_lcg     = (payload[0]&0xc0) >> 6;
+      nonzero_lcg              = (payload[0]&0xc0) >> 6;
       buff_size[nonzero_lcg%4] =  payload[0]&0x3f;
     }
     for (int i=0;i<4;i++) {
       if (buff_size[i]) {
-        buff_size[i] = btable[buff_size[i]%64];
+        if (buff_size[i]<63) {
+          buff_size[i] = btable[1+buff_size[i]];
+        } else {
+          buff_size[i] = btable[63];
+        }
       }
     }
     return nonzero_lcg;
diff --git a/srsenb/drb.conf.example b/srsenb/drb.conf.example
index 4a2eab0d6..0f8e83372 100644
--- a/srsenb/drb.conf.example
+++ b/srsenb/drb.conf.example
@@ -15,14 +15,14 @@ qci_config = (
     };
     dl_um = {
       sn_field_length = 10; 
-      t_reordering    = 80;             
+      t_reordering    = 45;             
     };
   };
   logical_channel_config = {
-    priority = 11; 
+    priority = 13; 
     prioritized_bit_rate   = -1; 
     bucket_size_duration  = 100; 
-    log_chan_group = 3; 
+    log_chan_group = 2; 
   };
 },
 {
diff --git a/srsenb/hdr/mac/scheduler.h b/srsenb/hdr/mac/scheduler.h
index 74ca8cc95..66f8998c1 100644
--- a/srsenb/hdr/mac/scheduler.h
+++ b/srsenb/hdr/mac/scheduler.h
@@ -108,7 +108,7 @@ public:
   
   int ul_crc_info(uint32_t tti, uint16_t rnti, bool crc);
   int ul_sr_info(uint32_t tti, uint16_t rnti); 
-  int ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr); 
+  int ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr, bool set_value = true);
   int ul_recv_len(uint16_t rnti, uint32_t lcid, uint32_t len);
   int ul_phr(uint16_t rnti, int phr); 
   int ul_cqi_info(uint32_t tti, uint16_t rnti, uint32_t cqi, uint32_t ul_ch_code); 
diff --git a/srsenb/hdr/mac/scheduler_harq.h b/srsenb/hdr/mac/scheduler_harq.h
index 9a38306ae..2e09136f9 100644
--- a/srsenb/hdr/mac/scheduler_harq.h
+++ b/srsenb/hdr/mac/scheduler_harq.h
@@ -104,7 +104,8 @@ public:
   void       set_alloc(ul_alloc_t alloc);
   void       same_alloc();
   bool       is_adaptive_retx(); 
-  
+
+  void       reset_pending_data();
   bool       has_pending_ack();
   uint32_t   get_pending_data(); 
   
diff --git a/srsenb/hdr/mac/scheduler_ue.h b/srsenb/hdr/mac/scheduler_ue.h
index fa38b2396..1cb223e57 100644
--- a/srsenb/hdr/mac/scheduler_ue.h
+++ b/srsenb/hdr/mac/scheduler_ue.h
@@ -62,7 +62,7 @@ public:
   void rem_bearer(uint32_t lc_id);
   
   void dl_buffer_state(uint8_t lc_id, uint32_t tx_queue, uint32_t retx_queue);
-  void ul_buffer_state(uint8_t lc_id, uint32_t bsr); 
+  void ul_buffer_state(uint8_t lc_id, uint32_t bsr, bool set_value = true);
   void ul_phr(int phr); 
   void mac_buffer_state(uint32_t ce_code);
   void ul_recv_len(uint32_t lcid, uint32_t len);
@@ -114,8 +114,10 @@ public:
   uint32_t   get_max_retx(); 
   
   bool       get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32_t *L);
-  bool       pucch_sr_collision(uint32_t current_tti, uint32_t n_cce); 
-  
+  bool       pucch_sr_collision(uint32_t current_tti, uint32_t n_cce);
+
+  uint32_t   get_pending_ul_old_data();
+
 private: 
   
   typedef struct {
@@ -126,8 +128,7 @@ private:
   } ue_bearer_t; 
   
   bool       is_sr_triggered();
-  uint32_t   get_pending_ul_old_data();  
-  int        alloc_pdu(int tbs, sched_interface::dl_sched_pdu_t* pdu);  
+  int        alloc_pdu(int tbs, sched_interface::dl_sched_pdu_t* pdu);
 
   static uint32_t format1_count_prb(uint32_t bitmask, uint32_t cell_nof_prb); 
   static int cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t max_mcs, uint32_t *mcs);
diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index ef2df2f22..b98b07f9d 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -85,7 +85,9 @@ public:
   
   uint32_t rl_failure();
   void     rl_failure_reset();
-  
+
+  void set_lcg(uint32_t lcid, uint32_t lcg);
+
   void metrics_read(srsenb::mac_metrics_t* metrics);
   void metrics_rx(bool crc, uint32_t tbs);
   void metrics_tx(bool crc, uint32_t tbs);
@@ -100,7 +102,9 @@ private:
   void allocate_ce(srslte::sch_pdu *pdu, uint32_t lcid);
   
   void metrics_phr(float phr); 
-  uint32_t phr_counter; 
+  uint32_t phr_counter;
+
+  std::vector<uint32_t> lc_groups[4];
   
   mac_metrics_t metrics;
   
diff --git a/srsenb/rr.conf.example b/srsenb/rr.conf.example
index 830dd3bc6..af6dcf2c1 100644
--- a/srsenb/rr.conf.example
+++ b/srsenb/rr.conf.example
@@ -9,7 +9,7 @@ mac_cnfg =
   ulsch_cnfg = 
   {
     max_harq_tx = 4;
-    periodic_bsr_timer = 5; // in ms 
+    periodic_bsr_timer = 40; // in ms 
     retx_bsr_timer = 320;   // in ms
   };
   
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 5711ccce0..fb2c3110e 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -33,6 +33,7 @@
 #include <strings.h>
 #include <pthread.h>
 #include <unistd.h>
+#include <srslte/interfaces/sched_interface.h>
 
 #include "srslte/common/log.h"
 #include "mac/mac.h"
@@ -163,7 +164,9 @@ int mac::rlc_buffer_state(uint16_t rnti, uint32_t lc_id, uint32_t tx_queue, uint
 
 int mac::bearer_ue_cfg(uint16_t rnti, uint32_t lc_id, sched_interface::ue_bearer_cfg_t* cfg)
 {
-  if (ue_db.count(rnti)) {   
+  if (ue_db.count(rnti)) {
+    // configure BSR group in UE
+    ue_db[rnti]->set_lcg(lc_id, (uint32_t) cfg->group);
     return scheduler.bearer_ue_cfg(rnti, lc_id, cfg);      
   } else {
     Error("User rnti=0x%x not found\n", rnti);
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 5a9a8a6e5..12a7ea1cc 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -302,12 +302,12 @@ int sched::ul_cqi_info(uint32_t tti, uint16_t rnti, uint32_t cqi, uint32_t ul_ch
   return ret; 
 }
 
-int sched::ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr)
+int sched::ul_bsr(uint16_t rnti, uint32_t lcid, uint32_t bsr, bool set_value)
 {
   pthread_mutex_lock(&mutex);
   int ret = 0; 
   if (ue_db.count(rnti)) {         
-    ue_db[rnti].ul_buffer_state(lcid, bsr);
+    ue_db[rnti].ul_buffer_state(lcid, bsr, set_value);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     ret = -1;
@@ -792,13 +792,13 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
             user->unset_sr();
           }
 
-          log_h->info("SCHED: %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=%d,%d, n_rtx=%d, tbs=%d, bsr=%d (%d)\n", 
+          log_h->info("SCHED: %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=%d,%d, n_rtx=%d, tbs=%d, bsr=%d (%d-%d)\n",
                       is_rar?"RAR":"UL",
                       is_newtx?"tx":"retx",                
                       rnti, h->get_id(), 
                       sched_result->pusch[nof_dci_elems].dci_location.L, sched_result->pusch[nof_dci_elems].dci_location.ncce,
                       alloc.RB_start, alloc.L, h->nof_retx(), sched_result->pusch[nof_dci_elems].tbs, 
-                      user->get_pending_ul_new_data(current_tti),pending_data_before);
+                      user->get_pending_ul_new_data(current_tti),pending_data_before, user->get_pending_ul_old_data());
 
           nof_dci_elems++;          
         } else {
@@ -812,8 +812,16 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
         }   
       }
     }
-  } 
-  
+  }
+
+  // Update pending data counters after this TTI
+  for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
+    sched_ue *user = (sched_ue *) &iter->second;
+    uint16_t rnti = (uint16_t) iter->first;
+
+    user->get_ul_harq(current_tti)->reset_pending_data();
+  }
+
   sched_result->nof_dci_elems   = nof_dci_elems;
   sched_result->nof_phich_elems = nof_phich_elems;
 
diff --git a/srsenb/src/mac/scheduler_harq.cc b/srsenb/src/mac/scheduler_harq.cc
index 4651b5533..009872153 100644
--- a/srsenb/src/mac/scheduler_harq.cc
+++ b/srsenb/src/mac/scheduler_harq.cc
@@ -98,7 +98,7 @@ bool harq_proc::get_ack()
 void harq_proc::set_ack(bool ack_)
 {
   ack = ack_;   
-  ack_received = true; 
+  ack_received = true;
   log_h->debug("ACK=%d received pid=%d, n_rtx=%d, max_retx=%d\n", ack_, id, n_rtx, max_retx);
   if (n_rtx >= max_retx) {
     Warning("SCHED: discarting TB pid=%d, tti=%d, maximum number of retx exceeded (%d)\n", id, tti, max_retx);
@@ -229,13 +229,22 @@ bool ul_harq_proc::has_pending_ack()
     active = false;     
   }
   if (!active) {
-    pending_data = 0; 
-    need_ack = false; 
+    need_ack = false;
   }
   return ret; 
 }
 
-uint32_t ul_harq_proc::get_pending_data()
+
+
+void ul_harq_proc::reset_pending_data()
+{
+  if (!active) {
+    pending_data = 0;
+  }
+}
+
+
+  uint32_t ul_harq_proc::get_pending_data()
 {
   return pending_data; 
 }
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 178fe6446..5545efb51 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -163,12 +163,18 @@ void sched_ue::phy_config_enabled(uint32_t tti, bool enabled)
   phy_config_dedicated_enabled = enabled; 
 }
 
-void sched_ue::ul_buffer_state(uint8_t lc_id, uint32_t bsr)
+void sched_ue::ul_buffer_state(uint8_t lc_id, uint32_t bsr, bool set_value)
 {
   if (lc_id < sched_interface::MAX_LC) {
-    lch[lc_id].bsr = bsr;
+    if (set_value) {
+      lch[lc_id].bsr = bsr;
+    } else {
+      lch[lc_id].bsr += bsr;
+    }
     Debug("SCHED: UL lcid=%d buffer_state=%d\n", lc_id, bsr);
-  }  
+  }
+  Info("SCHED: bsr=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", bsr, lc_id,
+       lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
 }
 
 void sched_ue::ul_phr(int phr)
@@ -297,6 +303,8 @@ void sched_ue::ul_recv_len(uint32_t lcid, uint32_t len)
       }
     }
   }
+  Info("SCHED: recv_len=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", len, lcid,
+       lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
 }
 
 void sched_ue::set_ul_crc(uint32_t tti, bool crc_res)
@@ -554,6 +562,10 @@ uint32_t sched_ue::get_pending_ul_new_data(uint32_t tti)
   } else {
     pending_data = 0; 
   }
+  if (pending_data) {
+    Info("SCHED: pending_data=%d, pending_ul_data=%d, bsr={%d,%d,%d,%d}\n", pending_data,pending_ul_data,
+         lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
+  }
   return pending_data; 
 }
 
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index 123f66e46..b338a202d 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -58,6 +58,10 @@ void ue::config(uint16_t rnti_, uint32_t nof_prb, sched_interface *sched_, rrc_i
   for(int i=0;i<NOF_HARQ_PROCESSES;i++) {
     pending_buffers[i] = NULL; 
   }
+
+  // Set LCID group for SRB0 and SRB1
+  set_lcg(0, 0);
+  set_lcg(1, 0);
 }  
 
 
@@ -88,7 +92,16 @@ void ue::rl_failure_reset()
 {
   nof_failures = 0;
 }
-  
+
+void ue::set_lcg(uint32_t lcid, uint32_t lcg)
+{
+  // find and remove if already exists
+  for (int i=0;i<4;i++) {
+    lc_groups[lcg].erase(std::remove(lc_groups[lcg].begin(), lc_groups[lcg].end(), lcid), lc_groups[lcg].end());
+  }
+  lc_groups[lcg].push_back(lcid);
+}
+
 srslte_softbuffer_rx_t* ue::get_rx_softbuffer(uint32_t tti)
 {
   return &softbuffer_rx[tti%NOF_HARQ_PROCESSES];
@@ -138,63 +151,83 @@ void ue::process_pdu(uint8_t* pdu, uint32_t nof_bytes, uint32_t tstamp)
   if (pcap) {
     pcap->write_ul_crnti(pdu, nof_bytes, rnti, true, last_tti);
   }
+
+  uint32_t lcid_most_data = 0;
+  int most_data = -99;
   
   while(mac_msg_ul.next()) {
     assert(mac_msg_ul.get());
-    if (mac_msg_ul.get()->is_sdu()) 
-    {
+    if (mac_msg_ul.get()->is_sdu()) {
       // Route logical channel 
-      log_h->debug_hex(mac_msg_ul.get()->get_sdu_ptr(), mac_msg_ul.get()->get_payload_size(), 
-                      "PDU:   rnti=0x%x, lcid=%d, %d bytes\n", 
-                      rnti, mac_msg_ul.get()->get_sdu_lcid(), mac_msg_ul.get()->get_payload_size());
-      
-      
+      log_h->debug_hex(mac_msg_ul.get()->get_sdu_ptr(), mac_msg_ul.get()->get_payload_size(),
+                       "PDU:   rnti=0x%x, lcid=%d, %d bytes\n",
+                       rnti, mac_msg_ul.get()->get_sdu_lcid(), mac_msg_ul.get()->get_payload_size());
+
+
       /* In some cases, an uplink transmission with only CQI has all zeros and gets routed to RRC 
        * Compute the checksum if lcid=0 and avoid routing in that case 
        */
-      bool route_pdu = true; 
+      bool route_pdu = true;
       if (mac_msg_ul.get()->get_sdu_lcid() == 0) {
         uint8_t *x = mac_msg_ul.get()->get_sdu_ptr();
-        uint32_t sum = 0; 
-        for (uint32_t i=0;i<mac_msg_ul.get()->get_payload_size();i++) {
+        uint32_t sum = 0;
+        for (uint32_t i = 0; i < mac_msg_ul.get()->get_payload_size(); i++) {
           sum += x[i];
         }
         if (sum == 0) {
-          route_pdu = false; 
+          route_pdu = false;
           Warning("Received all zero PDU\n");
         }
       }
-      
+
       if (route_pdu) {
-        rlc->write_pdu(rnti, 
-                     mac_msg_ul.get()->get_sdu_lcid(), 
-                     mac_msg_ul.get()->get_sdu_ptr(), 
-                     mac_msg_ul.get()->get_payload_size());          
+        rlc->write_pdu(rnti,
+                       mac_msg_ul.get()->get_sdu_lcid(),
+                       mac_msg_ul.get()->get_sdu_ptr(),
+                       mac_msg_ul.get()->get_payload_size());
       }
-      
+
       // Indicate scheduler to update BSR counters 
       sched->ul_recv_len(rnti, mac_msg_ul.get()->get_sdu_lcid(), mac_msg_ul.get()->get_payload_size());
-      
+
+      if ((int) mac_msg_ul.get()->get_payload_size() > most_data) {
+        most_data = (int) mac_msg_ul.get()->get_payload_size();
+        lcid_most_data = mac_msg_ul.get()->get_sdu_lcid();
+      }
+
       // Save contention resolution if lcid == 0
       if (mac_msg_ul.get()->get_sdu_lcid() == 0 && route_pdu) {
-        uint32_t nbytes = srslte::sch_subh::MAC_CE_CONTRES_LEN;          
+        uint32_t nbytes = srslte::sch_subh::MAC_CE_CONTRES_LEN;
         if (mac_msg_ul.get()->get_payload_size() >= nbytes) {
-          uint8_t *ue_cri_ptr  = (uint8_t*) &conres_id;
-          uint8_t *pkt_ptr     = mac_msg_ul.get()->get_sdu_ptr(); // Warning here: we want to include the 
-          for (uint32_t i=0;i<nbytes;i++) {
-            ue_cri_ptr[nbytes-i-1] = pkt_ptr[i];
+          uint8_t *ue_cri_ptr = (uint8_t *) &conres_id;
+          uint8_t *pkt_ptr = mac_msg_ul.get()->get_sdu_ptr(); // Warning here: we want to include the
+          for (uint32_t i = 0; i < nbytes; i++) {
+            ue_cri_ptr[nbytes - i - 1] = pkt_ptr[i];
           }
         } else {
           Error("Received CCCH UL message of invalid size=%d bytes\n", mac_msg_ul.get()->get_payload_size());
         }
-      }      
-    } else {
-      // Process MAC Control Element
-      if (!process_ce(mac_msg_ul.get())) {
-        Warning("Received Subheader with invalid or unkonwn LCID\n");
       }
     }
-  }      
+  }
+  mac_msg_ul.reset();
+
+  /* Process CE after all SDUs because we need to update BSR after */
+  bool bsr_received = false;
+  while(mac_msg_ul.next()) {
+    assert(mac_msg_ul.get());
+    if (!mac_msg_ul.get()->is_sdu()) {
+      // Process MAC Control Element
+      bsr_received |= process_ce(mac_msg_ul.get());
+    }
+  }
+
+  // If BSR is not received means that new data has arrived and there is no space for BSR transmission
+  if (!bsr_received && lcid_most_data > 2) {
+    // Add BSR to the LCID for which most data was received
+    sched->ul_bsr(rnti, lcid_most_data, 256, false); // false adds BSR instead of setting
+    Info("BSR not received. Giving extra grant\n");
+  }
 
   Debug("MAC PDU processed\n");
   
@@ -222,9 +255,10 @@ void ue::push_pdu(uint32_t tti, uint32_t len)
 
 bool ue::process_ce(srslte::sch_subh *subh) {
   uint32_t buff_size[4] = {0, 0, 0, 0};
-  uint32_t idx = 0; 
-  float phr = 0; 
-  uint16_t old_rnti = 0; 
+  float phr = 0;
+  int idx = 0;
+  uint16_t old_rnti = 0;
+  bool is_bsr = false;
   switch(subh->ce_type()) {
     case srslte::sch_subh::PHR_REPORT: 
       phr = subh->get_phr(); 
@@ -243,23 +277,26 @@ bool ue::process_ce(srslte::sch_subh *subh) {
       }
       break;
     case srslte::sch_subh::TRUNC_BSR: 
-    case srslte::sch_subh::SHORT_BSR: 
-    case srslte::sch_subh::LONG_BSR: 
+    case srslte::sch_subh::SHORT_BSR:
       idx = subh->get_bsr(buff_size);
-      if (idx > 0) {
-        // Indicate BSR to scheduler 
-        sched->ul_bsr(rnti, idx, 2*buff_size[idx]);
-        Info("CE:    Received BSR rnti=0x%x, lcid=%d, value=%d\n", rnti, idx, buff_size[idx]);        
-      } else if (idx == 0) {
-        // TODO: map lcid group to lcid
-        for (int i=0;i<4;i++) {
-          sched->ul_bsr(rnti, i, 2*buff_size[i]);
+      for (uint32_t i=0;i<lc_groups[idx].size();i++) {
+        // Indicate BSR to scheduler
+        sched->ul_bsr(rnti, lc_groups[idx][i], buff_size[idx]);
+        Info("CE:    Received %s BSR rnti=0x%x, lcg=%d, lcid=%d, value=%d\n",
+             subh->ce_type()==srslte::sch_subh::SHORT_BSR?"Short":"Trunc", rnti, idx, lc_groups[idx][i], buff_size[idx]);
+      }
+      is_bsr = true;
+      break;
+    case srslte::sch_subh::LONG_BSR:
+      subh->get_bsr(buff_size);
+      for (int idx=0;idx<4;idx++) {
+        for (uint32_t i=0;i<lc_groups[idx].size();i++) {
+          sched->ul_bsr(rnti, lc_groups[idx][i], buff_size[idx]);
         }
-        Info("CE:    Received Long BSR rnti=0x%x, value=%d,%d,%d,%d\n", rnti, 
-             buff_size[0], buff_size[1], buff_size[2], buff_size[3]);        
-      } else {
-        printf("Error!\n");
       }
+      is_bsr = true;
+      Info("CE:    Received Long BSR rnti=0x%x, value=%d,%d,%d,%d\n", rnti,
+           buff_size[0], buff_size[1], buff_size[2], buff_size[3]);
       break;
     case srslte::sch_subh::PADDING: 
       Debug("CE:    Received padding for rnti=0x%x\n", rnti);
@@ -268,7 +305,7 @@ bool ue::process_ce(srslte::sch_subh *subh) {
       Error("CE:    Invalid lcid=0x%x\n", subh->ce_type());
       break;
   }
-  return true; 
+  return is_bsr;
 }
 
 
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 7c06ebab7..de1c61b9e 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -24,6 +24,8 @@
  *
  */
 
+#include <srslte/interfaces/sched_interface.h>
+#include <srslte/asn1/liblte_rrc.h>
 #include "srslte/asn1/liblte_mme.h"
 #include "upper/rrc.h"
 
@@ -1311,7 +1313,9 @@ void rrc::ue::send_connection_reconf(srslte::byte_buffer_t *pdu)
   // Add SRB2 and DRB1 to the scheduler
   srsenb::sched_interface::ue_bearer_cfg_t bearer_cfg;
   bearer_cfg.direction = srsenb::sched_interface::ue_bearer_cfg_t::BOTH;
+  bearer_cfg.group = 0;
   parent->mac->bearer_ue_cfg(rnti, 2, &bearer_cfg);
+  bearer_cfg.group = conn_reconf->rr_cnfg_ded.drb_to_add_mod_list[0].lc_cnfg.ul_specific_params.log_chan_group;
   parent->mac->bearer_ue_cfg(rnti, 3, &bearer_cfg);
   
   // Configure SRB2 in RLC and PDCP
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 8c9ba7d3d..532bb9849 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -26,7 +26,7 @@ ul_freq = 2565000000
 tx_gain = 80
 rx_gain = 60
 
-#nof_rx_ant = 1
+nof_rx_ant = 2
 #device_name = auto
 #device_args = auto
 #time_adv_nsamples = auto

From e22a526350c8c095ba0f8d0b55d4e37c76cb6708 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 28 Aug 2017 13:05:27 +0200
Subject: [PATCH 090/269] Correction RF module for supporting Rx diversity in
 the UE

---
 lib/include/srslte/phy/rf/rf.h | 15 +++++++++------
 lib/src/phy/rf/rf_blade_imp.c  |  2 +-
 lib/src/phy/rf/rf_blade_imp.h  |  2 +-
 lib/src/phy/rf/rf_dev.h        |  2 +-
 lib/src/phy/rf/rf_imp.c        | 32 ++++++++++++++++++--------------
 lib/src/phy/rf/rf_uhd_imp.c    | 12 ++++++------
 lib/src/phy/rf/rf_uhd_imp.h    |  3 +--
 lib/src/radio/radio.cc         |  9 ++++++---
 8 files changed, 43 insertions(+), 34 deletions(-)

diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index 42b2a691d..50ced76d9 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -99,12 +99,6 @@ SRSLTE_API int srslte_rf_open_devname_multi(srslte_rf_t *h,
                                             char *args, 
                                             uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_rf_open_devname_multi2(srslte_rf_t *rf,
-                                             char *devname,
-                                             char *args,
-                                             uint32_t nof_tx_antennas,
-                                             uint32_t nof_rx_antennas);
-
 SRSLTE_API const char *srslte_rf_name(srslte_rf_t *h); 
 
 SRSLTE_API int srslte_rf_start_gain_thread(srslte_rf_t *rf, 
@@ -221,6 +215,15 @@ SRSLTE_API int srslte_rf_send_timed2(srslte_rf_t *h,
                               bool is_start_of_burst,
                               bool is_end_of_burst);
 
+SRSLTE_API int srslte_rf_send_timed_multi(srslte_rf_t *rf,
+                                          void *data[4],
+                                          int nsamples,
+                                          time_t secs,
+                                          double frac_secs,
+                                          bool blocking,
+                                          bool is_start_of_burst,
+                                          bool is_end_of_burst);
+
 SRSLTE_API int srslte_rf_send_multi(srslte_rf_t *rf,
                                     void *data[4],
                                     int nsamples,
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index c48a2ad5b..d8996925f 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -178,7 +178,7 @@ float rf_blade_get_rssi(void *h)
   return 0;
 }
 
-int rf_blade_open_multi(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
+int rf_blade_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   return rf_blade_open(args, h); 
 }
diff --git a/lib/src/phy/rf/rf_blade_imp.h b/lib/src/phy/rf/rf_blade_imp.h
index 2876a5404..fbaeab1df 100644
--- a/lib/src/phy/rf/rf_blade_imp.h
+++ b/lib/src/phy/rf/rf_blade_imp.h
@@ -34,7 +34,7 @@ SRSLTE_API int rf_blade_open(char *args,
                         void **handler);
 
 SRSLTE_API int rf_blade_open_multi(char *args, 
-                        void **handler, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas);
+                        void **handler, uint32_t nof_channels);
 
 SRSLTE_API char* rf_blade_devname(void *h);
 
diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index 4e3f2681b..00f157b6b 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -38,7 +38,7 @@ typedef struct {
   void   (*srslte_rf_suppress_stdout)(void *h);
   void   (*srslte_rf_register_error_handler)(void *h, srslte_rf_error_handler_t error_handler);
   int    (*srslte_rf_open)(char *args, void **h);
-  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas);
+  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_channels);
   int    (*srslte_rf_close)(void *h);
   void   (*srslte_rf_set_master_clock_rate)(void *h, double rate);
   bool   (*srslte_rf_is_master_clock_dynamic)(void *h);
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 0ae771043..cb22b301b 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -102,11 +102,7 @@ int srslte_rf_open_devname(srslte_rf_t *rf, char *devname, char *args) {
   return srslte_rf_open_devname_multi(rf, devname, args, 1);
 }
 
-int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_rx_antennas) {
-  return srslte_rf_open_devname_multi2(rf, devname, args, 1, nof_rx_antennas);
-}
-
-int srslte_rf_open_devname_multi2(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas) {
+int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_channels) {
   /* Try to open the device if name is provided */
   if (devname) {
     if (devname[0] != '\0') {
@@ -114,7 +110,7 @@ int srslte_rf_open_devname_multi2(srslte_rf_t *rf, char *devname, char *args, ui
       while(available_devices[i] != NULL) {
         if (!strcmp(available_devices[i]->name, devname)) {
           rf->dev = available_devices[i];
-          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_tx_antennas, nof_rx_antennas);
+          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_channels);
         }
         i++;
       }    
@@ -125,7 +121,7 @@ int srslte_rf_open_devname_multi2(srslte_rf_t *rf, char *devname, char *args, ui
   /* If in auto mode or provided device not found, try to open in order of apperance in available_devices[] array */
   int i=0;
   while(available_devices[i] != NULL) {
-    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_tx_antennas, nof_rx_antennas)) {
+    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_channels)) {
       rf->dev = available_devices[i];
       return 0; 
     }
@@ -198,11 +194,6 @@ int srslte_rf_open_multi(srslte_rf_t *h, char *args, uint32_t nof_rx_antennas)
   return srslte_rf_open_devname_multi(h, NULL, args, nof_rx_antennas);
 }
 
-int srslte_rf_open_multi2(srslte_rf_t *h, char *args, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
-{
-  return srslte_rf_open_devname_multi2(h, NULL, args, nof_tx_antennas, nof_rx_antennas);
-}
-
 int srslte_rf_close(srslte_rf_t *rf)
 {
   return ((rf_dev_t*) rf->dev)->srslte_rf_close(rf->handler);  
@@ -310,6 +301,20 @@ int srslte_rf_send_timed3(srslte_rf_t *rf,
                                  has_time_spec, blocking, is_start_of_burst, is_end_of_burst);  
 }
 
+int srslte_rf_send_timed_multi(srslte_rf_t *rf,
+                               void *data[4],
+                               int nsamples,
+                               time_t secs,
+                               double frac_secs,
+                               bool blocking,
+                               bool is_start_of_burst,
+                               bool is_end_of_burst)
+{
+
+  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, secs, frac_secs,
+                                                           true, blocking, is_start_of_burst, is_end_of_burst);
+}
+
 int srslte_rf_send_multi(srslte_rf_t *rf,
                      void *data[4],
                      int nsamples,
@@ -318,8 +323,7 @@ int srslte_rf_send_multi(srslte_rf_t *rf,
                      bool is_end_of_burst)
 {
 
-  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, 0, 0,
-                                 false, blocking, is_start_of_burst, is_end_of_burst);
+  return srslte_rf_send_timed_multi(rf, data, nsamples, 0, 0, blocking, is_start_of_burst, is_end_of_burst);
 }
 
 int srslte_rf_send(srslte_rf_t *rf, void *data, uint32_t nsamples, bool blocking)
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index e0f3ac573..8819b706c 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -286,10 +286,10 @@ float rf_uhd_get_rssi(void *h) {
 
 int rf_uhd_open(char *args, void **h)
 {
-  return rf_uhd_open_multi(args, h, 1, 1);
+  return rf_uhd_open_multi(args, h, 1);
 }
 
-int rf_uhd_open_multi(char *args, void **h, uint32_t nof_tx_antennas, uint32_t nof_rx_antennas)
+int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   if (h) {
     *h = NULL; 
@@ -395,11 +395,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_tx_antennas, uint32_t n
           .otw_format = "sc16",
           .args = "",
           .channel_list = channel,
-          .n_channels = (nof_tx_antennas > nof_rx_antennas)?nof_tx_antennas:nof_rx_antennas,
+          .n_channels = nof_channels,
       };
       
-    handler->nof_rx_channels = nof_rx_antennas; 
-    handler->nof_tx_channels = nof_tx_antennas;
+    handler->nof_rx_channels = nof_channels;
+    handler->nof_tx_channels = nof_channels;
     /* Initialize rx and tx stremers */
     uhd_rx_streamer_make(&handler->rx_stream);
     error = uhd_usrp_get_rx_stream(handler->usrp, &stream_args, handler->rx_stream);
@@ -664,7 +664,7 @@ int rf_uhd_send_timed_multi(void *h,
   rf_uhd_handler_t* handler = (rf_uhd_handler_t*) h;
   
   /* Resets the USRP time FIXME: this might cause problems for burst transmissions */
-  if (is_start_of_burst && handler->nof_tx_channels > 1) {
+  if (!has_time_spec && is_start_of_burst && handler->nof_tx_channels > 1) {
     uhd_usrp_set_time_now(handler->usrp, 0, 0, 0);
     uhd_tx_metadata_set_time_spec(&handler->tx_md, 0, 0.1);
   }
diff --git a/lib/src/phy/rf/rf_uhd_imp.h b/lib/src/phy/rf/rf_uhd_imp.h
index 6c0ab7dc7..2be799333 100644
--- a/lib/src/phy/rf/rf_uhd_imp.h
+++ b/lib/src/phy/rf/rf_uhd_imp.h
@@ -39,8 +39,7 @@ SRSLTE_API int rf_uhd_open(char *args,
 
 SRSLTE_API int rf_uhd_open_multi(char *args, 
                                  void **handler, 
-                                 uint32_t nof_tx_antennas,
-                                 uint32_t nof_rx_antennas);
+                                 uint32_t nof_channels);
 
 SRSLTE_API char* rf_uhd_devname(void *h);
 
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index bc74660b0..f508dc046 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -169,6 +169,8 @@ bool radio::has_rssi()
 
 bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
 {
+  void *iq_samples[4] = {(void *) zeros, (void *) zeros, (void *) zeros, (void *) zeros};
+
   if (!tx_adv_negative) {
     srslte_timestamp_sub(&tx_time, 0, tx_adv_sec);
   } else {
@@ -177,11 +179,11 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   
   if (is_start_of_burst) {
     if (burst_preamble_samples != 0) {
-      srslte_timestamp_t tx_time_pad; 
+      srslte_timestamp_t tx_time_pad;
       srslte_timestamp_copy(&tx_time_pad, &tx_time);
       srslte_timestamp_sub(&tx_time_pad, 0, burst_preamble_time_rounded); 
       save_trace(1, &tx_time_pad);
-      srslte_rf_send_timed2(&rf_device, zeros, burst_preamble_samples, tx_time_pad.full_secs, tx_time_pad.frac_secs, true, false);
+      srslte_rf_send_timed_multi(&rf_device, iq_samples, burst_preamble_samples, tx_time_pad.full_secs, tx_time_pad.frac_secs, true, true, false);
       is_start_of_burst = false; 
     }        
   }
@@ -191,7 +193,8 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   srslte_timestamp_add(&end_of_burst_time, 0, (double) nof_samples/cur_tx_srate); 
   
   save_trace(0, &tx_time);
-  int ret = srslte_rf_send_timed2(&rf_device, buffer, nof_samples+offset, tx_time.full_secs, tx_time.frac_secs, is_start_of_burst, false);
+  iq_samples[0] = buffer;
+  int ret = srslte_rf_send_timed_multi(&rf_device, (void**) iq_samples, nof_samples+offset, tx_time.full_secs, tx_time.frac_secs, true, is_start_of_burst, false);
   offset = 0; 
   is_start_of_burst = false; 
   if (ret > 0) {

From 8c71c716c94ad3c72ab00f853994efd7c584c04d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 28 Aug 2017 13:06:54 +0200
Subject: [PATCH 091/269] PDSCH decoder acknowledges TBs/CWs per sepparate and
 softbuffers are arrays of pointers

---
 lib/examples/cell_measurement.c            |  5 +-
 lib/examples/pdsch_enodeb.c                | 13 ++++--
 lib/examples/pdsch_ue.c                    |  7 +--
 lib/include/srslte/phy/common/phy_common.h | 13 +++---
 lib/include/srslte/phy/phch/pdsch.h        |  7 +--
 lib/include/srslte/phy/ue/ue_dl.h          |  7 +--
 lib/src/phy/phch/pdsch.c                   | 30 ++++++------
 lib/src/phy/phch/test/pdsch_test.c         | 54 ++++++++++++++++------
 lib/src/phy/ue/ue_dl.c                     | 48 +++++++++++--------
 9 files changed, 117 insertions(+), 67 deletions(-)

diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index 90b963db8..c6135984c 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -160,7 +160,8 @@ int main(int argc, char **argv) {
   int sfn_offset; 
   float rssi_utra=0,rssi=0, rsrp=0, rsrq=0, snr=0;
   cf_t *ce[SRSLTE_MAX_PORTS];
-  float cfo = 0; 
+  float cfo = 0;
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
 
   if (parse_args(&prog_args, argc, argv)) {
     exit(-1);
@@ -313,7 +314,7 @@ int main(int argc, char **argv) {
         case DECODE_SIB:
           /* We are looking for SI Blocks, search only in appropiate places */
           if ((srslte_ue_sync_get_sfidx(&ue_sync) == 5 && (sfn%2)==0)) {
-            n = srslte_ue_dl_decode_multi(&ue_dl, sf_buffer, data, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync));
+            n = srslte_ue_dl_decode_multi(&ue_dl, sf_buffer, data, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
             if (n < 0) {
               fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
               return -1;
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 1bc2d5230..a4558ff30 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -84,7 +84,7 @@ srslte_pcfich_t pcfich;
 srslte_pdcch_t pdcch;
 srslte_pdsch_t pdsch;
 srslte_pdsch_cfg_t pdsch_cfg; 
-srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS];
+srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
 srslte_regs_t regs;
 srslte_ra_dl_dci_t ra_dl;
 int rvidx[SRSLTE_MAX_CODEWORDS] = {0, 0};
@@ -267,7 +267,7 @@ void base_init() {
   } else {
 #ifndef DISABLE_RF
     printf("Opening RF device...\n");
-    if (srslte_rf_open_multi2(&rf, rf_args, cell.nof_ports, 1)) {
+    if (srslte_rf_open_multi(&rf, rf_args, cell.nof_ports)) {
       fprintf(stderr, "Error opening rf\n");
       exit(-1);
     }
@@ -334,7 +334,7 @@ void base_init() {
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
 
   for (i = 0; i < nof_tb; i++) {
-    if (srslte_softbuffer_tx_init(&softbuffers[i], cell.nof_prb)) {
+    if (srslte_softbuffer_tx_init(softbuffers[i], cell.nof_prb)) {
       fprintf(stderr, "Error initiating soft buffer\n");
       exit(-1);
     }
@@ -344,7 +344,10 @@ void base_init() {
 void base_free() {
   int i;
   for (i = 0; i < nof_tb; i++) {
-    srslte_softbuffer_tx_free(&softbuffers[i]);
+    if (softbuffers[i]) {
+      free(softbuffers[i]);
+    }
+    srslte_softbuffer_tx_free(softbuffers[i]);
   }
   srslte_pdsch_free(&pdsch);
   srslte_pdcch_free(&pdcch);
@@ -649,7 +652,7 @@ int main(int argc, char **argv) {
   
   bool send_data = false; 
   for (i = 0; i < nof_tb; i++) {
-    srslte_softbuffer_tx_reset(&softbuffers[i]);
+    srslte_softbuffer_tx_reset(softbuffers[i]);
   }
 
 #ifndef DISABLE_RF
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 60cc0a4db..4ce537bdd 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -540,7 +540,8 @@ int main(int argc, char **argv) {
   INFO("\nEntering main loop...\n\n", 0);
   /* Main loop */
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
-    
+    bool acks [SRSLTE_MAX_CODEWORDS] = {false};
+
     ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
     if (ret < 0) {
       fprintf(stderr, "Error calling srslte_ue_sync_work()\n");
@@ -585,7 +586,7 @@ int main(int argc, char **argv) {
             n = srslte_ue_dl_decode_multi(&ue_dl, 
                                           sf_buffer, 
                                           data, 
-                                          sfn*10+srslte_ue_sync_get_sfidx(&ue_sync));
+                                          sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
           
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
@@ -623,7 +624,7 @@ int main(int argc, char **argv) {
             rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05f);
             noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05f);
             enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f, enodebrate, 0.05f);
-            uerate = SRSLTE_VEC_EMA((n>0)?(ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f:0.0f, uerate, 0.01f);
+            uerate = SRSLTE_VEC_EMA(((acks[0]?ue_dl.pdsch_cfg.grant.mcs[0].tbs:0) + (acks[1]?ue_dl.pdsch_cfg.grant.mcs[1].tbs:0))/1000.0f, uerate, 0.01f);
 
             nframes++;
             if (isnan(rsrq)) {
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index ad9be3e0f..821908fcb 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -52,6 +52,7 @@
 #define SRSLTE_MAX_PORTS     4
 #define SRSLTE_MAX_LAYERS    4
 #define SRSLTE_MAX_CODEWORDS 2
+#define SRSLTE_MAX_TB        SRSLTE_MAX_CODEWORDS
 
 #define SRSLTE_MAX_CODEBLOCKS 32
 
@@ -150,12 +151,12 @@ typedef enum SRSLTE_API {
 } srslte_phich_resources_t;
 
 typedef enum {
-  SRSLTE_RNTI_USER = 0,
-  SRSLTE_RNTI_SI,
-  SRSLTE_RNTI_RAR,
-  SRSLTE_RNTI_TEMP,
-  SRSLTE_RNTI_SPS,
-  SRSLTE_RNTI_PCH,  
+  SRSLTE_RNTI_USER = 0, /* Cell RNTI */
+  SRSLTE_RNTI_SI,       /* System Information RNTI */
+  SRSLTE_RNTI_RAR,      /* Random Access RNTI */
+  SRSLTE_RNTI_TEMP,     /* Temporary C-RNTI */
+  SRSLTE_RNTI_SPS,      /* Semi-Persistent Scheduling C-RNTI */
+  SRSLTE_RNTI_PCH,      /* Paging RNTI */
   SRSLTE_RNTI_NOF_TYPES
 } srslte_rnti_type_t;
 
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 99710b965..7cfa1b616 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -124,7 +124,7 @@ SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
 
 SRSLTE_API int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
                                          srslte_pdsch_cfg_t *cfg,
-                                         srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                         srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                                          uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                          uint16_t rnti,
                                          cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
@@ -140,12 +140,13 @@ SRSLTE_API int srslte_pdsch_decode(srslte_pdsch_t *q,
 
 SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
                                          srslte_pdsch_cfg_t *cfg, 
-                                         srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                                         srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                                          cf_t *sf_symbols[SRSLTE_MAX_PORTS], 
                                          cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                          float noise_estimate, 
                                          uint16_t rnti,
-                                         uint8_t *data[SRSLTE_MAX_CODEWORDS]);
+                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                         bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                        srslte_pdsch_cfg_t *cfg,
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 4101fadd9..97b2344f5 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -84,7 +84,7 @@ typedef struct SRSLTE_API {
   srslte_cfo_t sfo_correct; 
   
   srslte_pdsch_cfg_t pdsch_cfg; 
-  srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS];
+  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
   srslte_ra_dl_dci_t dl_dci;
   srslte_cell_t cell;
 
@@ -180,7 +180,7 @@ SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t * q,
 SRSLTE_API int srslte_ue_dl_decode_multi(srslte_ue_dl_t * q, 
                                          cf_t *input[SRSLTE_MAX_PORTS], 
                                          uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                                         uint32_t tti);
+                                         uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q, 
                                         cf_t *input, 
@@ -192,7 +192,8 @@ SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q,
                                               cf_t *input[SRSLTE_MAX_PORTS], 
                                               uint8_t *data[SRSLTE_MAX_CODEWORDS],
                                               uint32_t tti,
-                                              uint16_t rnti);
+                                              uint16_t rnti,
+                                              bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           uint32_t *ri,
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 35e10fedb..0e824aaa7 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -553,30 +553,33 @@ void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 int srslte_pdsch_decode(srslte_pdsch_t *q,
                         srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
                         cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate,
-                        uint16_t rnti, uint8_t *data)
-{
+                        uint16_t rnti, uint8_t *data) {
   cf_t *_sf_symbols[SRSLTE_MAX_PORTS];
   cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  bool acks[SRSLTE_MAX_CODEWORDS];
+  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB] = {NULL};
 
   _sf_symbols[0] = sf_symbols;
-  for (int i=0;i<q->cell.nof_ports;i++) {
+  for (int i = 0; i < q->cell.nof_ports; i++) {
     _ce[i][0] = ce[i];
   }
-  return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, &data);
+
+  softbuffers[0] = softbuffer;
+
+  return srslte_pdsch_decode_multi(q, cfg, softbuffers, _sf_symbols, _ce, noise_estimate, rnti, &data, acks);
 }
 
 /** Decodes the PDSCH from the received symbols
  */
 int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
-                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
-                              uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS])
-{
+                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                              float noise_estimate, uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                              bool acks[SRSLTE_MAX_CODEWORDS]) {
 
   /* Set pointers for layermapping & precoding */
   uint32_t i;
   cf_t *x[SRSLTE_MAX_LAYERS];
-  int ret = 0;
 
   if (q            != NULL &&
       sf_symbols   != NULL &&
@@ -658,7 +661,8 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     }
 
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-        ret |= srslte_pdsch_codeword_decode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
+      int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+      acks[tb] = (ret == SRSLTE_SUCCESS);
     }
 
     if (SRSLTE_VERBOSE_ISDEBUG()) {
@@ -666,7 +670,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       srslte_vec_save_file("llr.dat", q->e, cfg->nbits[0].nof_bits*sizeof(int16_t));
     }
 
-    return ret;
+    return SRSLTE_SUCCESS;
 
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
@@ -776,7 +780,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
 }
 
 int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                               uint8_t *data[SRSLTE_MAX_CODEWORDS], uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) {
 
   int i;
@@ -806,7 +810,7 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
     }
 
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      ret |= srslte_pdsch_codeword_encode(q, cfg, &softbuffers[tb], rnti, data[tb], tb);
+      ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
     }
 
     if (q->cell.nof_ports > 1) {
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 777481d6a..afc53d326 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -135,7 +135,7 @@ void parse_args(int argc, char **argv) {
 
 uint8_t *data[SRSLTE_MAX_CODEWORDS] = {NULL};
 cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-srslte_softbuffer_rx_t softbuffers_rx[SRSLTE_MAX_CODEWORDS];
+srslte_softbuffer_rx_t *softbuffers_rx[SRSLTE_MAX_CODEWORDS];
 srslte_ra_dl_grant_t grant; 
 srslte_pdsch_cfg_t pdsch_cfg; 
 #ifdef DO_OFDM
@@ -151,8 +151,9 @@ int main(int argc, char **argv) {
   uint32_t i, j, k;
   int ret = -1;
   struct timeval t[3];
-  srslte_softbuffer_tx_t softbuffers_tx[SRSLTE_MAX_CODEWORDS];
+  srslte_softbuffer_tx_t *softbuffers_tx[SRSLTE_MAX_CODEWORDS];
   int M=10;
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
 
   parse_args(argc,argv);
 
@@ -163,8 +164,6 @@ int main(int argc, char **argv) {
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  bzero(softbuffers_tx, sizeof(srslte_softbuffer_tx_t)*SRSLTE_MAX_CODEWORDS);
-  bzero(softbuffers_rx, sizeof(srslte_softbuffer_rx_t)*SRSLTE_MAX_CODEWORDS);
 
   /* Parse transmission mode */
   if (srslte_str2mimotype(mimo_type_str, &mimo_type)) {
@@ -275,8 +274,14 @@ int main(int argc, char **argv) {
 
   srslte_pdsch_set_rnti(&pdsch_rx, rnti);
 
-  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-    if (srslte_softbuffer_rx_init(&softbuffers_rx[i], cell.nof_prb)) {
+  for (i = 0; i < SRSLTE_MAX_TB; i++) {
+    softbuffers_rx[i] = calloc(sizeof(srslte_softbuffer_rx_t), 1);
+    if (!softbuffers_rx[i]) {
+      fprintf(stderr, "Error allocating RX soft buffer\n");
+      goto quit;
+    }
+
+    if (srslte_softbuffer_rx_init(softbuffers_rx[i], cell.nof_prb)) {
       fprintf(stderr, "Error initiating RX soft buffer\n");
       goto quit;
     }
@@ -339,7 +344,13 @@ int main(int argc, char **argv) {
     srslte_pdsch_set_rnti(&pdsch_tx, rnti);
 
     for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-      if (srslte_softbuffer_tx_init(&softbuffers_tx[i], cell.nof_prb)) {
+      softbuffers_tx[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
+
+      if (!softbuffers_tx[i]) {
+        fprintf(stderr, "Error allocating TX soft buffer\n");
+      }
+
+      if (srslte_softbuffer_tx_init(softbuffers_tx[i], cell.nof_prb)) {
         fprintf(stderr, "Error initiating TX soft buffer\n");
         goto quit;
       }
@@ -427,29 +438,44 @@ int main(int argc, char **argv) {
 #endif
     for (i = 0; i < grant.nof_tb; i++) {
       if (grant.mcs[i].tbs) {
-        srslte_softbuffer_rx_reset_tbs(&softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
+        srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
-    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data);
+    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data, acks);
   }
   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[0].tbs + grant.mcs[1].tbs)/1000.0f,
          (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
+
+  /* If there is an error in PDSCH decode */
   if (r) {
-    ret = -1;
+    ERROR("PDSCH decode");
+    ret = SRSLTE_ERROR;
     goto quit;
-  } 
+  }
+
+  /* Check all transport blocks have been decoded OK */
+  for (int tb = 0; tb < grant.nof_tb; tb++) {
+    ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+  }
 
-  ret = 0;
+  ret = SRSLTE_SUCCESS;
 
 quit:
   srslte_pdsch_free(&pdsch_tx);
   srslte_pdsch_free(&pdsch_rx);
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-    srslte_softbuffer_tx_free(&softbuffers_tx[i]);
-    srslte_softbuffer_rx_free(&softbuffers_rx[i]);
+    srslte_softbuffer_tx_free(softbuffers_tx[i]);
+    if (softbuffers_tx[i]) {
+      free(softbuffers_tx[i]);
+    }
+
+    srslte_softbuffer_rx_free(softbuffers_rx[i]);
+    if (softbuffers_rx[i]) {
+      free(softbuffers_rx[i]);
+    }
 
     if (data[i]) {
       free(data[i]);
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index c9a5dea1a..7e40876ec 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -102,8 +102,14 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
       goto clean_exit;
     }
 
-    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-      if (srslte_softbuffer_rx_init(&q->softbuffers[i], q->cell.nof_prb)) {
+    for (int i = 0; i < SRSLTE_MAX_TB; i++) {
+      q->softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_rx_t));
+      if (!q->softbuffers[i]) {
+        fprintf(stderr, "Error allocating soft buffer\n");
+        goto clean_exit;
+      }
+
+      if (srslte_softbuffer_rx_init(q->softbuffers[i], q->cell.nof_prb)) {
         fprintf(stderr, "Error initiating soft buffer\n");
         goto clean_exit;
       }
@@ -157,8 +163,11 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     srslte_cfo_free(&q->sfo_correct);
-    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-      srslte_softbuffer_rx_free(&q->softbuffers[i]);
+    for (int i = 0; i < SRSLTE_MAX_TB; i++) {
+      srslte_softbuffer_rx_free(q->softbuffers[i]);
+      if (q->softbuffers[i]) {
+        free(q->softbuffers[i]);
+      }
     }
     for (int j=0;j<q->nof_rx_antennas;j++) {
       if (q->sf_symbols_m[j]) {
@@ -194,7 +203,7 @@ void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
 
 void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
   for(int i = 0; i < SRSLTE_MAX_CODEWORDS; i++){
-    srslte_softbuffer_rx_reset(&q->softbuffers[i]);
+    srslte_softbuffer_rx_reset(q->softbuffers[i]);
   }
   bzero(&q->pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
 }
@@ -212,14 +221,16 @@ void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
  */
 int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti) {
   uint8_t *_data[SRSLTE_MAX_CODEWORDS];
-  cf_t *_input[SRSLTE_MAX_PORTS]; 
+  cf_t *_input[SRSLTE_MAX_PORTS];
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
   _data[0] = data;
   _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, q->current_rnti);
+  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, q->current_rnti, acks);
 }
 
-int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti) {
-  return srslte_ue_dl_decode_rnti_multi(q, input, data, tti, q->current_rnti);
+int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                              uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]) {
+  return srslte_ue_dl_decode_rnti_multi(q, input, data, tti, q->current_rnti, acks);
 }
 
 int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input, uint32_t sf_idx, uint32_t *cfi) 
@@ -307,16 +318,18 @@ int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint3
   return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
 }
 
-int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti)
-{
+int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti) {
   uint8_t *_data[SRSLTE_MAX_CODEWORDS];
   cf_t *_input[SRSLTE_MAX_PORTS];
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
   _input[0] = input; 
   _data[0] = data;
-  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, rnti);
+  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, rnti, acks);
 }
 
-int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti, uint16_t rnti)
+int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti, uint16_t rnti,
+                                   bool acks[SRSLTE_MAX_CODEWORDS])
 {
   srslte_mimo_type_t mimo_type;
   srslte_dci_msg_t dci_msg;
@@ -324,8 +337,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   srslte_ra_dl_grant_t grant; 
   int ret = SRSLTE_ERROR; 
   uint32_t cfi;
-  
-  uint32_t sf_idx = tti%10; 
+  uint32_t sf_idx = tti%10;
   
   if ((ret = srslte_ue_dl_decode_fft_estimate_multi(q, input, sf_idx, &cfi)) < 0) {
     return ret; 
@@ -357,7 +369,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
       uint32_t k   = (sfn/2)%4;
       for (int i = 0; i < grant.nof_tb; i++) {
         rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
-        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     } else {
       for (int i = 0; i < grant.nof_tb; i++) {
@@ -372,7 +384,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
             ERROR("Wrong number of transport blocks");
             return SRSLTE_ERROR;
         }
-        srslte_softbuffer_rx_reset_tbs(&q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
 
@@ -421,7 +433,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
       ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
                                     q->sf_symbols_m, q->ce_m, 
                                     noise_estimate, 
-                                    rnti, data);
+                                    rnti, data, acks);
       
       if (ret == SRSLTE_ERROR) {
         q->pkt_errors++;

From be0c6ebd203196930357b9e13733a6347dcb2a51 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 28 Aug 2017 13:07:44 +0200
Subject: [PATCH 092/269] Added multiple codeword architecture to MAC layer

---
 lib/include/srslte/interfaces/ue_interfaces.h |  22 +-
 srsue/hdr/mac/dl_harq.h                       | 379 ++++++++++--------
 srsue/hdr/mac/mac.h                           |   2 +-
 srsue/hdr/mac/ul_harq.h                       |  26 +-
 srsue/hdr/phy/phch_worker.h                   |  14 +-
 srsue/src/mac/mac.cc                          |  15 +-
 srsue/src/mac/proc_ra.cc                      |  12 +-
 srsue/src/phy/phch_worker.cc                  | 131 +++---
 srsue/test/phy/ue_itf_test_prach.cc           |  76 ++--
 srsue/test/phy/ue_itf_test_sib1.cc            |  29 +-
 10 files changed, 392 insertions(+), 314 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 81ed67b74..879118021 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -277,10 +277,10 @@ public:
     uint32_t    pid;    
     uint32_t    tti;
     uint32_t    last_tti;
-    bool        ndi; 
-    bool        last_ndi; 
-    uint32_t    n_bytes;
-    int         rv; 
+    bool        ndi[SRSLTE_MAX_CODEWORDS];
+    bool        last_ndi[SRSLTE_MAX_CODEWORDS];
+    uint32_t    n_bytes[SRSLTE_MAX_CODEWORDS];
+    int         rv[SRSLTE_MAX_CODEWORDS];
     uint16_t    rnti; 
     bool        is_from_rar;
     bool        is_sps_release;
@@ -291,28 +291,28 @@ public:
   
   typedef struct {
     bool                    decode_enabled;
-    int                     rv;
+    int                     rv[SRSLTE_MAX_TB];
     uint16_t                rnti; 
     bool                    generate_ack; 
     bool                    default_ack; 
     // If non-null, called after tb_decoded_ok to determine if ack needs to be sent
     bool                  (*generate_ack_callback)(void*); 
     void                   *generate_ack_callback_arg;
-    uint8_t                *payload_ptr; 
-    srslte_softbuffer_rx_t *softbuffer;
+    uint8_t                *payload_ptr[SRSLTE_MAX_TB];
+    srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB];
     srslte_phy_grant_t      phy_grant;
   } tb_action_dl_t;
 
   typedef struct {
     bool                    tx_enabled;
     bool                    expect_ack;
-    uint32_t                rv;
+    uint32_t                rv[SRSLTE_MAX_TB];
     uint16_t                rnti; 
     uint32_t                current_tx_nb;
     int32_t                 tti_offset;     // relative offset between grant and UL tx/HARQ rx
-    srslte_softbuffer_tx_t *softbuffer;
+    srslte_softbuffer_tx_t *softbuffers;
     srslte_phy_grant_t      phy_grant;
-    uint8_t                *payload_ptr; 
+    uint8_t                *payload_ptr[SRSLTE_MAX_TB];
   } tb_action_ul_t;
   
   /* Indicate reception of UL grant. 
@@ -329,7 +329,7 @@ public:
   virtual void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) = 0;
   
   /* Indicate successfull decoding of PDSCH TB. */
-  virtual void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid) = 0;
+  virtual void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) = 0;
   
   /* Indicate successfull decoding of BCH TB through PBCH */
   virtual void bch_decoded_ok(uint8_t *payload, uint32_t len) = 0;  
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 809ecd151..4f6df9b51 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -84,20 +84,20 @@ public:
         harq_pid = grant.pid%N;
       }
       if (grant.rnti_type == SRSLTE_RNTI_TEMP && last_temporal_crnti != grant.rnti) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
         Info("Set NDI=1 for Temp-RNTI DL grant\n");
         last_temporal_crnti = grant.rnti;
       }
       if (grant.rnti_type == SRSLTE_RNTI_USER && proc[harq_pid].is_sps()) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
         Info("Set NDI=1 for C-RNTI DL grant\n");
       }
       proc[harq_pid].new_grant_dl(grant, action);
     } else {
       /* This is for SPS scheduling */
       uint32_t harq_pid = get_harq_sps_pid(grant.tti)%N;
-      if (grant.ndi) {
-        grant.ndi = false; 
+      if (grant.ndi[0]) {
+        grant.ndi[0] = false;
         proc[harq_pid].new_grant_dl(grant, action);
       } else {
         if (grant.is_sps_release) {
@@ -117,12 +117,12 @@ public:
   }
 
 
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
   {
     if (rnti_type == SRSLTE_RNTI_SI) {
-      proc[N].tb_decoded(ack);
+      proc[N].tb_decoded(ack, 0);
     } else {
-      proc[harq_pid%N].tb_decoded(ack);
+      proc[harq_pid%N].tb_decoded(ack, tb_idx);
     }
   }
 
@@ -137,200 +137,239 @@ public:
 
   void start_pcap(srslte::mac_pcap* pcap_) { pcap = pcap_; }
 
-  int  get_current_tbs(uint32_t harq_pid) { return proc[harq_pid%N].get_current_tbs(); }
+  int  get_current_tbs(uint32_t harq_pid, uint32_t tb_idx) { return proc[harq_pid%N].get_current_tbs(tb_idx); }
 
   void set_si_window_start(int si_window_start_) { si_window_start = si_window_start_; }
 
   float get_average_retx() { return average_retx; }
 
-private:  
+private:
   class dl_harq_process {
   public:
-    dl_harq_process()
-    {
-      is_initiated = false; 
-      ack = false; 
-      bzero(&cur_grant, sizeof(Tgrant));
+    dl_harq_process() : subproc(SRSLTE_MAX_TB) {
+
     }
 
-    bool init(uint32_t pid_, dl_harq_entity *parent)
-    {
-      if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
-        Error("Error initiating soft buffer\n");
-        return false; 
-      } else {
-        pid = pid_;
-        is_initiated = true; 
-        harq_entity = parent; 
-        log_h = harq_entity->log_h; 
-        return true;
+    bool init(uint32_t pid_, dl_harq_entity *parent) {
+      bool ret = true;
+
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        ret &= subproc[tb].init(pid_, parent, tb);
       }
+      return ret;
     }
 
-    void reset()
-    {
-      ack = false; 
-      payload_buffer_ptr = NULL; 
-      bzero(&cur_grant, sizeof(Tgrant));
-      if (is_initiated) {
-        srslte_softbuffer_rx_reset(&softbuffer);
+    void reset(void) {
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        subproc[tb].reset();
       }
     }
-    
-    void new_grant_dl(Tgrant grant, Taction *action)
-    {
-      // Compute RV for BCCH when not specified in PDCCH format
-      if (pid == HARQ_BCCH_PID && grant.rv == -1) {
-        uint32_t k; 
-        if ((grant.tti/10)%2 == 0 && grant.tti%10 == 5) { // This is SIB1, k is different
-          k = (grant.tti/20)%4; 
-          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-        } else if (grant.rv == -1) {      
-          k = (grant.tti-harq_entity->si_window_start)%4; 
-          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-        }
-      }
-      calc_is_new_transmission(grant);
-      if (is_new_transmission) {
-        ack = false; 
-        srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
-        n_retx = 0; 
+
+    void new_grant_dl(Tgrant grant, Taction *action) {
+      for (uint32_t tb = 0; tb < grant.phy_grant.dl.nof_tb; tb++) {
+        subproc[tb].new_grant_dl(grant, action);
       }
-      
-      // Save grant 
-      grant.last_ndi = cur_grant.ndi; 
-      grant.last_tti = cur_grant.tti; 
-      memcpy(&cur_grant, &grant, sizeof(Tgrant));
-      
-      // Fill action structure 
-      bzero(action, sizeof(Taction));
-      action->default_ack = ack; 
-      action->generate_ack = true; 
-      action->decode_enabled = false; 
-      
-      // If data has not yet been successfully decoded
-      if (ack == false) {
-        
-        // Instruct the PHY To combine the received data and attempt to decode it
-        payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid, cur_grant.n_bytes);
-        action->payload_ptr = payload_buffer_ptr;
-        if (!action->payload_ptr) {
-          action->decode_enabled = false; 
-          Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes);
-          return;       
-        }    
-        action->decode_enabled = true;     
-        action->rv = cur_grant.rv; 
-        action->rnti = cur_grant.rnti; 
-        action->softbuffer = &softbuffer;     
-        memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
-        n_retx++; 
-        
-      } else {
-        Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+    }
+
+    int get_current_tbs(uint32_t tb_idx) { return subproc[tb_idx].get_current_tbs(); }
+
+    bool is_sps() { return false; }
+
+    void tb_decoded(bool ack_, uint32_t tb_idx) {
+      subproc[tb_idx].tb_decoded(ack_);
+    }
+
+  private:
+    class dl_tb_process {
+    public:
+      dl_tb_process(void) {
+        is_initiated = false;
+        ack = false;
+        bzero(&cur_grant, sizeof(Tgrant));
       }
-        
-      if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
-        // Do not generate ACK
-        Debug("Not generating ACK\n");
-        action->generate_ack = false;    
-      } else {
-        if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && ack == false) {
-          // Postpone ACK after contention resolution is resolved
-          action->generate_ack_callback = harq_entity->generate_ack_callback; 
-          action->generate_ack_callback_arg = harq_entity->demux_unit;
-          Debug("ACK pending contention resolution\n");
+
+      bool init(uint32_t pid_, dl_harq_entity *parent, uint32_t tb_idx) {
+        tid = tb_idx;
+        if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
+          Error("Error initiating soft buffer\n");
+          return false;
         } else {
-          Debug("Generating ACK\n");
+          pid = pid_;
+          is_initiated = true;
+          harq_entity = parent;
+          log_h = harq_entity->log_h;
+          return true;
         }
       }
-    }
-    
-    void tb_decoded(bool ack_)
-    {
-      ack = ack_;
-      if (ack == true) {
-        if (pid == HARQ_BCCH_PID) {
-          if (harq_entity->pcap) {
-            harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes, ack, cur_grant.tti);
+
+      void reset(void) {
+        ack = false;
+        payload_buffer_ptr = NULL;
+        bzero(&cur_grant, sizeof(Tgrant));
+        if (is_initiated) {
+          srslte_softbuffer_rx_reset(&softbuffer);
+        }
+      }
+
+      void new_grant_dl(Tgrant grant, Taction *action) {
+        // Compute RV for BCCH when not specified in PDCCH format
+        if (pid == HARQ_BCCH_PID && grant.rv[tid] == -1) {
+          uint32_t k;
+          if ((grant.tti / 10) % 2 == 0 && grant.tti % 10 == 5) { // This is SIB1, k is different
+            k = (grant.tti / 20) % 4;
+            grant.rv[tid] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+          } else if (grant.rv[tid] == -1) {
+            k = (grant.tti - harq_entity->si_window_start) % 4;
+            grant.rv[tid] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
           }
-          Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes);
-          harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-        } else {      
-          if (harq_entity->pcap) {
-            harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes, cur_grant.rnti, ack, cur_grant.tti);            
+        }
+        calc_is_new_transmission(grant);
+        if (is_new_transmission) {
+          ack = false;
+          srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes[tid] * 8);
+          n_retx = 0;
+        }
+
+        // Save grant
+        grant.last_ndi[tid] = cur_grant.ndi[tid];
+        grant.last_tti = cur_grant.tti;
+        memcpy(&cur_grant, &grant, sizeof(Tgrant));
+
+        // Fill action structure
+        bzero(action, sizeof(Taction));
+        action->default_ack = ack;
+        action->generate_ack = true;
+        action->decode_enabled = false;
+
+        // If data has not yet been successfully decoded
+        if (!ack) {
+
+          // Instruct the PHY To combine the received data and attempt to decode it
+          payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid * SRSLTE_MAX_TB + tid,
+                                                                       cur_grant.n_bytes[tid]);
+          action->payload_ptr[tid] = payload_buffer_ptr;
+          if (!action->payload_ptr) {
+            action->decode_enabled = false;
+            Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes[tid]);
+            return;
           }
-          if (ack) {
-            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n", cur_grant.n_bytes);
-              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes);
-            } else {
-              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes);
-              harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-
-              // Compute average number of retransmissions per packet 
-              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx, harq_entity->nof_pkts++); 
-            }
+          action->decode_enabled = true;
+          action->rv[tid] = cur_grant.rv[tid];
+          action->rnti = cur_grant.rnti;
+          action->softbuffers[tid] = &softbuffer;
+          memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
+          n_retx++;
+
+        } else {
+          Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+        }
+
+        if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
+          // Do not generate ACK
+          Debug("Not generating ACK\n");
+          action->generate_ack = false;
+        } else {
+          if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && !ack) {
+            // Postpone ACK after contention resolution is resolved
+            action->generate_ack_callback = harq_entity->generate_ack_callback;
+            action->generate_ack_callback_arg = harq_entity->demux_unit;
+            Debug("ACK pending contention resolution\n");
+          } else {
+            Debug("Generating ACK\n");
           }
         }
-      } else {
-        harq_entity->demux_unit->deallocate(payload_buffer_ptr);
-      }
-      
-      Info("DL %d:  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n", 
-           pid, is_new_transmission?"newTX":"reTX ", 
-           cur_grant.n_bytes, cur_grant.rv, ack?"OK":"KO", 
-           cur_grant.ndi, cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
-      
-      if (ack && pid == HARQ_BCCH_PID) {
-        reset();
       }
-    }
 
-    bool is_sps() { return false; }
 
-    int get_current_tbs() { return cur_grant.n_bytes*8; }
-    
-  private: 
-    bool calc_is_new_transmission(Tgrant grant)
-    {
-      bool is_new_tb = true; 
-      if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes == cur_grant.n_bytes)) ||
-          pid == HARQ_BCCH_PID) 
-      {
-        is_new_tb = false; 
+      void tb_decoded(bool ack_) {
+        ack = ack_;
+        if (ack) {
+          if (pid == HARQ_BCCH_PID) {
+            if (harq_entity->pcap) {
+              harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes[tid], ack, cur_grant.tti);
+            }
+            Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes[tid]);
+            harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                              cur_grant.tti);
+          } else {
+            if (harq_entity->pcap) {
+              harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid], cur_grant.rnti, ack,
+                                                cur_grant.tti);
+            }
+            if (ack) {
+              if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
+                      cur_grant.n_bytes[tid]);
+                harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
+              } else {
+                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
+                harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                                  cur_grant.tti);
+
+                // Compute average number of retransmissions per packet
+                harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
+                                                           harq_entity->nof_pkts++);
+              }
+            }
+          }
+        } else {
+          harq_entity->demux_unit->deallocate(payload_buffer_ptr);
+        }
+
+        Info("DL %d (TB %d):  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n",
+             pid, tid, is_new_transmission ? "newTX" : "reTX ",
+             cur_grant.n_bytes[tid], cur_grant.rv[tid], ack ? "OK" : "KO",
+             cur_grant.ndi[tid], cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
+
+        if (ack && pid == HARQ_BCCH_PID) {
+          reset();
+        }
       }
-      
-      if ((grant.ndi != cur_grant.ndi && !is_new_tb) || // NDI toggled for same TB
-          is_new_tb                                  || // is new TB
-          (pid == HARQ_BCCH_PID && grant.rv == 0))      // Broadcast PID and 1st TX (RV=0)
-      {
-        is_new_transmission = true; 
-        Debug("Set HARQ for new transmission\n");
-      } else {
-        is_new_transmission = false; 
-        Debug("Set HARQ for retransmission\n");
+
+      int get_current_tbs(void) { return cur_grant.n_bytes[tid] * 8; }
+
+    private:
+      bool calc_is_new_transmission(Tgrant grant) {
+        bool is_new_tb = true;
+        if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes[tid] == cur_grant.n_bytes[tid])) ||
+            pid == HARQ_BCCH_PID) {
+          is_new_tb = false;
+        }
+
+        if ((grant.ndi[tid] != cur_grant.ndi[tid] && !is_new_tb) || // NDI toggled for same TB
+            is_new_tb || // is new TB
+            (pid == HARQ_BCCH_PID && grant.rv[tid] == 0))      // Broadcast PID and 1st TX (RV=0)
+        {
+          is_new_transmission = true;
+          Debug("Set HARQ for new transmission\n");
+        } else {
+          is_new_transmission = false;
+          Debug("Set HARQ for retransmission\n");
+        }
+
+        return is_new_transmission;
       }
 
-      return is_new_transmission;
-    }
+      bool is_initiated;
+      dl_harq_entity *harq_entity;
+      srslte::log *log_h;
 
-    bool            is_initiated; 
-    dl_harq_entity *harq_entity; 
-    srslte::log    *log_h;
-    
-    bool            is_new_transmission; 
-    
-    uint32_t        pid;    
-    uint8_t        *payload_buffer_ptr; 
-    bool            ack;
-    
-    uint32_t        n_retx; 
-    
-    Tgrant                         cur_grant;
-    srslte_softbuffer_rx_t         softbuffer; 
+      bool is_new_transmission;
+
+      uint32_t pid;     /* HARQ Proccess ID   */
+      uint32_t tid;     /* Transport block ID */
+      uint8_t *payload_buffer_ptr;
+      bool ack;
+
+      uint32_t n_retx;
+
+      Tgrant cur_grant;
+      srslte_softbuffer_rx_t softbuffer;
+    };
+
+    /* Transport blocks */
+    std::vector<dl_tb_process> subproc;
   };
-  
   // Private members of dl_harq_entity
   
   static bool      generate_ack_callback(void *arg)
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index 660f7fbba..f2785e288 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -65,7 +65,7 @@ public:
   void new_grant_ul_ack(mac_grant_t grant, bool ack, tb_action_ul_t *action);
   void harq_recv(uint32_t tti, bool ack, tb_action_ul_t *action);
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action);
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
   void bch_decoded_ok(uint8_t *payload, uint32_t len);
   void pch_decoded_ok(uint32_t len);    
   void tti_clock(uint32_t tti);
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 2ef1ed2b6..13ec0b9b3 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -113,12 +113,12 @@ public:
       grant.rnti_type == SRSLTE_RNTI_RAR)
     {
       if (grant.rnti_type == SRSLTE_RNTI_USER && proc[pidof(grant.tti)].is_sps()) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
       }
       run_tti(grant.tti, &grant, action);
     } else if (grant.rnti_type == SRSLTE_RNTI_SPS) {
-      if (grant.ndi) {
-        grant.ndi = proc[pidof(grant.tti)].get_ndi();
+      if (grant.ndi[0]) {
+        grant.ndi[0] = proc[pidof(grant.tti)].get_ndi();
         run_tti(grant.tti, &grant, action);
       } else {
         Info("Not implemented\n");
@@ -208,7 +208,7 @@ private:
 
       // Receive and route HARQ feedbacks
       if (grant) {
-        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi != get_ndi()) ||
+        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi[0] != get_ndi()) ||
           (grant->rnti_type == SRSLTE_RNTI_USER && !has_grant())             ||
           grant->is_from_rar)
         {
@@ -216,8 +216,8 @@ private:
 
           // Uplink grant in a RAR
           if (grant->is_from_rar) {
-            Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes);
-            pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes);
+            Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes[0]);
+            pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes[0]);
             if (pdu_ptr) {
               generate_new_tx(tti_tx, true, grant, action);
             } else {
@@ -227,7 +227,7 @@ private:
             // Normal UL grant
           } else {
             // Request a MAC PDU from the Multiplexing & Assemble Unit
-            pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes, tti_tx, pid);
+            pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes[0], tti_tx, pid);
             if (pdu_ptr) {
               generate_new_tx(tti_tx, false, grant, action);
             } else {
@@ -258,7 +258,7 @@ private:
         if (grant->is_from_rar) {
           grant->rnti = harq_entity->rntis->temp_rnti;
         }
-        harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes, grant->rnti, get_nof_retx(), tti_tx);
+        harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes[0], grant->rnti, get_nof_retx(), tti_tx);
       }
     }
 
@@ -285,7 +285,7 @@ private:
     bool is_sps() { return false; }
     uint32_t last_tx_tti() { return tti_last_tx; }
     uint32_t get_nof_retx() { return current_tx_nb; }
-    int get_current_tbs() { return cur_grant.n_bytes*8; }
+    int get_current_tbs() { return cur_grant.n_bytes[0]*8; }
    
   private: 
     Tgrant                      cur_grant;
@@ -321,7 +321,7 @@ private:
       if (grant) {
         // HARQ entity requests an adaptive transmission
         if (grant->rv) {
-          current_irv = irv_of_rv[grant->rv%4];
+          current_irv = irv_of_rv[grant->rv[0]%4];
         }
         memcpy(&cur_grant, grant, sizeof(Tgrant));
         harq_feedback = false;
@@ -370,10 +370,10 @@ private:
       current_tx_nb++;
       action->expect_ack = true;
       action->rnti = is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti;
-      action->rv = cur_grant.rv>0?cur_grant.rv:get_rv();
-      action->softbuffer = &softbuffer;
+      action->rv[0] = cur_grant.rv[0]>0?cur_grant.rv[0]:get_rv();
+      action->softbuffers = &softbuffer;
       action->tx_enabled = true;
-      action->payload_ptr = pdu_ptr;
+      action->payload_ptr[0] = pdu_ptr;
       memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
 
       current_irv = (current_irv+1)%4;
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index ed1d7af26..a47008629 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -78,18 +78,20 @@ private:
   bool decode_pdcch_dl(mac_interface_phy::mac_grant_t *grant);
   bool decode_phich(bool *ack);
 
-  bool decode_pdsch(srslte_ra_dl_grant_t *grant,
+  int decode_pdsch(srslte_ra_dl_grant_t *grant,
                     uint8_t *payload, srslte_softbuffer_rx_t *softbuffer,
                     int rv,
                     uint16_t rnti,
-                    uint32_t pid);
+                    uint32_t pid,
+                    bool acks[SRSLTE_MAX_CODEWORDS]);
 
-  bool decode_pdsch_multi(srslte_ra_dl_grant_t *grant,
+  int decode_pdsch_multi(srslte_ra_dl_grant_t *grant,
                           uint8_t *payload[SRSLTE_MAX_CODEWORDS],
-                          srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+                          srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                           int rv[SRSLTE_MAX_CODEWORDS],
                           uint16_t rnti,
-                          uint32_t pid);
+                          uint32_t pid,
+                          bool acks[SRSLTE_MAX_CODEWORDS]);
 
   /* ... for UL */
   void encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, srslte_softbuffer_tx_t *softbuffer, 
@@ -100,7 +102,7 @@ private:
   void set_uci_sr();
   void set_uci_periodic_cqi();
   void set_uci_aperiodic_cqi();
-  void set_uci_ack(bool ack);
+  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb);
   bool srs_is_ready_to_send();
   float set_power(float tx_power);
   void setup_tx_gain();
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index e1426780e..1bc335bb1 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -44,7 +44,7 @@ namespace srsue {
 mac::mac() : ttisync(10240), 
              timers_db((uint32_t) NOF_MAC_TIMERS),
              mux_unit(MAC_NOF_HARQ_PROC),
-             demux_unit(MAC_NOF_HARQ_PROC),
+             demux_unit(SRSLTE_MAX_TB*MAC_NOF_HARQ_PROC),
              pdu_process_thread(&demux_unit)
 {
   started = false;  
@@ -255,17 +255,17 @@ void mac::pch_decoded_ok(uint32_t len)
   }
 }
 
-void mac::tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
+void mac::tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
 {
   if (rnti_type == SRSLTE_RNTI_RAR) {
     if (ack) {
       ra_procedure.tb_decoded_ok();
     }
   } else {
-    dl_harq.tb_decoded(ack, rnti_type, harq_pid);
+    dl_harq.tb_decoded(ack, tb_idx, rnti_type, harq_pid);
     if (ack) {
       pdu_process_thread.notify();
-      metrics.rx_brate += dl_harq.get_current_tbs(harq_pid);
+      metrics.rx_brate += dl_harq.get_current_tbs(harq_pid, tb_idx);
     } else {
       metrics.rx_errors++;
     }
@@ -282,12 +282,9 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     action->generate_ack = false; 
     action->decode_enabled = true; 
-    srslte_softbuffer_rx_reset_cb(&pch_softbuffer, 1);
-    action->payload_ptr = pch_payload_buffer;
-    action->softbuffer  = &pch_softbuffer;
     action->rnti = grant.rnti;
-    action->rv   = grant.rv; 
-    if (grant.n_bytes > pch_payload_buffer_sz) {
+    action->rv[0]   = grant.rv[0];
+    if (grant.n_bytes[0] > pch_payload_buffer_sz) {
       Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes, pch_payload_buffer_sz);
       action->decode_enabled = false; 
     }
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 73b51d61c..213042018 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -269,19 +269,19 @@ void ra_proc::step_pdcch_setup() {
 
 void ra_proc::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t* action) 
 {
-  if (grant.n_bytes < MAX_RAR_PDU_LEN) {
+  if (grant.n_bytes[0] < MAX_RAR_PDU_LEN) {
     rDebug("DL grant found RA-RNTI=%d\n", ra_rnti);        
     action->decode_enabled = true; 
     action->default_ack = false; 
     action->generate_ack = false; 
-    action->payload_ptr = rar_pdu_buffer; 
+    action->payload_ptr[0] = rar_pdu_buffer;
     action->rnti = grant.rnti; 
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
-    action->rv = grant.rv;
-    action->softbuffer = &softbuffer_rar;
-    rar_grant_nbytes = grant.n_bytes;
+    action->rv[0] = grant.rv[0];
+    action->softbuffers[0] = &softbuffer_rar;
+    rar_grant_nbytes = grant.n_bytes[0];
     rar_grant_tti    = grant.tti; 
-    if (action->rv == 0) {
+    if (action->rv[0] == 0) {
       srslte_softbuffer_rx_reset(&softbuffer_rar);
     }
   } else {
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 6dabbbf01..78c5b2f71 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -178,8 +178,8 @@ void phch_worker::work_imp()
   reset_uci();
 
   bool dl_grant_available = false; 
-  bool ul_grant_available = false; 
-  bool dl_ack = false;
+  bool ul_grant_available = false;
+  bool dl_ack[SRSLTE_MAX_CODEWORDS] = {false};
 
   mac_interface_phy::mac_grant_t    dl_mac_grant;
   mac_interface_phy::tb_action_dl_t dl_action; 
@@ -200,22 +200,28 @@ void phch_worker::work_imp()
     if(dl_grant_available) {
       /* Send grant to MAC and get action for this TB */
       phy->mac->new_grant_dl(dl_mac_grant, &dl_action);
-      
+
+      /* Set DL ACKs to default */
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+        dl_ack[tb] = dl_action.default_ack;
+      }
+
       /* Decode PDSCH if instructed to do so */
-      dl_ack = dl_action.default_ack; 
       if (dl_action.decode_enabled) {
-        dl_ack = decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr,
-                              dl_action.softbuffer, dl_action.rv, dl_action.rnti, 
-                              dl_mac_grant.pid);
+        decode_pdsch_multi(&dl_action.phy_grant.dl, dl_action.payload_ptr,
+                              dl_action.softbuffers, dl_action.rv, dl_action.rnti,
+                              dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-        phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
-        dl_ack = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-        Debug("Calling generate ACK callback returned=%d\n", dl_ack);
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+          dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
+          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack);
+        }
       }
-      Debug("dl_ack=%d, generate_ack=%d\n", dl_ack, dl_action.generate_ack);
+      Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);
       if (dl_action.generate_ack) {
-        set_uci_ack(dl_ack);
+        set_uci_ack(dl_ack, dl_mac_grant.phy_grant.dl.nof_tb);
       }
     }
   }
@@ -258,8 +264,8 @@ void phch_worker::work_imp()
   /* Transmit PUSCH, PUCCH or SRS */
   bool signal_ready = false; 
   if (ul_action.tx_enabled) {
-    encode_pusch(&ul_action.phy_grant.ul, ul_action.payload_ptr, ul_action.current_tx_nb, 
-                 ul_action.softbuffer, ul_action.rv, ul_action.rnti, ul_mac_grant.is_from_rar);          
+    encode_pusch(&ul_action.phy_grant.ul, ul_action.payload_ptr[0], ul_action.current_tx_nb,
+                 &ul_action.softbuffers[0], ul_action.rv[0], ul_action.rnti, ul_mac_grant.is_from_rar);
     signal_ready = true; 
     if (ul_action.expect_ack) {
       phy->set_pending_ack(tti + 8, ue_ul.pusch_cfg.grant.n_prb_tilde[0], ul_action.phy_grant.ul.ncs_dmrs);
@@ -279,9 +285,11 @@ void phch_worker::work_imp()
   
   if (dl_action.decode_enabled && !dl_action.generate_ack_callback) {
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
-      phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes);
+      phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
-      phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      for (uint32_t tb = 0; tb < dl_mac_grant.phy_grant.dl.nof_tb; tb++) {
+        phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      }
     }
   }
 
@@ -380,13 +388,16 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     }
     
     /* Fill MAC grant structure */
-    grant->ndi = dci_unpacked.ndi;
+    grant->ndi[0] = dci_unpacked.ndi;
+    grant->ndi[1] = dci_unpacked.ndi_1;
     grant->pid = dci_unpacked.harq_process;
-    grant->n_bytes = grant->phy_grant.dl.mcs[0].tbs/8;
-    grant->tti = tti; 
-    grant->rv  = dci_unpacked.rv_idx;
-    grant->rnti = dl_rnti; 
-    grant->rnti_type = type; 
+    grant->n_bytes[0] = grant->phy_grant.dl.mcs[0].tbs / (uint32_t) 8;
+    grant->n_bytes[1] = grant->phy_grant.dl.mcs[1].tbs / (uint32_t) 8;
+    grant->tti = tti;
+    grant->rv[0] = dci_unpacked.rv_idx;
+    grant->rv[1] = dci_unpacked.rv_idx_1;
+    grant->rnti = dl_rnti;
+    grant->rnti_type = type;
     grant->last_tti = 0;
     
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
@@ -405,23 +416,27 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
   }
 }
 
-bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
+int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
                                srslte_softbuffer_rx_t *softbuffer, int rv,
-                               uint16_t rnti, uint32_t harq_pid) {
-  int _rv [SRSLTE_MAX_CODEWORDS] = {1};
+                               uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
+  int _rv [SRSLTE_MAX_TB] = {1};
+  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB] = {NULL};
+
   _rv[0] = rv;
+  softbuffers[0] = softbuffer;
 
-  return decode_pdsch_multi(grant, &payload, softbuffer, _rv, rnti, harq_pid);
+  return decode_pdsch_multi(grant, &payload, softbuffers, _rv, rnti, harq_pid, acks);
 }
 
-bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
-                                     srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_CODEWORDS],
+int phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                                     srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                                      int rv[SRSLTE_MAX_CODEWORDS],
-                                     uint16_t rnti, uint32_t harq_pid) {
+                                     uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
   char timestr[64];
   bool valid_config = true;
   timestr[0]='\0';
   srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+  int ret = SRSLTE_SUCCESS;
 
   for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
     if (rv[tb] < 0 || rv[tb] > 3) {
@@ -497,38 +512,39 @@ bool phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *paylo
         struct timeval t[3];
         gettimeofday(&t[1], NULL);
   #endif
-        
-        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
-                                      ue_dl.ce_m, noise_estimate, rnti, payload) == 0;
+        ret = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
+                                        ue_dl.ce_m, noise_estimate, rnti, payload, acks);
+        if (ret) {
+          Error("Decoding PDSCH");
+        }
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
         get_time_interval(t);
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
-        
-        Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n", 
-              grant->nof_prb, harq_pid, 
-              grant->mcs[0].tbs/8, grant->mcs[0].idx, rv,
-              ack?"OK":"KO", 
-              10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), 
-              srslte_pdsch_last_noi(&ue_dl.pdsch),
-              timestr);
+
+        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
+             grant->nof_prb, harq_pid,
+             grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
+             rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
+             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+             srslte_pdsch_last_noi(&ue_dl.pdsch),
+             timestr);
 
         //printf("tti=%d, cfo=%f\n", tti, cfo*15000);
         //srslte_vec_save_file("pdsch", signal_buffer, sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
         
         // Store metrics
         dl_metrics.mcs    = grant->mcs[0].idx;
-        
-        return ack; 
       } else {
         Warning("Received grant for TBS=0\n");
       }
     } else {
-      Error("Error configuring DL grant\n"); 
+      Error("Error configuring DL grant\n");
+      ret = SRSLTE_ERROR;
     }
   }
-  return true; 
+  return ret;
 }
 
 bool phch_worker::decode_phich(bool *ack)
@@ -625,12 +641,12 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
   }
   
   if (ret) {    
-    grant->ndi = dci_unpacked.ndi;
+    grant->ndi[0] = dci_unpacked.ndi;
     grant->pid = 0; // This is computed by MAC from TTI 
-    grant->n_bytes = grant->phy_grant.ul.mcs.tbs/8;
+    grant->n_bytes[0] = grant->phy_grant.ul.mcs.tbs / (uint32_t) 8;
     grant->tti = tti; 
     grant->rnti = ul_rnti; 
-    grant->rv = dci_unpacked.rv_idx;
+    grant->rv[0] = dci_unpacked.rv_idx;
     if (SRSLTE_VERBOSE_ISINFO()) {
       srslte_ra_pusch_fprint(stdout, &dci_unpacked, cell.nof_prb);
     }
@@ -646,11 +662,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-void phch_worker::set_uci_ack(bool ack)
-{
-  uci_data.uci_ack = ack?1:0;
-  uci_data.uci_ack_len = 1; 
-}
+  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
+    if (nof_tb > 0) {
+      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    }
+
+    if (nof_tb > 1) {
+      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    }
+
+    if (nof_tb > 2) {
+      Error("Number of transport blocks is not supported");
+    }
+
+    uci_data.uci_ack_len = nof_tb;
+
+  }
 
 void phch_worker::set_uci_sr()
 {
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index 30d3e9f3d..c2d78470d 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -158,8 +158,8 @@ int rar_unpack(uint8_t *buffer, rar_msg_t *msg)
 srsue::phy my_phy;
 bool bch_decoded = false; 
 
-uint8_t payload[10240]; 
-uint8_t payload_bits[10240]; 
+uint8_t payload[SRSLTE_MAX_TB][10240];
+uint8_t payload_bits[SRSLTE_MAX_TB][10240];
 const uint8_t conn_request_msg[] = {0x20, 0x06, 0x1F, 0x5C, 0x2C, 0x04, 0xB2, 0xAC, 0xF6, 0x00, 0x00, 0x00};
 
 enum mac_state {RA, RAR, CONNREQUEST, CONNSETUP} state = RA; 
@@ -168,7 +168,7 @@ uint32_t preamble_idx = 0;
 rar_msg_t rar_msg;
 
 uint32_t nof_rtx_connsetup = 0; 
-uint32_t rv_value[4] = {0, 2, 3, 1}; 
+uint32_t rv_value[4] = {0, 2, 3, 1};
 
 void config_phy() {
   srsue::phy_interface_rrc::phy_cfg_t config; 
@@ -194,8 +194,8 @@ void config_phy() {
   my_phy.configure_prach_params();
 }
 
-srslte_softbuffer_rx_t softbuffer_rx; 
-srslte_softbuffer_tx_t softbuffer_tx; 
+srslte_softbuffer_rx_t softbuffers_rx[SRSLTE_MAX_TB];
+srslte_softbuffer_tx_t softbuffers_tx[SRSLTE_MAX_TB];
 
 uint16_t temp_c_rnti; 
 
@@ -218,7 +218,7 @@ public:
   
   bool rar_rnti_set;
 
-  void pch_decoded_ok(uint32_t len) {} 
+  void pch_decoded_ok(uint32_t len) {}
 
   
   void tti_clock(uint32_t tti) {
@@ -233,19 +233,21 @@ public:
   
   void new_grant_ul(mac_grant_t grant, tb_action_ul_t *action) {
     printf("New grant UL\n");
-    memcpy(payload, conn_request_msg, grant.n_bytes*sizeof(uint8_t));
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+      memcpy(payload[tb], conn_request_msg, grant.n_bytes[tb]*sizeof(uint8_t));
+      action->rv[tb] = rv_value[nof_rtx_connsetup%4];
+      action->payload_ptr[tb] = payload[tb];
+      if (action->rv[tb] == 0) {
+        srslte_softbuffer_tx_reset(&softbuffers_tx[tb]);
+      }
+    }
     action->current_tx_nb = nof_rtx_connsetup;
-    action->rv = rv_value[nof_rtx_connsetup%4];
-    action->softbuffer = &softbuffer_tx;     
+    action->softbuffers = softbuffers_tx;
     action->rnti = temp_c_rnti; 
     action->expect_ack = (nof_rtx_connsetup < 5)?true:false;
-    action->payload_ptr = payload;
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     memcpy(&last_grant, &grant, sizeof(mac_grant_t));
     action->tx_enabled = true; 
-    if (action->rv == 0) {
-      srslte_softbuffer_tx_reset(&softbuffer_tx);      
-    }
     my_phy.pdcch_dl_search(SRSLTE_RNTI_USER, temp_c_rnti);
   }
   
@@ -258,17 +260,20 @@ public:
     if (!ack) {
       nof_rtx_connsetup++;
       action->current_tx_nb = nof_rtx_connsetup;
-      action->rv = rv_value[nof_rtx_connsetup%4];
-      action->softbuffer = &softbuffer_tx;     
+      action->softbuffers = softbuffers_tx;
       action->rnti = temp_c_rnti; 
       action->expect_ack = true; 
       memcpy(&action->phy_grant, &last_grant.phy_grant, sizeof(srslte_phy_grant_t));
-      action->tx_enabled = true; 
-      if (action->rv == 0) {
-        srslte_softbuffer_tx_reset(&softbuffer_tx);      
+      action->tx_enabled = true;
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+        action->rv[tb] = rv_value[nof_rtx_connsetup%4];
+        if (action->rv[tb] == 0) {
+          srslte_softbuffer_tx_reset(&softbuffers_tx[tb]);
+        }
+        printf("Retransmission %d (TB %d), rv=%d\n", nof_rtx_connsetup, tb, action->rv[tb]);
+
       }
-      printf("Retransmission %d, rv=%d\n", nof_rtx_connsetup, action->rv);
-    } 
+    }
   }
 
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) {
@@ -279,24 +284,25 @@ public:
     } else {
       action->generate_ack = true; 
     }
-    action->payload_ptr = payload; 
-    action->rnti = grant.rnti; 
+    action->rnti = grant.rnti;
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     memcpy(&last_grant, &grant, sizeof(mac_grant_t));
-    action->rv = grant.rv;
-    action->softbuffer = &softbuffer_rx;
-    
-    if (action->rv == 0) {
-      srslte_softbuffer_rx_reset(&softbuffer_rx);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+      action->softbuffers[tb] = &softbuffers_rx[tb];
+      action->rv[tb] = grant.rv[tb];
+      action->payload_ptr[tb] = payload[tb];
+      if (action->rv[tb] == 0) {
+        srslte_softbuffer_rx_reset(&softbuffers_rx[tb]);
+      }
     }
   }
   
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
     if (ack) {
       if (rnti_type == SRSLTE_RNTI_RAR) {
         my_phy.pdcch_dl_search_reset();
-        srslte_bit_unpack_vector(payload, payload_bits, last_grant.n_bytes*8);
-        rar_unpack(payload_bits, &rar_msg);
+        srslte_bit_unpack_vector(payload[tb_idx], payload_bits[tb_idx], last_grant.n_bytes[tb_idx]*8);
+        rar_unpack(payload_bits[tb_idx], &rar_msg);
         if (rar_msg.RAPID == preamble_idx) {
 
           printf("Received RAR at TTI: %d\n", last_grant.tti);
@@ -304,7 +310,7 @@ public:
           
           temp_c_rnti = rar_msg.temp_c_rnti; 
           
-          if (last_grant.n_bytes*8 > 20 + SRSLTE_RAR_GRANT_LEN) {
+          if (last_grant.n_bytes[0]*8 > 20 + SRSLTE_RAR_GRANT_LEN) {
             uint8_t rar_grant[SRSLTE_RAR_GRANT_LEN];
             memcpy(rar_grant, &payload_bits[20], sizeof(uint8_t)*SRSLTE_RAR_GRANT_LEN);
             my_phy.set_rar_grant(last_grant.tti, rar_grant);          
@@ -323,9 +329,11 @@ public:
     printf("BCH decoded\n");
     bch_decoded = true; 
     srslte_cell_t cell; 
-    my_phy.get_current_cell(&cell); 
-    srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb);
-    srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb);
+    my_phy.get_current_cell(&cell);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      srslte_softbuffer_rx_init(&softbuffers_rx[tb], cell.nof_prb);
+      srslte_softbuffer_tx_init(&softbuffers_tx[tb], cell.nof_prb);
+    }
   }
    
 private: 
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index 803c5ff11..be3cb18e8 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -87,8 +87,8 @@ bool bch_decoded = false;
 uint32_t total_pkts=0;
 uint32_t total_dci=0;
 uint32_t total_oks=0;
-uint8_t payload[1024]; 
-srslte_softbuffer_rx_t softbuffer; 
+uint8_t payload[SRSLTE_MAX_TB][1024];
+srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_TB];
 
 class rrc_dummy : public srsue::rrc_interface_phy
 {
@@ -118,20 +118,23 @@ public:
 
     action->decode_enabled = true; 
     action->default_ack = false; 
-    action->generate_ack = false; 
-    action->payload_ptr = payload; 
-    memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
-    action->rv = ((uint32_t) ceilf((float)3*((my_phy.tti_to_SFN(grant.tti)/2)%4)/2))%4;
-    action->softbuffer = &softbuffer;
-    action->rnti = grant.rnti; 
-    if (action->rv == 0) {
-      srslte_softbuffer_rx_reset(&softbuffer);
+    action->generate_ack = false;
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      action->payload_ptr[tb] = payload[tb];
+      action->rv[tb] = ((uint32_t) ceilf((float) 3 * ((my_phy.tti_to_SFN(grant.tti) / 2) % 4) / 2)) % 4;
+      if (action->rv == 0) {
+        srslte_softbuffer_rx_reset(&softbuffers[tb]);
+      }
+      action->softbuffers[0] = &softbuffers[tb];
     }
+    memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
+
+    action->rnti = grant.rnti;
   }
   
   
   
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti, uint32_t harq_pid) {
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
     if (ack) {
       total_oks++;     
     }
@@ -144,7 +147,9 @@ public:
     bch_decoded = true; 
     srslte_cell_t cell; 
     my_phy.get_current_cell(&cell); 
-    srslte_softbuffer_rx_init(&softbuffer, cell.nof_prb);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      srslte_softbuffer_rx_init(&softbuffers[tb], cell.nof_prb);
+    }
   }
   void tti_clock(uint32_t tti) {
     

From cedc32fbb04093b8cbca13ab8aa6ff91255fb74a Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 28 Aug 2017 14:12:56 +0200
Subject: [PATCH 093/269] Added RI (CQI periodic reporting) send condition
 function

---
 lib/include/srslte/phy/phch/cqi.h |   9 ++-
 lib/src/phy/phch/cqi.c            | 103 ++++++++++++++++++++++--------
 2 files changed, 83 insertions(+), 29 deletions(-)

diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index f6857dfa0..593592f1c 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -45,7 +45,8 @@
 typedef struct {
   bool     configured; 
   uint32_t pmi_idx; 
-  bool     simul_cqi_ack; 
+  uint32_t ri_idx;
+  bool     simul_cqi_ack;
   bool     format_is_subband; 
   uint32_t subband_size; 
 } srslte_cqi_periodic_cfg_t; 
@@ -137,7 +138,11 @@ SRSLTE_API int srslte_cqi_format2_subband_unpack(uint8_t buff[SRSLTE_CQI_MAX_BIT
                                                  srslte_cqi_format2_subband_t *msg);
 
 SRSLTE_API bool srslte_cqi_send(uint32_t I_cqi_pmi, 
-                                uint32_t tti); 
+                                uint32_t tti);
+
+SRSLTE_API bool srslte_ri_send(uint32_t I_cqi_pmi,
+                               uint32_t I_ri,
+                               uint32_t tti);
 
 SRSLTE_API uint8_t srslte_cqi_from_snr(float snr);
 
diff --git a/lib/src/phy/phch/cqi.c b/lib/src/phy/phch/cqi.c
index a684db8c9..0041c3fcb 100644
--- a/lib/src/phy/phch/cqi.c
+++ b/lib/src/phy/phch/cqi.c
@@ -159,46 +159,54 @@ int srslte_cqi_size(srslte_cqi_value_t *value) {
   return -1;
 }
 
-bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
-  
-  uint32_t N_p = 0;
-  uint32_t N_offset = 0;
-  
+static bool srslte_cqi_get_N(uint32_t I_cqi_pmi, uint32_t *N_p, uint32_t *N_offset) {
   if (I_cqi_pmi <= 1) {
-    N_p = 2; 
-    N_offset = I_cqi_pmi; 
+    *N_p = 2;
+    *N_offset = I_cqi_pmi;
   } else if (I_cqi_pmi <= 6) {
-    N_p = 5; 
-    N_offset = I_cqi_pmi - 2;     
+    *N_p = 5;
+    *N_offset = I_cqi_pmi - 2;
   } else if (I_cqi_pmi <= 16) {
-    N_p = 10; 
-    N_offset = I_cqi_pmi - 7;     
+    *N_p = 10;
+    *N_offset = I_cqi_pmi - 7;
   } else if (I_cqi_pmi <= 36) {
-    N_p = 20; 
-    N_offset = I_cqi_pmi - 17;     
+    *N_p = 20;
+    *N_offset = I_cqi_pmi - 17;
   } else if (I_cqi_pmi <= 76) {
-    N_p = 40; 
-    N_offset = I_cqi_pmi - 37;     
+    *N_p = 40;
+    *N_offset = I_cqi_pmi - 37;
   } else if (I_cqi_pmi <= 156) {
-    N_p = 80; 
-    N_offset = I_cqi_pmi - 77;     
+    *N_p = 80;
+    *N_offset = I_cqi_pmi - 77;
   } else if (I_cqi_pmi <= 316) {
-    N_p = 160; 
-    N_offset = I_cqi_pmi - 157;   
+    *N_p = 160;
+    *N_offset = I_cqi_pmi - 157;
   } else if (I_cqi_pmi == 317) {
-    return false; 
+    return false;
   } else if (I_cqi_pmi <= 349) {
-    N_p = 32; 
-    N_offset = I_cqi_pmi - 318;     
+    *N_p = 32;
+    *N_offset = I_cqi_pmi - 318;
   } else if (I_cqi_pmi <= 413) {
-    N_p = 64; 
-    N_offset = I_cqi_pmi - 350;     
+    *N_p = 64;
+    *N_offset = I_cqi_pmi - 350;
   } else if (I_cqi_pmi <= 541) {
-    N_p = 128; 
-    N_offset = I_cqi_pmi - 414;     
+    *N_p = 128;
+    *N_offset = I_cqi_pmi - 414;
   } else if (I_cqi_pmi <= 1023) {
-    return false; 
+    return false;
+  }
+  return true;
+}
+
+bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
+  
+  uint32_t N_p = 0;
+  uint32_t N_offset = 0;
+
+  if (!srslte_cqi_get_N(I_cqi_pmi, &N_p, &N_offset)) {
+    return false;
   }
+
   if (N_p) {
     if ((tti-N_offset)%N_p == 0) {
       return true; 
@@ -207,6 +215,47 @@ bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
   return false; 
 }
 
+bool srslte_ri_send(uint32_t I_cqi_pmi, uint32_t I_ri, uint32_t tti) {
+
+  uint32_t M_ri = 0;
+  uint32_t N_offset_ri = 0;
+  uint32_t N_p = 0;
+  uint32_t N_offset_p = 0;
+
+  if (!srslte_cqi_get_N(I_cqi_pmi, &N_p, &N_offset_p)) {
+    return false;
+  }
+
+  if (I_ri <= 160) {
+    M_ri = 1;
+    N_offset_ri = I_ri;
+  } else if (I_ri <= 161) {
+    M_ri = 2;
+    N_offset_ri = I_ri - 161;
+  } else if (I_ri <= 322) {
+    M_ri = 4;
+    N_offset_ri = I_ri - 322;
+  } else if (I_ri <= 483) {
+    M_ri = 8;
+    N_offset_ri = I_ri - 483;
+  } else if (I_ri <= 644) {
+    M_ri = 16;
+    N_offset_ri = I_ri - 644;
+  } else if (I_ri <= 805) {
+    M_ri = 32;
+    N_offset_ri = I_ri - 805;
+  } else if (I_ri <= 966) {
+    return false;
+  }
+
+  if (M_ri) {
+    if ((tti - N_offset_p + N_offset_ri) % (N_p * M_ri) == 0) {
+      return true;
+    }
+  }
+  return false;
+}
+
 
 // CQI-to-Spectral Efficiency:  36.213 Table 7.2.3-1  */
 static float cqi_to_coderate[16] = {0, 0.1523, 0.2344, 0.3770, 0.6016, 0.8770, 1.1758, 1.4766, 1.9141, 2.4063, 2.7305, 3.3223, 3.9023, 4.5234, 5.1152, 5.5547}; 

From 0ca8c8aac698cbcb18fba5d2a011bdc27409b07d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 28 Aug 2017 16:49:53 +0200
Subject: [PATCH 094/269] considering PUCCH2 in PUSCH scheduler

---
 srsenb/src/mac/scheduler_ue.cc | 30 +++++++++++++++++++++++-------
 srsenb/src/mac/ue.cc           |  6 +++---
 2 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 5545efb51..b217a8157 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -26,6 +26,7 @@
 
 #include <string.h>
 #include <boost/concept_check.hpp>
+#include <srslte/interfaces/sched_interface.h>
 
 #include "srslte/srslte.h"
 #include "srslte/common/pdu.h"
@@ -171,9 +172,8 @@ void sched_ue::ul_buffer_state(uint8_t lc_id, uint32_t bsr, bool set_value)
     } else {
       lch[lc_id].bsr += bsr;
     }
-    Debug("SCHED: UL lcid=%d buffer_state=%d\n", lc_id, bsr);
   }
-  Info("SCHED: bsr=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", bsr, lc_id,
+  Debug("SCHED: bsr=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", bsr, lc_id,
        lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
 }
 
@@ -245,7 +245,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
   
   // First check if it has pending ACKs 
   for (int i=0;i<SCHED_MAX_HARQ_PROC;i++) {
-    if (((dl_harq[i].get_tti()+8)%10240) == current_tti) {
+    if (((dl_harq[i].get_tti()+4)%10240) == current_tti) {
       uint32_t n_pucch = srslte_pucch_get_npucch(dl_harq[i].get_n_cce(), SRSLTE_PUCCH_FORMAT_1A, has_sr, &pucch_sched);
       if (prb_idx) {
         for (int i=0;i<2;i++) {
@@ -255,7 +255,8 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
       if (L) {
         *L = 1; 
       }
-      Debug("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);      
+      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, n_cce=%d, has_sr=%d\n",
+           rnti, prb_idx[0], prb_idx[1], n_pucch, dl_harq[i].get_n_cce(), has_sr);
       return true; 
     }
   }
@@ -269,9 +270,24 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
     if (L) {
       *L = 1; 
     }
-    Debug("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
+    Info("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
     return true; 
   }
+  // Finally check Format2 (periodic CQI)
+  if (cfg.cqi_enabled && srslte_cqi_send(cfg.cqi_idx, current_tti)) {
+    if (prb_idx) {
+      for (int i=0;i<2;i++) {
+        prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_2, cfg.cqi_pucch, cell.nof_prb, cell.cp, i);
+      }
+    }
+    if(L) {
+      *L = 2;
+    }
+    Info("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
+         rnti, prb_idx[0], prb_idx[1], cfg.cqi_pucch, cfg.cqi_idx);
+    return true;
+  }
+
   return false; 
 }
 
@@ -303,7 +319,7 @@ void sched_ue::ul_recv_len(uint32_t lcid, uint32_t len)
       }
     }
   }
-  Info("SCHED: recv_len=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", len, lcid,
+  Debug("SCHED: recv_len=%d, lcid=%d, bsr={%d,%d,%d,%d}\n", len, lcid,
        lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
 }
 
@@ -563,7 +579,7 @@ uint32_t sched_ue::get_pending_ul_new_data(uint32_t tti)
     pending_data = 0; 
   }
   if (pending_data) {
-    Info("SCHED: pending_data=%d, pending_ul_data=%d, bsr={%d,%d,%d,%d}\n", pending_data,pending_ul_data,
+    Debug("SCHED: pending_data=%d, pending_ul_data=%d, bsr={%d,%d,%d,%d}\n", pending_data,pending_ul_data,
          lch[0].bsr, lch[1].bsr, lch[2].bsr, lch[3].bsr);
   }
   return pending_data; 
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index b338a202d..0b9776918 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -226,7 +226,7 @@ void ue::process_pdu(uint8_t* pdu, uint32_t nof_bytes, uint32_t tstamp)
   if (!bsr_received && lcid_most_data > 2) {
     // Add BSR to the LCID for which most data was received
     sched->ul_bsr(rnti, lcid_most_data, 256, false); // false adds BSR instead of setting
-    Info("BSR not received. Giving extra grant\n");
+    Debug("BSR not received. Giving extra grant\n");
   }
 
   Debug("MAC PDU processed\n");
@@ -282,9 +282,9 @@ bool ue::process_ce(srslte::sch_subh *subh) {
       for (uint32_t i=0;i<lc_groups[idx].size();i++) {
         // Indicate BSR to scheduler
         sched->ul_bsr(rnti, lc_groups[idx][i], buff_size[idx]);
-        Info("CE:    Received %s BSR rnti=0x%x, lcg=%d, lcid=%d, value=%d\n",
-             subh->ce_type()==srslte::sch_subh::SHORT_BSR?"Short":"Trunc", rnti, idx, lc_groups[idx][i], buff_size[idx]);
       }
+      Info("CE:    Received %s BSR rnti=0x%x, lcg=%d, value=%d\n",
+           subh->ce_type()==srslte::sch_subh::SHORT_BSR?"Short":"Trunc", rnti, idx, buff_size[idx]);
       is_bsr = true;
       break;
     case srslte::sch_subh::LONG_BSR:

From 2832ece415db43e5b9ea8486ba6bd5a346e517cf Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 28 Aug 2017 18:41:23 +0200
Subject: [PATCH 095/269] Fixed SCTP stream id to 1.

---
 srsenb/src/upper/s1ap.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index 032a6018f..9328060e6 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -173,7 +173,7 @@ void s1ap::build_tai_cgi()
 void s1ap::initial_ue(uint16_t rnti, srslte::byte_buffer_t *pdu)
 {
   ue_ctxt_map[rnti].eNB_UE_S1AP_ID = next_eNB_UE_S1AP_ID++;
-  ue_ctxt_map[rnti].stream_id      = next_ue_stream_id++;
+  ue_ctxt_map[rnti].stream_id      = 1;
   ue_ctxt_map[rnti].release_requested = false;
   enbid_to_rnti_map[ue_ctxt_map[rnti].eNB_UE_S1AP_ID] = rnti;
   send_initialuemessage(rnti, pdu, false);
@@ -182,7 +182,7 @@ void s1ap::initial_ue(uint16_t rnti, srslte::byte_buffer_t *pdu)
 void s1ap::initial_ue(uint16_t rnti, srslte::byte_buffer_t *pdu, uint32_t m_tmsi, uint8_t mmec)
 {
   ue_ctxt_map[rnti].eNB_UE_S1AP_ID = next_eNB_UE_S1AP_ID++;
-  ue_ctxt_map[rnti].stream_id      = next_ue_stream_id++;
+  ue_ctxt_map[rnti].stream_id      = 1;
   ue_ctxt_map[rnti].release_requested = false;
   enbid_to_rnti_map[ue_ctxt_map[rnti].eNB_UE_S1AP_ID] = rnti;
   send_initialuemessage(rnti, pdu, true, m_tmsi, mmec);

From c10f13139ef47aa6ead92a34b69d6d183ed6d6da Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 10:09:56 +0200
Subject: [PATCH 096/269] Added default value for filter coefficient in UL
 power control dedicated

---
 srsue/src/upper/rrc.cc | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 8bb327b9d..80a6303b4 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -27,6 +27,7 @@
 
 #include <unistd.h>
 #include <sstream>
+#include <srslte/asn1/liblte_rrc.h>
 
 #include "upper/rrc.h"
 #include "srslte/phy/utils/bit.h"
@@ -1084,8 +1085,11 @@ void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT
     current_cfg->ul_pwr_ctrl_ded.accumulation_en = true; 
     current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch     = 0;
     current_cfg->ul_pwr_ctrl_ded.p_srs_offset    = 7;
-    current_cfg->ul_pwr_ctrl_ded.filter_coeff    = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;      
-    current_cfg->ul_pwr_ctrl_ded.filter_coeff_present = true; 
+  }
+  if (phy_cnfg->ul_pwr_ctrl_ded.filter_coeff_present) {
+    current_cfg->ul_pwr_ctrl_ded.filter_coeff = current_cfg->ul_pwr_ctrl_ded.filter_coeff;
+  } else {
+    current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
   }
   if(phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
     memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch, sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT)); 

From 9997f8c20ae4a36838b5f87a5cd7d0daef95f876 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 11:22:39 +0200
Subject: [PATCH 097/269] (fix typo) Added default value for filter coefficient
 in UL power control dedicated

---
 srsue/src/upper/rrc.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 80a6303b4..58015030d 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -1087,7 +1087,7 @@ void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT
     current_cfg->ul_pwr_ctrl_ded.p_srs_offset    = 7;
   }
   if (phy_cnfg->ul_pwr_ctrl_ded.filter_coeff_present) {
-    current_cfg->ul_pwr_ctrl_ded.filter_coeff = current_cfg->ul_pwr_ctrl_ded.filter_coeff;
+    current_cfg->ul_pwr_ctrl_ded.filter_coeff = phy_cnfg->ul_pwr_ctrl_ded.filter_coeff;
   } else {
     current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
   }

From 51fc9bffb1589282e4c1ffa158f911a08a6aca5d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 12:01:30 +0200
Subject: [PATCH 098/269] added release function for mac timers

---
 lib/include/srslte/common/timers.h | 37 +++++++++++++++++++++++-------
 srsue/src/upper/rrc.cc             | 11 +++++----
 2 files changed, 36 insertions(+), 12 deletions(-)

diff --git a/lib/include/srslte/common/timers.h b/lib/include/srslte/common/timers.h
index 56e40e152..377df7b1d 100644
--- a/lib/include/srslte/common/timers.h
+++ b/lib/include/srslte/common/timers.h
@@ -97,11 +97,13 @@ public:
     bool running; 
   };
   
-  timers(uint32_t nof_timers_) : timer_list(nof_timers_) {
+  timers(uint32_t nof_timers_) : timer_list(nof_timers_),used_timers(nof_timers_) {
     nof_timers = nof_timers_; 
     next_timer = 0;
+    nof_used_timers = 0;
     for (uint32_t i=0;i<nof_timers;i++) {
-      timer_list[i].id = i; 
+      timer_list[i].id = i;
+      used_timers[i] = false;
     }
   }
   
@@ -133,17 +135,36 @@ public:
       return NULL; 
     }
   }
+  void release_id(uint32_t i) {
+    if (nof_used_timers > 0 && i < nof_timers) {
+      used_timers[i] = false;
+      nof_used_timers--;
+    } else {
+      fprintf(stderr, "Error releasing timer: nof_used_timers=%d, nof_timers=%d\n", nof_used_timers, nof_timers);
+    }
+  }
   uint32_t get_unique_id() {
-    if (next_timer == nof_timers){
-      printf("No more unique timer ids (Only %d timers available)\n", nof_timers);
-      next_timer = 0;
+    if (nof_used_timers >= nof_timers) {
+      fprintf(stderr, "Error getting uinque timer id: no more timers available\n");
+      return 0;
+    } else {
+      while(used_timers[next_timer]) {
+        next_timer++;
+        if (next_timer >= nof_timers) {
+          next_timer=0;
+        }
+      }
+      used_timers[next_timer] = true;
+      nof_used_timers++;
+      return next_timer;
     }
-    return next_timer++;
   }
 private:
-  uint32_t nof_timers; 
   uint32_t next_timer;
-  std::vector<timer>   timer_list;   
+  uint32_t nof_used_timers;
+  uint32_t nof_timers;
+  std::vector<timer>   timer_list;
+  std::vector<bool>    used_timers;
 };
 
 } // namespace srslte
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 58015030d..291024c05 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -79,7 +79,14 @@ void rrc::init(phy_interface_rrc     *phy_,
   nas     = nas_;
   usim    = usim_;
   rrc_log = rrc_log_;
+
+  // Use MAC timers
   mac_timers = mac_timers_;
+  t301 = mac_timers->get_unique_id();
+  t310 = mac_timers->get_unique_id();
+  t311 = mac_timers->get_unique_id();
+  safe_reset_timer = mac_timers->get_unique_id();
+
 
   pthread_mutex_init(&mutex, NULL); 
   
@@ -1492,10 +1499,6 @@ void rrc::set_mac_default()
 void rrc::set_rrc_default() {
   N310 = 1;
   N311 = 1; 
-  t301 = mac_timers->get_unique_id();
-  t310 = mac_timers->get_unique_id();
-  t311 = mac_timers->get_unique_id();
-  safe_reset_timer = mac_timers->get_unique_id();
   mac_timers->get(t310)->set(this, 1000);
   mac_timers->get(t311)->set(this, 1000);
   mac_timers->get(safe_reset_timer)->set(this, 10);

From b7a91b597190426748db155c02cdee486976f621 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 12:20:47 +0200
Subject: [PATCH 099/269] fixed unititiazed pucch_sched in
 sched_ue::get_pucch_sched

---
 srsenb/src/mac/scheduler_ue.cc | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index b217a8157..0d5dd87bb 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -27,6 +27,7 @@
 #include <string.h>
 #include <boost/concept_check.hpp>
 #include <srslte/interfaces/sched_interface.h>
+#include <srslte/phy/phch/pucch.h>
 
 #include "srslte/srslte.h"
 #include "srslte/common/pdu.h"
@@ -236,7 +237,8 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
   if (!phy_config_dedicated_enabled) {
     return false; 
   }
-  srslte_pucch_sched_t pucch_sched; 
+  srslte_pucch_sched_t pucch_sched;
+  pucch_sched.sps_enabled = false;
   pucch_sched.n_pucch_sr = cfg.sr_N_pucch;
   pucch_sched.n_pucch_2  = cfg.n_pucch_cqi;
   pucch_sched.N_pucch_1  = cfg.pucch_cfg.n1_pucch_an; 
@@ -255,8 +257,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
       if (L) {
         *L = 1; 
       }
-      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, n_cce=%d, has_sr=%d\n",
-           rnti, prb_idx[0], prb_idx[1], n_pucch, dl_harq[i].get_n_cce(), has_sr);
+      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
       return true; 
     }
   }

From 8a25cac41bc71fc26c8d2d1516c4cf02765acc9e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 15:07:47 +0200
Subject: [PATCH 100/269] increased timeout for RRCConnectionRelease
 transmission

---
 srsenb/src/upper/rrc.cc  | 5 ++---
 srsenb/src/upper/s1ap.cc | 2 +-
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index de1c61b9e..03ea65f2c 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -314,12 +314,11 @@ void rrc::release_complete(uint16_t rnti)
     if (!users[rnti].is_idle()) {
       rlc->clear_buffer(rnti); 
       users[rnti].send_connection_release();
-      // There is no RRCReleaseComplete message from UE thus sleep to enable all retx in PHY +50%
-      usleep(1500*8*cfg.mac_cnfg.ulsch_cnfg.max_harq_tx);
+      // There is no RRCReleaseComplete message from UE thus wait ~100 subframes for tx
+      usleep(100000);
     }
     rem_user(rnti);
   } else {
-    
     rrc_log->error("Received ReleaseComplete for unknown rnti=0x%x\n", rnti);
   }
 }
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index 9328060e6..eba9186e7 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -594,7 +594,7 @@ bool s1ap::handle_uectxtreleasecommand(LIBLTE_S1AP_MESSAGE_UECONTEXTRELEASECOMMA
     s1ap_log->warning("Not handling S1AP message extension\n");
   }
 
-  uint16_t rnti;
+  uint16_t rnti = 0;
   if(msg->UE_S1AP_IDs.choice_type == LIBLTE_S1AP_UE_S1AP_IDS_CHOICE_UE_S1AP_ID_PAIR) {
 
     if(msg->UE_S1AP_IDs.choice.uE_S1AP_ID_pair.ext) {

From fabeca49c741fc19812560bb9c0b926ef4186578 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 15:38:16 +0200
Subject: [PATCH 101/269] moved pdsch_coderate to phy_common

---
 lib/include/srslte/phy/common/phy_common.h | 3 +++
 lib/include/srslte/phy/phch/pdsch.h        | 5 +----
 lib/src/phy/common/phy_common.c            | 5 +++++
 lib/src/phy/phch/pdsch.c                   | 7 +------
 srsenb/src/mac/scheduler_ue.cc             | 2 +-
 5 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index efd498c86..65dc2319c 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -242,6 +242,9 @@ SRSLTE_API uint32_t srslte_N_ta_new_rar(uint32_t ta);
 SRSLTE_API uint32_t srslte_N_ta_new(uint32_t N_ta_old, 
                                     uint32_t ta);
 
+SRSLTE_API float srslte_coderate(uint32_t tbs,
+                                 uint32_t nof_re);
+
 SRSLTE_API char *srslte_cp_string(srslte_cp_t cp); 
 
 SRSLTE_API char *srslte_mod_string(srslte_mod_t mod);
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index ad01c4ef8..a73129444 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -93,10 +93,7 @@ SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q,
 SRSLTE_API void srslte_pdsch_free_rnti(srslte_pdsch_t *q, 
                                       uint16_t rnti);
 
-SRSLTE_API float srslte_pdsch_coderate(uint32_t tbs, 
-                                       uint32_t nof_re); 
-
-SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, 
+SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg,
                                 srslte_cell_t cell, 
                                 srslte_ra_dl_grant_t *grant, 
                                 uint32_t cfi, 
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 8386ae18b..76dc0366f 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -178,6 +178,11 @@ uint32_t srslte_N_ta_new(uint32_t N_ta_old, uint32_t ta) {
   }
 }
 
+float srslte_coderate(uint32_t tbs, uint32_t nof_re)
+{
+  return (float) (tbs + 24)/(nof_re);
+}
+
 /* Returns the new time advance as indicated by the random access response 
  * as specified in Section 4.2.3 of 36.213 */
 uint32_t srslte_N_ta_new_rar(uint32_t ta) {
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 9b6128c64..a31e98c77 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -58,12 +58,7 @@ extern int indices[100000];
 extern int indices_ptr; 
 #endif
 
-float srslte_pdsch_coderate(uint32_t tbs, uint32_t nof_re) 
-{
-  return (float) (tbs + 24)/(nof_re);
-}
-
-int srslte_pdsch_cp(srslte_pdsch_t *q, cf_t *input, cf_t *output, srslte_ra_dl_grant_t *grant, uint32_t lstart_grant, uint32_t nsubframe, bool put) 
+int srslte_pdsch_cp(srslte_pdsch_t *q, cf_t *input, cf_t *output, srslte_ra_dl_grant_t *grant, uint32_t lstart_grant, uint32_t nsubframe, bool put)
 {
   uint32_t s, n, l, lp, lstart, lend, nof_refs;
   bool is_pbch, is_sss;
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 0d5dd87bb..0518318f3 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -770,7 +770,7 @@ int sched_ue::cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32
     sel_mcs--; 
     uint32_t tbs_idx = srslte_ra_tbs_idx_from_mcs(sel_mcs);
     tbs = srslte_ra_tbs_from_idx(tbs_idx, nof_prb);
-    coderate = srslte_pdsch_coderate(tbs, nof_re);     
+    coderate = srslte_coderate(tbs, nof_re);
   } while(sel_mcs > 0 && coderate >= max_coderate);
   if (mcs) {
     *mcs = (uint32_t) sel_mcs; 

From 58aecf818d5eecdf874fec877ec84adfff26e0fe Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 29 Aug 2017 16:22:45 +0200
Subject: [PATCH 102/269] Allowing any PUSCH MCS

---
 srsenb/enb.conf.example        |  2 +-
 srsenb/hdr/mac/scheduler_ue.h  |  6 ++--
 srsenb/sib.conf.example        |  2 +-
 srsenb/src/mac/scheduler_ue.cc | 58 ++++++++++++++++++++++++----------
 srsenb/src/main.cc             |  2 +-
 srsenb/src/phy/phch_worker.cc  |  8 ++++-
 6 files changed, 55 insertions(+), 23 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 8cc063d41..7446df122 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -121,7 +121,7 @@ enable = false
 #pdsch_mcs        = -1
 #pdsch_max_mcs    = -1
 #pusch_mcs        = -1
-pusch_max_mcs    = 16
+#pusch_max_mcs    = -1
 nof_ctrl_symbols = 2
 
 #####################################################################
diff --git a/srsenb/hdr/mac/scheduler_ue.h b/srsenb/hdr/mac/scheduler_ue.h
index 1cb223e57..b95e5dda2 100644
--- a/srsenb/hdr/mac/scheduler_ue.h
+++ b/srsenb/hdr/mac/scheduler_ue.h
@@ -131,8 +131,10 @@ private:
   int        alloc_pdu(int tbs, sched_interface::dl_sched_pdu_t* pdu);
 
   static uint32_t format1_count_prb(uint32_t bitmask, uint32_t cell_nof_prb); 
-  static int cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t max_mcs, uint32_t *mcs);
-  static int alloc_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t req_bytes, uint32_t max_mcs, int *mcs); 
+  static int cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t max_mcs, uint32_t max_Qm, uint32_t *mcs);
+  int alloc_tbs_dl(uint32_t nof_prb, uint32_t nof_re, uint32_t req_bytes, int *mcs);
+  int alloc_tbs_ul(uint32_t nof_prb, uint32_t nof_re, uint32_t req_bytes, int *mcs);
+  int alloc_tbs(uint32_t nof_prb, uint32_t nof_re, uint32_t req_bytes, bool is_ul, int *mcs);
   
   static bool bearer_is_ul(ue_bearer_t *lch);
   static bool bearer_is_dl(ue_bearer_t *lch);
diff --git a/srsenb/sib.conf.example b/srsenb/sib.conf.example
index 7002f4a7f..f23d18981 100644
--- a/srsenb/sib.conf.example
+++ b/srsenb/sib.conf.example
@@ -60,7 +60,7 @@ sib2 =
             n_sb = 1;
             hopping_mode = "inter-subframe";
             pusch_hopping_offset = 2;
-            enable_64_qam = false;
+            enable_64_qam = false; // 64QAM PUSCH is not currently enabled
             ul_rs = 
             {
                 cyclic_shift = 0; 
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 0518318f3..21aa61b19 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -395,7 +395,7 @@ int sched_ue::generate_format1(dl_harq_proc *h,
     uint32_t nof_ctrl_symbols = cfi+(cell.nof_prb<10?1:0);
     uint32_t nof_re = srslte_ra_dl_grant_nof_re(&grant, cell, sf_idx, nof_ctrl_symbols);
     if (fixed_mcs_dl < 0) {
-      tbs = alloc_tbs(dl_cqi, nof_prb, nof_re, req_bytes, max_mcs_dl, &mcs);      
+      tbs = alloc_tbs_dl(nof_prb, nof_re, req_bytes, &mcs);
     } else {
       tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), nof_prb);
       mcs = fixed_mcs_dl; 
@@ -466,7 +466,7 @@ int sched_ue::generate_format0(ul_harq_proc *h,
     uint32_t N_srs = 0; 
     uint32_t nof_re = (2*(SRSLTE_CP_NSYMB(cell.cp)-1) - N_srs)*allocation.L*SRSLTE_NRE;
     if (fixed_mcs_ul < 0) {
-      tbs = alloc_tbs(ul_cqi, allocation.L, nof_re, req_bytes, max_mcs_ul, &mcs);      
+      tbs = alloc_tbs_ul(allocation.L, nof_re, req_bytes, &mcs);
     } else {
       tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), allocation.L);
       mcs = fixed_mcs_ul;
@@ -610,7 +610,7 @@ uint32_t sched_ue::get_required_prb_dl(uint32_t req_bytes, uint32_t nof_ctrl_sym
   for (n=1;n<cell.nof_prb && nbytes < req_bytes;n++) {
     nof_re = srslte_ra_dl_approx_nof_re(cell, n, nof_ctrl_symbols);
     if (fixed_mcs_dl < 0) {
-      tbs = alloc_tbs(dl_cqi, n, nof_re, 0, max_mcs_dl, &mcs);      
+      tbs = alloc_tbs_dl(n, nof_re, 0, &mcs);
     } else {
       tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), n);
     }
@@ -639,7 +639,7 @@ uint32_t sched_ue::get_required_prb_ul(uint32_t req_bytes)
     uint32_t nof_re = (2*(SRSLTE_CP_NSYMB(cell.cp)-1) - N_srs)*n*SRSLTE_NRE;
     int tbs = 0; 
     if (fixed_mcs_ul < 0) {
-      tbs = alloc_tbs(ul_cqi, n, nof_re, 0, max_mcs_ul, &mcs);      
+      tbs = alloc_tbs_ul(n, nof_re, 0, &mcs);
     } else {
       tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), n);
     }
@@ -760,10 +760,12 @@ uint32_t sched_ue::format1_count_prb(uint32_t bitmask, uint32_t cell_nof_prb) {
   return nof_prb; 
 }
 
-int sched_ue::cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t max_mcs, uint32_t *mcs) {
+int sched_ue::cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32_t max_mcs, uint32_t max_Qm, uint32_t *mcs) {
   float max_coderate = srslte_cqi_to_coderate(cqi);
   int sel_mcs = max_mcs+1; 
-  float coderate = 99; 
+  float coderate = 99;
+  float eff_coderate = 99;
+  uint32_t Qm = 1;
   int tbs = 0; 
 
   do {
@@ -771,25 +773,47 @@ int sched_ue::cqi_to_tbs(uint32_t cqi, uint32_t nof_prb, uint32_t nof_re, uint32
     uint32_t tbs_idx = srslte_ra_tbs_idx_from_mcs(sel_mcs);
     tbs = srslte_ra_tbs_from_idx(tbs_idx, nof_prb);
     coderate = srslte_coderate(tbs, nof_re);
-  } while(sel_mcs > 0 && coderate >= max_coderate);
+    Qm = SRSLTE_MIN(max_Qm, srslte_mod_bits_x_symbol(srslte_ra_mod_from_mcs(sel_mcs)));
+    eff_coderate = coderate/Qm;
+  } while((sel_mcs > 0 && coderate > max_coderate) || eff_coderate > 0.930);
   if (mcs) {
     *mcs = (uint32_t) sel_mcs; 
   }
   return tbs; 
 }
 
-/* In this scheduler we tend to use all the available bandwidth and select the MCS 
+int sched_ue::alloc_tbs_dl(uint32_t nof_prb,
+                        uint32_t nof_re,
+                        uint32_t req_bytes,
+                        int *mcs)
+{
+  return alloc_tbs(nof_prb, nof_re, req_bytes, false, mcs);
+}
+
+int sched_ue::alloc_tbs_ul(uint32_t nof_prb,
+                           uint32_t nof_re,
+                           uint32_t req_bytes,
+                           int *mcs)
+{
+  return alloc_tbs(nof_prb, nof_re, req_bytes, true, mcs);
+}
+
+  /* In this scheduler we tend to use all the available bandwidth and select the MCS
  * that approximates the minimum between the capacity and the requested rate 
  */
-int sched_ue::alloc_tbs(uint32_t cqi, 
-                              uint32_t nof_prb, 
-                              uint32_t nof_re,
-                              uint32_t req_bytes, 
-                              uint32_t max_mcs, 
-                              int *mcs) 
-{
-  uint32_t sel_mcs = 0; 
-  int tbs = cqi_to_tbs(cqi, nof_prb, nof_re, max_mcs, &sel_mcs)/8;
+int sched_ue::alloc_tbs(uint32_t nof_prb,
+                        uint32_t nof_re,
+                        uint32_t req_bytes,
+                        bool is_ul,
+                        int *mcs)
+{
+  uint32_t sel_mcs = 0;
+
+  uint32_t cqi     = is_ul?ul_cqi:dl_cqi;
+  uint32_t max_mcs = is_ul?max_mcs_ul:max_mcs_dl;
+  uint32_t max_Qm  = is_ul?4:6; // Allow 16-QAM in PUSCH Only
+
+  int tbs = cqi_to_tbs(cqi, nof_prb, nof_re, max_mcs, max_Qm, &sel_mcs)/8;
   
   /* If less bytes are requested, lower the MCS */
   if (tbs > (int) req_bytes && req_bytes > 0) {
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index bc6aedc16..1e337b3ba 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -130,7 +130,7 @@ void parse_args(all_args_t *args, int argc, char* argv[]) {
         bpo::value<int>(&args->expert.mac.sched.pusch_mcs)->default_value(-1),
         "Optional fixed PUSCH MCS (ignores reported CQIs if specified)")
     ("scheduler.pusch_max_mcs",
-        bpo::value<int>(&args->expert.mac.sched.pusch_max_mcs)->default_value(16),
+        bpo::value<int>(&args->expert.mac.sched.pusch_max_mcs)->default_value(-1),
         "Optional PUSCH MCS limit")
     ("scheduler.nof_ctrl_symbols",
         bpo::value<int>(&args->expert.mac.sched.nof_ctrl_symbols)->default_value(3),
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 095a4643c..f10ebdfa8 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -49,6 +49,8 @@ using namespace std;
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
+#include <srslte/phy/phch/ra.h>
+
 void init_plots(srsenb::phch_worker *worker);
 pthread_t plot_thread; 
 sem_t plot_sem; 
@@ -363,7 +365,11 @@ int phch_worker::decode_pusch(srslte_enb_ul_pusch_t *grants, uint32_t nof_pusch,
       srslte_ra_ul_grant_t phy_grant; 
       int res = -1;
       if (!srslte_ra_ul_dci_to_grant(&grants[i].grant, enb_ul.cell.nof_prb, n_rb_ho, &phy_grant, tti%8)) {
-        res = srslte_enb_ul_get_pusch(&enb_ul, &phy_grant, grants[i].softbuffer, 
+        if (phy_grant.mcs.mod == SRSLTE_MOD_64QAM) {
+          phy_grant.mcs.mod = SRSLTE_MOD_16QAM;
+        }
+        phy_grant.Qm = SRSLTE_MIN(phy_grant.Qm, 4);
+        res = srslte_enb_ul_get_pusch(&enb_ul, &phy_grant, grants[i].softbuffer,
                                                 rnti, grants[i].rv_idx, 
                                                 grants[i].current_tx_nb, 
                                                 grants[i].data, 

From bbf7ffffa21050ed58b7778ab074132f6ae11cb8 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 09:59:06 +0200
Subject: [PATCH 103/269] Added Condition number calculation for Rank Indicator
 in TM3

(cherry picked from commit f8a4472)
---
 lib/examples/pdsch_ue.c                 |   8 +-
 lib/include/srslte/phy/mimo/precoding.h |   7 +
 lib/include/srslte/phy/phch/pdsch.h     |   5 +
 lib/include/srslte/phy/ue/ue_dl.h       |   4 +
 lib/include/srslte/phy/utils/algebra.h  |   6 +
 lib/src/phy/mimo/precoding.c            |  34 ++++
 lib/src/phy/mimo/test/pmi_select_test.c |  14 ++
 lib/src/phy/mimo/test/pmi_select_test.h | 232 ++++++++++++++----------
 lib/src/phy/phch/pdsch.c                |   5 +
 lib/src/phy/ue/ue_dl.c                  |  26 ++-
 lib/src/phy/utils/algebra.c             |  23 +++
 11 files changed, 261 insertions(+), 103 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 7fc020118..be271049e 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -513,7 +513,7 @@ int main(int argc, char **argv) {
   uint32_t nframes=0;
   uint32_t ri = 0, pmi = 0;
   float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
-      sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS];
+      sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
 
   for (int i = 0; i < SRSLTE_MAX_LAYERS; i++) {
@@ -678,11 +678,14 @@ int main(int argc, char **argv) {
                      100 * (1 - (float) ue_dl.nof_detected / nof_trials),
                      (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
             } else {
+              /* Compute condition number */
+              srslte_ue_dl_ri_select(&ue_dl, NULL, &cn);
+
               /* Compute Rank Indicator (RI) and Precoding Matrix Indicator (PMI) */
               srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, NULL);
               for (uint32_t nl = 0; nl < SRSLTE_MAX_LAYERS; nl++) {
                 for (uint32_t cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb ++) {
-                  sinr[nl][cb] = SRSLTE_VEC_EMA(ue_dl.sinr[nl][cb], sinr[nl][cb], 0.05f);
+                  sinr[nl][cb] = SRSLTE_VEC_EMA(ue_dl.sinr[nl][cb], sinr[nl][cb], 0.5f);
                 }
               }
 
@@ -703,6 +706,7 @@ int main(int argc, char **argv) {
               printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
               printf("\033[K         TB 0: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[0].idx, ue_dl.pdsch_cfg.grant.mcs[0].tbs);
               printf("\033[K         TB 1: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[1].idx, ue_dl.pdsch_cfg.grant.mcs[1].tbs);
+              printf("\033[K            κ: %.1f dB (Condition number, 0 dB => Best)\n", cn);
               printf("\033[K\n");
               printf("\033[KSINR (dB) Vs RI and PMI (for TM4, close loop MIMO only):\n");
               printf("\033[K   | RI |   1   |   2   |\n");
diff --git a/lib/include/srslte/phy/mimo/precoding.h b/lib/include/srslte/phy/mimo/precoding.h
index b32b975d2..58fe22a40 100644
--- a/lib/include/srslte/phy/mimo/precoding.h
+++ b/lib/include/srslte/phy/mimo/precoding.h
@@ -127,4 +127,11 @@ SRSLTE_API int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_
                                            uint32_t *pmi,
                                            float sinr[SRSLTE_MAX_CODEBOOKS]);
 
+SRSLTE_API int srslte_precoding_cn(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                   uint32_t nof_tx_antennas,
+                                   uint32_t nof_rx_antennas,
+                                   uint32_t nof_symbols,
+                                   float *cn);
+
+
 #endif /* PRECODING_H_ */
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7cfa1b616..ca6ce3478 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -156,6 +156,11 @@ SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                        uint32_t pmi[SRSLTE_MAX_LAYERS],
                                        float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]);
 
+SRSLTE_API int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
+                                       cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                       uint32_t nof_ce,
+                                       float *cn);
+
 SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter);
 
 SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q);
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 97b2344f5..416d467f8 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -200,6 +200,10 @@ SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           uint32_t *pmi,
                                           float *current_sinr);
 
+SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
+                                      uint32_t *ri,
+                                      float *cn);
+
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
                                           uint32_t sf_idx, 
                                           uint32_t n_prb_lowest, 
diff --git a/lib/include/srslte/phy/utils/algebra.h b/lib/include/srslte/phy/utils/algebra.h
index 8e731a29f..ee681d558 100644
--- a/lib/include/srslte/phy/utils/algebra.h
+++ b/lib/include/srslte/phy/utils/algebra.h
@@ -108,6 +108,12 @@ SRSLTE_API void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1,
                                             float noise_estimate,
                                             float norm);
 
+SRSLTE_API float srslte_algebra_2x2_cn(cf_t h00,
+                                       cf_t h01,
+                                       cf_t h10,
+                                       cf_t h11);
+
+
 #ifdef LV_HAVE_SSE
 
 /* SSE implementation for complex reciprocal */
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index a79b75b0a..94b60c033 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -2517,3 +2517,37 @@ int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uin
 
   return ret;
 }
+
+/* PMI Select for 1 layer */
+float srslte_precoding_2x2_cn_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols) {
+  uint32_t count = 0;
+  float cn_avg = 0.0f;
+
+  for (uint32_t i = 0; i < nof_symbols; i += PMI_SEL_PRECISION) {
+    /* 0. Load channel matrix */
+    cf_t h00 = h[0][0][i];
+    cf_t h01 = h[1][0][i];
+    cf_t h10 = h[0][1][i];
+    cf_t h11 = h[1][1][i];
+
+    cn_avg += srslte_algebra_2x2_cn(h00, h01, h10, h11);
+
+    count++;
+  }
+
+  return cn_avg/count;
+}
+
+/* Computes the condition number for a given number of antennas,
+ * stores in the parameter *cn the Condition Number in dB */
+int srslte_precoding_cn(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_tx_antennas,
+                          uint32_t nof_rx_antennas, uint32_t nof_symbols, float *cn) {
+  if (nof_tx_antennas == 2 && nof_rx_antennas == 2) {
+    *cn = srslte_precoding_2x2_cn_gen(h, nof_symbols);
+    return SRSLTE_SUCCESS;
+  } else {
+    ERROR("MIMO Condition Number calculation not implemented for %d×%d", nof_tx_antennas, nof_rx_antennas);
+    return SRSLTE_ERROR;
+  }
+}
+
diff --git a/lib/src/phy/mimo/test/pmi_select_test.c b/lib/src/phy/mimo/test/pmi_select_test.c
index 30d358e37..9ac45f008 100644
--- a/lib/src/phy/mimo/test/pmi_select_test.c
+++ b/lib/src/phy/mimo/test/pmi_select_test.c
@@ -45,6 +45,7 @@ int main(int argc, char **argv) {
   float noise_estimate;
   float sinr_1l[SRSLTE_MAX_CODEBOOKS];
   float sinr_2l[SRSLTE_MAX_CODEBOOKS];
+  float cn;
   uint32_t pmi[2];
   uint32_t nof_symbols = (uint32_t) SRSLTE_SF_LEN_RE(6, SRSLTE_CP_NORM);
   int ret = SRSLTE_ERROR;
@@ -120,6 +121,19 @@ int main(int argc, char **argv) {
       ERROR("Test case %d failed computing 2 layer PMI (test=%d; gold=%d)\n", c + 1, pmi[1], gold->pmi[1]);
       goto clean;
     }
+
+    /* Condition number */
+    if (srslte_precoding_cn(h, 2, 2, nof_symbols, &cn)) {
+      ERROR("Test case %d condition number returned error\n");
+      goto clean;
+    }
+
+    /* Check condition number */
+    if (fabsf(gold->k - cn) > 0.1) {
+      ERROR("Test case %d failed computing condition number (test=%.2f; gold=%.2f)\n",
+            c + 1, cn, gold->k);
+      goto clean;
+    }
   }
 
   /* Test passed */
diff --git a/lib/src/phy/mimo/test/pmi_select_test.h b/lib/src/phy/mimo/test/pmi_select_test.h
index a55869a8b..14c72c266 100644
--- a/lib/src/phy/mimo/test/pmi_select_test.h
+++ b/lib/src/phy/mimo/test/pmi_select_test.h
@@ -30,173 +30,207 @@
 #define PMI_SELECT_TEST_NOF_CASES 16
 
 typedef struct {
-  cf_t h[2][2];
-  float n;
-  float snri_1l[4];
-  float snri_2l[2];
-  uint32_t pmi[2];
+  cf_t h[2][2];     /* Channel estimate */
+  float n;          /* Noise estimation */
+  float snri_1l[4]; /* SINR Approximation for 1 layer (linear) */
+  float snri_2l[2]; /* SINR Approximation for 2 layers (linear) */
+  uint32_t pmi[2];  /* Precoding Matrix Indicator for 1 and 2 layers */
+  uint32_t ri;      /* Rank indicator */
+  float k;          /* Condition number (κ) in dB */
 } pmi_select_test_case_gold_t;
 
 static pmi_select_test_case_gold_t pmi_select_test_case_gold [PMI_SELECT_TEST_NOF_CASES] = {
   { /* Test case 1 */
     .h = {
-      {+0.103430f+0.455920f*_Complex_I, +0.042050f+0.751883f*_Complex_I}, 
-      {-0.641951f-0.053356f*_Complex_I, +0.217847f+0.504428f*_Complex_I}
+      {+0.861552f+0.205546f*_Complex_I, +0.955538f-0.052364f*_Complex_I}, 
+      {-0.812807f-0.287487f*_Complex_I, +0.323470f-0.048843f*_Complex_I}
       },
-    .n = 0.181048, 
-    .snri_1l = {5.793827f, 2.505115f, 3.350922f, 4.948020f},
-    .snri_2l = {2.015732f, 1.848130f},
-    .pmi = {0, 0},
+    .n = 0.671022, 
+    .snri_1l = {1.233339f, 2.567786f, 2.065012f, 1.736113f},
+    .snri_2l = {0.893739f, 0.841600f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 4.6758,
   },
   { /* Test case 2 */
     .h = {
-      {-0.957328f-0.624816f*_Complex_I, -0.741457f-0.657570f*_Complex_I}, 
-      {+0.720104f+0.351137f*_Complex_I, +0.593419f-0.200211f*_Complex_I}
+      {+0.919290f+0.824394f*_Complex_I, -0.821831f-0.797741f*_Complex_I}, 
+      {+0.595485f-0.413411f*_Complex_I, +0.181551f-0.896824f*_Complex_I}
       },
-    .n = 0.935527, 
-    .snri_1l = {0.475069f, 3.077055f, 1.078656f, 2.473467f},
-    .snri_2l = {0.747362f, 0.594324f},
-    .pmi = {1, 0},
+    .n = 0.504128, 
+    .snri_1l = {5.697916f, 2.631874f, 4.143253f, 4.186538f},
+    .snri_2l = {1.897704f, 1.718554f},
+    .pmi = {0, 0},
+    .ri = 2,
+    .k = 4.8715,
   },
   { /* Test case 3 */
     .h = {
-      {-0.047530f-0.118039f*_Complex_I, -0.195528f-0.724032f*_Complex_I}, 
-      {-0.619953f+0.960678f*_Complex_I, -0.325868f-0.120700f*_Complex_I}
+      {+0.536752f-0.093497f*_Complex_I, -0.434029f+0.474769f*_Complex_I}, 
+      {-0.549280f+0.019771f*_Complex_I, -0.337421f-0.234972f*_Complex_I}
       },
-    .n = 0.803842, 
-    .snri_1l = {1.331730f, 1.164592f, 1.660155f, 0.836167f},
-    .snri_2l = {0.554942f, 0.579321f},
-    .pmi = {2, 1},
+    .n = 0.905483, 
+    .snri_1l = {0.363469f, 0.941684f, 0.408025f, 0.897129f},
+    .snri_2l = {0.311544f, 0.307074f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 6.2219,
   },
   { /* Test case 4 */
     .h = {
-      {+0.635330f-0.751786f*_Complex_I, -0.536944f-0.185884f*_Complex_I}, 
-      {+0.282517f-0.864615f*_Complex_I, -0.484380f-0.780479f*_Complex_I}
+      {+0.930515f-0.233960f*_Complex_I, +0.256535f+0.982387f*_Complex_I}, 
+      {-0.735938f-0.426345f*_Complex_I, +0.236604f+0.412383f*_Complex_I}
       },
-    .n = 0.529556, 
-    .snri_1l = {5.128973f, 0.465969f, 2.812367f, 2.782574f},
-    .snri_2l = {1.381190f, 0.818813f},
-    .pmi = {0, 0},
+    .n = 0.535206, 
+    .snri_1l = {2.487298f, 2.932872f, 4.009672f, 1.410498f},
+    .snri_2l = {1.082110f, 1.248639f},
+    .pmi = {2, 1},
+    .ri = 2,
+    .k = 5.9758,
   },
   { /* Test case 5 */
     .h = {
-      {-0.576996f+0.964470f*_Complex_I, -0.948065f+0.902764f*_Complex_I}, 
-      {+0.988240f-0.056784f*_Complex_I, +0.489282f+0.975071f*_Complex_I}
+      {-0.613578f-0.908683f*_Complex_I, +0.378870f+0.770083f*_Complex_I}, 
+      {-0.899090f+0.679589f*_Complex_I, -0.631132f-0.763690f*_Complex_I}
       },
-    .n = 0.852921, 
-    .snri_1l = {2.772684f, 3.261802f, 5.698031f, 0.336455f},
-    .snri_2l = {0.768370f, 1.469069f},
+    .n = 0.410415, 
+    .snri_1l = {2.929140f, 7.281276f, 7.632612f, 2.577804f},
+    .snri_2l = {2.045272f, 2.159393f},
     .pmi = {2, 1},
+    .ri = 2,
+    .k = 6.8485,
   },
   { /* Test case 6 */
     .h = {
-      {-0.381846f-0.998609f*_Complex_I, +0.903472f-0.393687f*_Complex_I}, 
-      {-0.772703f-0.261637f*_Complex_I, -0.765452f-0.759318f*_Complex_I}
+      {-0.759543f+0.979731f*_Complex_I, +0.144185f-0.300384f*_Complex_I}, 
+      {+0.898780f-0.582955f*_Complex_I, -0.487230f+0.331654f*_Complex_I}
       },
-    .n = 0.711912, 
-    .snri_1l = {2.998736f, 2.538860f, 5.099274f, 0.438323f},
-    .snri_2l = {0.809381f, 1.371548f},
-    .pmi = {2, 1},
+    .n = 0.973345, 
+    .snri_1l = {0.151784f, 3.077111f, 2.165454f, 1.063441f},
+    .snri_2l = {0.755981f, 0.503361f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 18.0800,
   },
   { /* Test case 7 */
     .h = {
-      {+0.915028f-0.780771f*_Complex_I, -0.355424f+0.447925f*_Complex_I}, 
-      {+0.577968f+0.765204f*_Complex_I, +0.342972f-0.999014f*_Complex_I}
+      {+0.245400f-0.537444f*_Complex_I, -0.872924f-0.895583f*_Complex_I}, 
+      {-0.252981f+0.803513f*_Complex_I, -0.667497f+0.586583f*_Complex_I}
       },
-    .n = 0.101944, 
-    .snri_1l = {12.424177f, 24.940449f, 5.411339f, 31.953288f},
-    .snri_2l = {4.610588f, 7.664146f},
-    .pmi = {3, 1},
+    .n = 0.373014, 
+    .snri_1l = {3.403660f, 5.744504f, 7.385295f, 1.762868f},
+    .snri_2l = {1.642126f, 2.130892f},
+    .pmi = {2, 1},
+    .ri = 2,
+    .k = 7.1484,
   },
   { /* Test case 8 */
     .h = {
-      {-0.259232f-0.654765f*_Complex_I, +0.829378f-0.793859f*_Complex_I}, 
-      {+0.997978f+0.212295f*_Complex_I, -0.659012f-0.176220f*_Complex_I}
+      {+0.664109f-0.281444f*_Complex_I, +0.507669f-0.822295f*_Complex_I}, 
+      {+0.243726f-0.316646f*_Complex_I, -0.211814f+0.097341f*_Complex_I}
       },
-    .n = 0.255783, 
-    .snri_1l = {3.345813f, 9.635433f, 6.767844f, 6.213402f},
-    .snri_2l = {3.215386f, 2.640976f},
-    .pmi = {1, 0},
+    .n = 0.460547, 
+    .snri_1l = {1.948731f, 1.673386f, 2.389606f, 1.232511f},
+    .snri_2l = {0.623422f, 0.659548f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 9.8582,
   },
   { /* Test case 9 */
     .h = {
-      {-0.596630f+0.244853f*_Complex_I, -0.624622f+0.316537f*_Complex_I}, 
-      {+0.767545f-0.645831f*_Complex_I, +0.262828f+0.251697f*_Complex_I}
+      {+0.290905f-0.072573f*_Complex_I, +0.027042f+0.179635f*_Complex_I}, 
+      {+0.628853f-0.625656f*_Complex_I, -0.805634f+0.222660f*_Complex_I}
       },
-    .n = 0.876456, 
-    .snri_1l = {0.367264f, 1.965908f, 1.215674f, 1.117498f},
-    .snri_2l = {0.579923f, 0.479609f},
-    .pmi = {1, 0},
+    .n = 0.051942, 
+    .snri_1l = {20.229536f, 10.736104f, 15.206118f, 15.759523f},
+    .snri_2l = {2.426844f, 2.175025f},
+    .pmi = {0, 0},
+    .ri = 1,
+    .k = 12.8396,
   },
   { /* Test case 10 */
     .h = {
-      {-0.643594f+0.172442f*_Complex_I, +0.291148f-0.026254f*_Complex_I}, 
-      {+0.768244f+0.678173f*_Complex_I, -0.498968f-0.896649f*_Complex_I}
+      {+0.151454f-0.701886f*_Complex_I, +0.684689f-0.943441f*_Complex_I}, 
+      {-0.000548f+0.513340f*_Complex_I, -0.121950f+0.592212f*_Complex_I}
       },
-    .n = 0.739473, 
-    .snri_1l = {1.104856f, 2.455074f, 2.920106f, 0.639825f},
-    .snri_2l = {0.557672f, 0.658911f},
-    .pmi = {2, 1},
+    .n = 0.293556, 
+    .snri_1l = {0.848833f, 7.679596f, 3.011330f, 5.517099f},
+    .snri_2l = {1.442768f, 0.788147f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 22.0345,
   },
   { /* Test case 11 */
     .h = {
-      {+0.109032f-0.285542f*_Complex_I, -0.141055f+0.318945f*_Complex_I}, 
-      {+0.559445f-0.211656f*_Complex_I, -0.206665f-0.643045f*_Complex_I}
+      {-0.769587f+0.330477f*_Complex_I, -0.249817f+0.920280f*_Complex_I}, 
+      {+0.657787f+0.886236f*_Complex_I, +0.683553f-0.774601f*_Complex_I}
       },
-    .n = 0.054295, 
-    .snri_1l = {8.472397f, 10.480333f, 4.074631f, 14.878099f},
-    .snri_2l = {2.121444f, 2.979095f},
-    .pmi = {3, 1},
+    .n = 0.648287, 
+    .snri_1l = {1.312873f, 4.697041f, 5.064183f, 0.945731f},
+    .snri_2l = {0.993660f, 1.125611f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 12.9593,
   },
   { /* Test case 12 */
     .h = {
-      {-0.505758f-0.710501f*_Complex_I, +0.803627f+0.023333f*_Complex_I}, 
-      {+0.964886f+0.987055f*_Complex_I, -0.031782f+0.525138f*_Complex_I}
+      {-0.038392f+0.542607f*_Complex_I, -0.866959f-0.879276f*_Complex_I}, 
+      {+0.795542f-0.475085f*_Complex_I, -0.005540f+0.302139f*_Complex_I}
       },
-    .n = 0.966024, 
-    .snri_1l = {0.612742f, 3.102514f, 1.227107f, 2.488149f},
-    .snri_2l = {0.848010f, 0.701000f},
-    .pmi = {1, 0},
+    .n = 0.133604, 
+    .snri_1l = {6.257962f, 14.479088f, 15.459994f, 5.277056f},
+    .snri_2l = {3.523645f, 3.845011f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 7.6196,
   },
   { /* Test case 13 */
     .h = {
-      {+0.859761f-0.737655f*_Complex_I, -0.527019f+0.509133f*_Complex_I}, 
-      {-0.804956f-0.303794f*_Complex_I, -0.180451f-0.100433f*_Complex_I}
+      {+0.277091f-0.237022f*_Complex_I, -0.230114f-0.399963f*_Complex_I}, 
+      {+0.531396f-0.319721f*_Complex_I, +0.305831f+0.837853f*_Complex_I}
       },
-    .n = 0.199335, 
-    .snri_1l = {4.402551f, 8.656756f, 10.092319f, 2.966987f},
-    .snri_2l = {2.048224f, 2.462759f},
-    .pmi = {2, 1},
+    .n = 0.456267, 
+    .snri_1l = {1.272387f, 2.072182f, 1.744873f, 1.599696f},
+    .snri_2l = {0.720254f, 0.700519f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 5.9972,
   },
   { /* Test case 14 */
     .h = {
-      {+0.473036f+0.227467f*_Complex_I, -0.593265f-0.308456f*_Complex_I}, 
-      {+0.536321f+0.445264f*_Complex_I, -0.517440f-0.765554f*_Complex_I}
+      {-0.115006f+0.764806f*_Complex_I, -0.091628f-0.960249f*_Complex_I}, 
+      {+0.890564f-0.316470f*_Complex_I, -0.561762f+0.532055f*_Complex_I}
       },
-    .n = 0.180788, 
-    .snri_1l = {10.671400f, 0.736020f, 3.584109f, 7.823311f},
-    .snri_2l = {2.029078f, 0.914443f},
-    .pmi = {0, 0},
+    .n = 0.342804, 
+    .snri_1l = {2.060602f, 6.750694f, 8.002153f, 0.809143f},
+    .snri_2l = {1.036665f, 1.575640f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 18.9097,
   },
   { /* Test case 15 */
     .h = {
-      {-0.612271f+0.338114f*_Complex_I, -0.278903f+0.914426f*_Complex_I}, 
-      {-0.191213f-0.136670f*_Complex_I, -0.548440f+0.607628f*_Complex_I}
+      {+0.237613f+0.203137f*_Complex_I, -0.093958f+0.298835f*_Complex_I}, 
+      {-0.979675f-0.314559f*_Complex_I, +0.198162f-0.013401f*_Complex_I}
       },
-    .n = 0.798189, 
-    .snri_1l = {2.309797f, 0.356735f, 0.731443f, 1.935089f},
-    .snri_2l = {0.577612f, 0.490806f},
-    .pmi = {0, 0},
+    .n = 0.701774, 
+    .snri_1l = {0.466961f, 1.376956f, 0.827475f, 1.016442f},
+    .snri_2l = {0.386935f, 0.354722f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 11.2469,
   },
   { /* Test case 16 */
     .h = {
-      {+0.990482f+0.513519f*_Complex_I, -0.576391f+0.922553f*_Complex_I}, 
-      {-0.341962f+0.139785f*_Complex_I, +0.524684f+0.217012f*_Complex_I}
+      {+0.775605f+0.528142f*_Complex_I, -0.889884f+0.975918f*_Complex_I}, 
+      {-0.803276f-0.749350f*_Complex_I, +0.299566f-0.271046f*_Complex_I}
       },
-    .n = 0.365092, 
-    .snri_1l = {2.942635f, 4.964827f, 4.761949f, 3.145513f},
-    .snri_2l = {1.291431f, 1.267123f},
+    .n = 0.676230, 
+    .snri_1l = {0.661772f, 5.245671f, 3.261470f, 2.645973f},
+    .snri_2l = {1.354145f, 0.857372f},
     .pmi = {1, 0},
+    .ri = 1,
+    .k = 10.7137,
   },
 };
 
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 0e824aaa7..bf50cb6ec 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -700,6 +700,11 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
   return SRSLTE_SUCCESS;
 }
 
+int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
+                            cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_ce, float *cn) {
+  return srslte_precoding_cn(ce, q->cell.nof_ports, q->nof_rx_antennas, nof_ce, cn);
+}
+
 int srslte_pdsch_encode(srslte_pdsch_t *q,
                         srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
                         uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 219325c1d..91116ab70 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -467,6 +467,8 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   }
 }
 
+/* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
+ * Noise Ratio (SINR), valid for TM4 */
 int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
   float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
   float best_sinr = -INFINITY;
@@ -484,8 +486,8 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
     for (uint32_t nof_layers = 1; nof_layers <= q->pdsch_cfg.nof_layers; nof_layers++ ) {
-      if (q->sinr[nof_layers][q->pmi[nof_layers]] > best_sinr) {
-        best_sinr = q->sinr[nof_layers][q->pmi[nof_layers]];
+      if (q->sinr[nof_layers][q->pmi[nof_layers]] > best_sinr + 0.1) {
+        best_sinr = q->sinr[nof_layers][q->pmi[nof_layers]]*nof_layers;
         best_pmi = q->pmi[nof_layers];
         best_ri = nof_layers;
       }
@@ -526,6 +528,26 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
   return SRSLTE_SUCCESS;
 }
 
+
+/* Compute the Rank Indicator (RI) by computing the condition number, valid for TM3 */
+int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
+  float _cn;
+  int ret = srslte_pdsch_cn_compute(&q->pdsch, q->ce_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), &_cn);
+
+  /* Set Condition number */
+  if (cn) {
+    *cn = _cn;
+  }
+
+  /* Set rank indicator */
+  if (!ret && ri) {
+    *ri = (_cn > 3.0f)? 1:0;
+  }
+
+  return ret;
+}
+
+
 uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q) {
   return q->last_location.ncce; 
 }
diff --git a/lib/src/phy/utils/algebra.c b/lib/src/phy/utils/algebra.c
index 3d1de2ffa..943ae57dd 100644
--- a/lib/src/phy/utils/algebra.c
+++ b/lib/src/phy/utils/algebra.c
@@ -26,6 +26,7 @@
 
 #include <complex.h>
 #include <immintrin.h>
+#include <math.h>
 
 #include "srslte/phy/utils/algebra.h"
 
@@ -92,6 +93,28 @@ inline void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf
   *x1 = (y0 * w10 + y1 * w11) * _norm;
 }
 
+inline float srslte_algebra_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
+  /* 1. A = H * H' (A = A') */
+  float a00 =
+      crealf(h00) * crealf(h00) + crealf(h01) * crealf(h01) + cimagf(h00) * cimagf(h00) + cimagf(h01) * cimagf(h01);
+  cf_t a01 = h00 * conjf(h10) + h01 * conjf(h11);
+  //cf_t a10 = h10*conjf(h00) + h11*conjf(h01) = conjf(a01);
+  float a11 =
+      crealf(h10) * crealf(h10) + crealf(h11) * crealf(h11) + cimagf(h10) * cimagf(h10) + cimagf(h11) * cimagf(h11);
+
+  /* 2. |H * H' - {λ0, λ1}| = 0 -> aλ² + bλ + c = 0 */
+  float b = a00 + a11;
+  float c = a00 * a11 - (crealf(a01) * crealf(a01) + cimagf(a01) * cimagf(a01));
+
+  /* 3. λ = (-b ± sqrt(b² - 4 * c))/2 */
+  float sqr = sqrtf(b * b - 4.0f * c);
+  float xmax = b + sqr;
+  float xmin = b - sqr;
+
+  /* 4. κ = sqrt(λ_max / λ_min) */
+  return 10 * log10f(xmax / xmin);
+}
+
 #ifdef LV_HAVE_SSE
 
 /* SSE implementation for complex reciprocal */

From 9deadc13dc79c44d7cdebc0e699ca889d0299cc3 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 12:00:52 +0200
Subject: [PATCH 104/269] Tx and Rx data check in PDSCH test

(cherry picked from commit 6679a160c6f39876aa8d5930a1c8d6bc58a551d3)

(cherry picked from commit 8ae673d)
---
 lib/src/phy/phch/test/pdsch_test.c | 58 ++++++++++++++++++++----------
 1 file changed, 40 insertions(+), 18 deletions(-)

diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 228cb5eb5..ea40ce824 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -133,7 +133,8 @@ void parse_args(int argc, char **argv) {
   }
 }
 
-uint8_t *data[SRSLTE_MAX_CODEWORDS] = {NULL};
+static uint8_t *data_tx[SRSLTE_MAX_CODEWORDS] = {NULL};
+static uint8_t *data_rx[SRSLTE_MAX_CODEWORDS] = {NULL};
 cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
 srslte_softbuffer_rx_t *softbuffers_rx[SRSLTE_MAX_CODEWORDS];
 srslte_ra_dl_grant_t grant; 
@@ -258,12 +259,20 @@ int main(int argc, char **argv) {
 
   for (int i = 0; i < grant.nof_tb; i++) {
     if (grant.mcs[i].tbs) {
-      data[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
-      if (!data[i]) {
+      data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_tx[i]) {
         perror("srslte_vec_malloc");
         goto quit;
       }
-      bzero(data[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+      bzero(data_tx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
+      data_rx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_rx[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data_rx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
     }
   }
 
@@ -274,7 +283,7 @@ int main(int argc, char **argv) {
 
   srslte_pdsch_set_rnti(&pdsch_rx, rnti);
 
-  for (i = 0; i < SRSLTE_MAX_TB; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     softbuffers_rx[i] = calloc(sizeof(srslte_softbuffer_rx_t), 1);
     if (!softbuffers_rx[i]) {
       fprintf(stderr, "Error allocating RX soft buffer\n");
@@ -322,7 +331,7 @@ int main(int argc, char **argv) {
 #ifdef DO_OFDM
     srslte_filesource_read(&fsrc, rx_slot_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
 #else
-    srslte_filesource_read_multi(&fsrc, (void*) rx_slot_symbols, SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp), pdsch_cfg.nof_layers);
+    srslte_filesource_read(&fsrc, rx_slot_symbols[0], SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
 #endif
     
     srslte_chest_dl_t chest; 
@@ -330,7 +339,7 @@ int main(int argc, char **argv) {
       fprintf(stderr, "Error initializing equalizer\n");
       exit(-1);
     }
-    srslte_chest_dl_estimate_multi(&chest, rx_slot_symbols, ce, subframe, nof_rx_antennas);
+    srslte_chest_dl_estimate(&chest, rx_slot_symbols[0], ce[0], subframe);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
@@ -345,7 +354,6 @@ int main(int argc, char **argv) {
 
     for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
       softbuffers_tx[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
-
       if (!softbuffers_tx[i]) {
         fprintf(stderr, "Error allocating TX soft buffer\n");
       }
@@ -364,9 +372,9 @@ int main(int argc, char **argv) {
       }
     }
 
-    for (i = 0; i< grant.nof_tb; i++) {
-      for (i = 0; i < grant.mcs[i].tbs / 8; i++) {
-        data[i][i] = (uint8_t) (rand() % 256);
+    for (int tb = 0; tb < grant.nof_tb; tb++) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        data_tx[tb][byte] = (uint8_t)(rand() % 256);
       }
     }
 
@@ -377,7 +385,7 @@ int main(int argc, char **argv) {
     if (rv_idx[0] != 0 || rv_idx[1] != 0) {
       /* Do 1st transmission for rv_idx!=0 */
       bzero(pdsch_cfg.rv, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
-      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
+      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
         fprintf(stderr, "Error encoding PDSCH\n");
         goto quit;
       }
@@ -385,7 +393,7 @@ int main(int argc, char **argv) {
     memcpy(pdsch_cfg.rv, rv_idx, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
     gettimeofday(&t[1], NULL);
     for (k = 0; k < M; k++) {
-      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data, rnti, tx_slot_symbols)) {
+      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
         ERROR("Error encoding PDSCH");
         goto quit;
       }
@@ -441,7 +449,7 @@ int main(int argc, char **argv) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
-    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data, acks);
+    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data_rx, acks);
   }
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
@@ -451,11 +459,21 @@ int main(int argc, char **argv) {
 
   /* If there is an error in PDSCH decode */
   if (r) {
-    ERROR("PDSCH decode");
-    ret = SRSLTE_ERROR;
+    ret = -1;
     goto quit;
   }
 
+  /* Check Tx and Rx bytes */
+  for (int tb = 0; tb < grant.nof_tb; tb++) {
+    for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+      if (data_tx[tb][byte] != data_rx[tb][byte]) {
+        ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+        ret = SRSLTE_ERROR;
+        goto quit;
+      }
+    }
+  }
+
   /* Check all transport blocks have been decoded OK */
   for (int tb = 0; tb < grant.nof_tb; tb++) {
     ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
@@ -477,8 +495,12 @@ quit:
       free(softbuffers_rx[i]);
     }
 
-    if (data[i]) {
-      free(data[i]);
+    if (data_tx[i]) {
+      free(data_tx[i]);
+    }
+
+    if (data_rx[i]) {
+      free(data_rx[i]);
     }
   }
 

From 38b38ee82f3fce7baf84d1414ad556c04c5eb4a6 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 13:01:15 +0200
Subject: [PATCH 105/269] Solved bug in RF Send call

(cherry picked from commit cc8a10e)
---
 lib/src/phy/rf/rf_imp.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index cb22b301b..c1074ebc5 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -323,7 +323,8 @@ int srslte_rf_send_multi(srslte_rf_t *rf,
                      bool is_end_of_burst)
 {
 
-  return srslte_rf_send_timed_multi(rf, data, nsamples, 0, 0, blocking, is_start_of_burst, is_end_of_burst);
+  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, 0, 0,
+                                                           false, blocking, is_start_of_burst, is_end_of_burst);
 }
 
 int srslte_rf_send(srslte_rf_t *rf, void *data, uint32_t nsamples, bool blocking)

From d9d72e8f7b3d20542400d4f7df887868d3f535ba Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 16:53:39 +0200
Subject: [PATCH 106/269] SINR calculation for PMI selection improvement for 2
 layers; test golden values updated

(cherry picked from commit 59b26dd)
---
 lib/src/phy/mimo/precoding.c            |   6 +-
 lib/src/phy/mimo/test/pmi_select_test.c |   2 +-
 lib/src/phy/mimo/test/pmi_select_test.h | 236 ++++++++++++------------
 3 files changed, 122 insertions(+), 122 deletions(-)

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 94b60c033..0e898448a 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -2195,7 +2195,7 @@ int srslte_precoding_pmi_select_2l_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
     }
 
     /* Divide average by noise */
-    sinr_list[i] /= (2 * count);
+    sinr_list[i] /= count;
 
     if (sinr_list[i] > max_sinr) {
       max_sinr = sinr_list[i];
@@ -2316,7 +2316,7 @@ int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
     }
 
     /* Divide average by noise */
-    sinr_list[i] /= 2 * count;
+    sinr_list[i] /= count;
 
     if (sinr_list[i] > max_sinr) {
       max_sinr = sinr_list[i];
@@ -2463,7 +2463,7 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
     }
 
     /* Divide average by noise */
-    sinr_list[i] /= 2 * count;
+    sinr_list[i] /= count;
 
     if (sinr_list[i] > max_sinr) {
       max_sinr = sinr_list[i];
diff --git a/lib/src/phy/mimo/test/pmi_select_test.c b/lib/src/phy/mimo/test/pmi_select_test.c
index 9ac45f008..c7e5b5c8e 100644
--- a/lib/src/phy/mimo/test/pmi_select_test.c
+++ b/lib/src/phy/mimo/test/pmi_select_test.c
@@ -155,4 +155,4 @@ int main(int argc, char **argv) {
   }
 
   return ret;
-}
\ No newline at end of file
+}
diff --git a/lib/src/phy/mimo/test/pmi_select_test.h b/lib/src/phy/mimo/test/pmi_select_test.h
index 14c72c266..c9c2ef5b3 100644
--- a/lib/src/phy/mimo/test/pmi_select_test.h
+++ b/lib/src/phy/mimo/test/pmi_select_test.h
@@ -42,195 +42,195 @@ typedef struct {
 static pmi_select_test_case_gold_t pmi_select_test_case_gold [PMI_SELECT_TEST_NOF_CASES] = {
   { /* Test case 1 */
     .h = {
-      {+0.861552f+0.205546f*_Complex_I, +0.955538f-0.052364f*_Complex_I}, 
-      {-0.812807f-0.287487f*_Complex_I, +0.323470f-0.048843f*_Complex_I}
+      {+0.626226f+0.060103f*_Complex_I, -0.233387f-0.449860f*_Complex_I}, 
+      {+0.234558f-0.502742f*_Complex_I, +0.150990f-0.096722f*_Complex_I}
       },
-    .n = 0.671022, 
-    .snri_1l = {1.233339f, 2.567786f, 2.065012f, 1.736113f},
-    .snri_2l = {0.893739f, 0.841600f},
-    .pmi = {1, 0},
-    .ri = 2,
-    .k = 4.6758,
+    .n = 0.227713, 
+    .snri_1l = {2.728043f, 1.630673f, 3.226421f, 1.132295f},
+    .snri_2l = {1.797660f, 1.982149f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 6.4007,
   },
   { /* Test case 2 */
     .h = {
-      {+0.919290f+0.824394f*_Complex_I, -0.821831f-0.797741f*_Complex_I}, 
-      {+0.595485f-0.413411f*_Complex_I, +0.181551f-0.896824f*_Complex_I}
+      {+0.608899f-0.825846f*_Complex_I, +0.972208f+0.604183f*_Complex_I}, 
+      {-0.940016f+0.978290f*_Complex_I, +0.071328f-0.866107f*_Complex_I}
       },
-    .n = 0.504128, 
-    .snri_1l = {5.697916f, 2.631874f, 4.143253f, 4.186538f},
-    .snri_2l = {1.897704f, 1.718554f},
-    .pmi = {0, 0},
-    .ri = 2,
-    .k = 4.8715,
+    .n = 0.939398, 
+    .snri_1l = {0.686850f, 4.591972f, 3.773925f, 1.504897f},
+    .snri_2l = {2.298235f, 1.761859f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 11.1305,
   },
   { /* Test case 3 */
     .h = {
-      {+0.536752f-0.093497f*_Complex_I, -0.434029f+0.474769f*_Complex_I}, 
-      {-0.549280f+0.019771f*_Complex_I, -0.337421f-0.234972f*_Complex_I}
+      {-0.963645f+0.770719f*_Complex_I, +0.367677f+0.798010f*_Complex_I}, 
+      {+0.567473f+0.251875f*_Complex_I, +0.068275f-0.724262f*_Complex_I}
       },
-    .n = 0.905483, 
-    .snri_1l = {0.363469f, 0.941684f, 0.408025f, 0.897129f},
-    .snri_2l = {0.311544f, 0.307074f},
-    .pmi = {1, 0},
+    .n = 0.217802, 
+    .snri_1l = {3.209674f, 11.525338f, 11.962786f, 2.772226f},
+    .snri_2l = {3.226053f, 3.526363f},
+    .pmi = {2, 1},
     .ri = 1,
-    .k = 6.2219,
+    .k = 15.4589,
   },
   { /* Test case 4 */
     .h = {
-      {+0.930515f-0.233960f*_Complex_I, +0.256535f+0.982387f*_Complex_I}, 
-      {-0.735938f-0.426345f*_Complex_I, +0.236604f+0.412383f*_Complex_I}
+      {-0.635718f+0.879322f*_Complex_I, -0.916360f-0.291089f*_Complex_I}, 
+      {-0.786117f-0.178742f*_Complex_I, +0.232887f+0.968699f*_Complex_I}
       },
-    .n = 0.535206, 
-    .snri_1l = {2.487298f, 2.932872f, 4.009672f, 1.410498f},
-    .snri_2l = {1.082110f, 1.248639f},
-    .pmi = {2, 1},
+    .n = 0.945579, 
+    .snri_1l = {1.818313f, 2.141519f, 1.995787f, 1.964045f},
+    .snri_2l = {1.965011f, 1.958537f},
+    .pmi = {1, 0},
     .ri = 2,
-    .k = 5.9758,
+    .k = 1.2910,
   },
   { /* Test case 5 */
     .h = {
-      {-0.613578f-0.908683f*_Complex_I, +0.378870f+0.770083f*_Complex_I}, 
-      {-0.899090f+0.679589f*_Complex_I, -0.631132f-0.763690f*_Complex_I}
+      {+0.353289f+0.324764f*_Complex_I, +0.976605f-0.511669f*_Complex_I}, 
+      {+0.533663f-0.408985f*_Complex_I, -0.326601f+0.360357f*_Complex_I}
       },
-    .n = 0.410415, 
-    .snri_1l = {2.929140f, 7.281276f, 7.632612f, 2.577804f},
-    .snri_2l = {2.045272f, 2.159393f},
-    .pmi = {2, 1},
+    .n = 0.527847, 
+    .snri_1l = {1.173803f, 2.869865f, 2.273783f, 1.769885f},
+    .snri_2l = {1.871430f, 1.713879f},
+    .pmi = {1, 0},
     .ri = 2,
-    .k = 6.8485,
+    .k = 5.5388,
   },
   { /* Test case 6 */
     .h = {
-      {-0.759543f+0.979731f*_Complex_I, +0.144185f-0.300384f*_Complex_I}, 
-      {+0.898780f-0.582955f*_Complex_I, -0.487230f+0.331654f*_Complex_I}
+      {-0.176813f+0.103585f*_Complex_I, +0.205276f+0.167141f*_Complex_I}, 
+      {+0.501040f+0.023640f*_Complex_I, +0.167066f-0.834815f*_Complex_I}
       },
-    .n = 0.973345, 
-    .snri_1l = {0.151784f, 3.077111f, 2.165454f, 1.063441f},
-    .snri_2l = {0.755981f, 0.503361f},
-    .pmi = {1, 0},
+    .n = 0.719570, 
+    .snri_1l = {0.490387f, 1.022313f, 1.111245f, 0.401456f},
+    .snri_2l = {0.578124f, 0.597176f},
+    .pmi = {2, 1},
     .ri = 1,
-    .k = 18.0800,
+    .k = 21.8808,
   },
   { /* Test case 7 */
     .h = {
-      {+0.245400f-0.537444f*_Complex_I, -0.872924f-0.895583f*_Complex_I}, 
-      {-0.252981f+0.803513f*_Complex_I, -0.667497f+0.586583f*_Complex_I}
+      {+0.992312f+0.773088f*_Complex_I, -0.290931f-0.090610f*_Complex_I}, 
+      {+0.942518f-0.173145f*_Complex_I, -0.307102f-0.564536f*_Complex_I}
       },
-    .n = 0.373014, 
-    .snri_1l = {3.403660f, 5.744504f, 7.385295f, 1.762868f},
-    .snri_2l = {1.642126f, 2.130892f},
-    .pmi = {2, 1},
-    .ri = 2,
-    .k = 7.1484,
+    .n = 0.125655, 
+    .snri_1l = {19.459529f, 4.467420f, 18.044021f, 5.882928f},
+    .snri_2l = {8.055238f, 6.832247f},
+    .pmi = {0, 0},
+    .ri = 1,
+    .k = 9.9136,
   },
   { /* Test case 8 */
     .h = {
-      {+0.664109f-0.281444f*_Complex_I, +0.507669f-0.822295f*_Complex_I}, 
-      {+0.243726f-0.316646f*_Complex_I, -0.211814f+0.097341f*_Complex_I}
+      {-0.382171f-0.980395f*_Complex_I, +0.452209f+0.686427f*_Complex_I}, 
+      {+0.565744f+0.844664f*_Complex_I, +0.387575f+0.541908f*_Complex_I}
       },
-    .n = 0.460547, 
-    .snri_1l = {1.948731f, 1.673386f, 2.389606f, 1.232511f},
-    .snri_2l = {0.623422f, 0.659548f},
-    .pmi = {2, 1},
-    .ri = 1,
-    .k = 9.8582,
+    .n = 0.042660, 
+    .snri_1l = {26.560881f, 49.864772f, 33.269985f, 43.155668f},
+    .snri_2l = {37.201526f, 34.461078f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 3.1172,
   },
   { /* Test case 9 */
     .h = {
-      {+0.290905f-0.072573f*_Complex_I, +0.027042f+0.179635f*_Complex_I}, 
-      {+0.628853f-0.625656f*_Complex_I, -0.805634f+0.222660f*_Complex_I}
+      {-0.243628f-0.461891f*_Complex_I, +0.408679f+0.346062f*_Complex_I}, 
+      {+0.459026f-0.045016f*_Complex_I, -0.551446f+0.247433f*_Complex_I}
       },
-    .n = 0.051942, 
-    .snri_1l = {20.229536f, 10.736104f, 15.206118f, 15.759523f},
-    .snri_2l = {2.426844f, 2.175025f},
-    .pmi = {0, 0},
+    .n = 0.236445, 
+    .snri_1l = {1.429443f, 3.381496f, 0.227617f, 4.583322f},
+    .snri_2l = {1.272903f, 2.118832f},
+    .pmi = {3, 1},
     .ri = 1,
-    .k = 12.8396,
+    .k = 24.1136,
   },
   { /* Test case 10 */
     .h = {
-      {+0.151454f-0.701886f*_Complex_I, +0.684689f-0.943441f*_Complex_I}, 
-      {-0.000548f+0.513340f*_Complex_I, -0.121950f+0.592212f*_Complex_I}
+      {-0.645752f-0.784222f*_Complex_I, +0.659287f-0.635545f*_Complex_I}, 
+      {+0.533843f-0.801809f*_Complex_I, +0.868957f-0.020472f*_Complex_I}
       },
-    .n = 0.293556, 
-    .snri_1l = {0.848833f, 7.679596f, 3.011330f, 5.517099f},
-    .snri_2l = {1.442768f, 0.788147f},
-    .pmi = {1, 0},
+    .n = 0.193245, 
+    .snri_1l = {13.697372f, 4.693597f, 1.561737f, 16.829232f},
+    .snri_2l = {2.961344f, 5.773049f},
+    .pmi = {3, 1},
     .ri = 1,
-    .k = 22.0345,
+    .k = 17.5194,
   },
   { /* Test case 11 */
     .h = {
-      {-0.769587f+0.330477f*_Complex_I, -0.249817f+0.920280f*_Complex_I}, 
-      {+0.657787f+0.886236f*_Complex_I, +0.683553f-0.774601f*_Complex_I}
+      {+0.791783f+0.544990f*_Complex_I, -0.801821f-0.376120f*_Complex_I}, 
+      {-0.911669f-0.642035f*_Complex_I, +0.114590f-0.322089f*_Complex_I}
       },
-    .n = 0.648287, 
-    .snri_1l = {1.312873f, 4.697041f, 5.064183f, 0.945731f},
-    .snri_2l = {0.993660f, 1.125611f},
-    .pmi = {2, 1},
-    .ri = 1,
-    .k = 12.9593,
+    .n = 0.210146, 
+    .snri_1l = {2.340213f, 12.261749f, 5.921675f, 8.680286f},
+    .snri_2l = {6.912040f, 4.520201f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 7.7819,
   },
   { /* Test case 12 */
     .h = {
-      {-0.038392f+0.542607f*_Complex_I, -0.866959f-0.879276f*_Complex_I}, 
-      {+0.795542f-0.475085f*_Complex_I, -0.005540f+0.302139f*_Complex_I}
+      {+0.020305f-0.218290f*_Complex_I, +0.812729f-0.890767f*_Complex_I}, 
+      {+0.257848f+0.002566f*_Complex_I, -0.796932f-0.136558f*_Complex_I}
       },
-    .n = 0.133604, 
-    .snri_1l = {6.257962f, 14.479088f, 15.459994f, 5.277056f},
-    .snri_2l = {3.523645f, 3.845011f},
+    .n = 0.997560, 
+    .snri_1l = {0.591218f, 1.636514f, 1.880263f, 0.347469f},
+    .snri_2l = {0.869026f, 0.967991f},
     .pmi = {2, 1},
     .ri = 1,
-    .k = 7.6196,
+    .k = 12.9774,
   },
   { /* Test case 13 */
     .h = {
-      {+0.277091f-0.237022f*_Complex_I, -0.230114f-0.399963f*_Complex_I}, 
-      {+0.531396f-0.319721f*_Complex_I, +0.305831f+0.837853f*_Complex_I}
+      {+0.623205f-0.219990f*_Complex_I, -0.028697f+0.854712f*_Complex_I}, 
+      {+0.788896f+0.834988f*_Complex_I, -0.724907f+0.427148f*_Complex_I}
       },
-    .n = 0.456267, 
-    .snri_1l = {1.272387f, 2.072182f, 1.744873f, 1.599696f},
-    .snri_2l = {0.720254f, 0.700519f},
-    .pmi = {1, 0},
-    .ri = 2,
-    .k = 5.9972,
+    .n = 0.618337, 
+    .snri_1l = {3.706176f, 1.461946f, 0.479632f, 4.688490f},
+    .snri_2l = {1.444336f, 2.102567f},
+    .pmi = {3, 1},
+    .ri = 1,
+    .k = 17.0493,
   },
   { /* Test case 14 */
     .h = {
-      {-0.115006f+0.764806f*_Complex_I, -0.091628f-0.960249f*_Complex_I}, 
-      {+0.890564f-0.316470f*_Complex_I, -0.561762f+0.532055f*_Complex_I}
+      {-0.313424f+0.292955f*_Complex_I, +0.872055f+0.666304f*_Complex_I}, 
+      {-0.750452f-0.203436f*_Complex_I, +0.461171f+0.499644f*_Complex_I}
       },
-    .n = 0.342804, 
-    .snri_1l = {2.060602f, 6.750694f, 8.002153f, 0.809143f},
-    .snri_2l = {1.036665f, 1.575640f},
-    .pmi = {2, 1},
+    .n = 0.835221, 
+    .snri_1l = {2.560265f, 0.379539f, 0.976562f, 1.963242f},
+    .snri_2l = {1.380223f, 1.109300f},
+    .pmi = {0, 0},
     .ri = 1,
-    .k = 18.9097,
+    .k = 10.1729,
   },
   { /* Test case 15 */
     .h = {
-      {+0.237613f+0.203137f*_Complex_I, -0.093958f+0.298835f*_Complex_I}, 
-      {-0.979675f-0.314559f*_Complex_I, +0.198162f-0.013401f*_Complex_I}
+      {-0.355079f-0.339153f*_Complex_I, +0.104523f+0.238943f*_Complex_I}, 
+      {+0.958258f-0.278727f*_Complex_I, +0.098617f+0.513019f*_Complex_I}
       },
-    .n = 0.701774, 
-    .snri_1l = {0.466961f, 1.376956f, 0.827475f, 1.016442f},
-    .snri_2l = {0.386935f, 0.354722f},
-    .pmi = {1, 0},
+    .n = 0.413901, 
+    .snri_1l = {1.633620f, 2.178855f, 0.809297f, 3.003178f},
+    .snri_2l = {1.250898f, 1.512017f},
+    .pmi = {3, 1},
     .ri = 1,
-    .k = 11.2469,
+    .k = 10.8925,
   },
   { /* Test case 16 */
     .h = {
-      {+0.775605f+0.528142f*_Complex_I, -0.889884f+0.975918f*_Complex_I}, 
-      {-0.803276f-0.749350f*_Complex_I, +0.299566f-0.271046f*_Complex_I}
+      {-0.015310f+0.675606f*_Complex_I, +0.389486f+0.478144f*_Complex_I}, 
+      {+0.945468f+0.908349f*_Complex_I, -0.344490f-0.936155f*_Complex_I}
       },
-    .n = 0.676230, 
-    .snri_1l = {0.661772f, 5.245671f, 3.261470f, 2.645973f},
-    .snri_2l = {1.354145f, 0.857372f},
-    .pmi = {1, 0},
-    .ri = 1,
-    .k = 10.7137,
+    .n = 0.356869, 
+    .snri_1l = {5.024121f, 4.926495f, 7.364348f, 2.586268f},
+    .snri_2l = {3.165416f, 3.851590f},
+    .pmi = {2, 1},
+    .ri = 2,
+    .k = 7.7799,
   },
 };
 

From b872f84255565db017657d7c38901af571b2fc75 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 16:54:33 +0200
Subject: [PATCH 107/269] Improved RI selection

(cherry picked from commit 034025d)
---
 lib/src/phy/phch/pdsch.c |  2 +-
 lib/src/phy/ue/ue_dl.c   | 21 +++++++++++++--------
 2 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index bf50cb6ec..62f2834ae 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -683,7 +683,7 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                   uint32_t pmi[SRSLTE_MAX_LAYERS], float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]) {
 
   if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
-    for (int nof_layers = 1; nof_layers <= cfg->nof_layers; nof_layers++ ) {
+    for (int nof_layers = 1; nof_layers <= 2; nof_layers++ ) {
       if (sinr[nof_layers - 1] && pmi) {
         if (srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, nof_layers, &pmi[nof_layers - 1],
                                         sinr[nof_layers - 1]) < 0) {
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 91116ab70..55f65ad3d 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -438,9 +438,15 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
                                     q->sf_symbols_m, q->ce_m, 
                                     noise_estimate, 
                                     rnti, data, acks);
-      
+
+      for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
+        if (!acks[tb]) {
+          q->pkt_errors++;
+        }
+        q->pkts_total++;
+      }
+
       if (ret == SRSLTE_ERROR) {
-        q->pkt_errors++;
       } else if (ret == SRSLTE_ERROR_INVALID_INPUTS) {
         fprintf(stderr, "Error calling srslte_pdsch_decode()\n");      
       }
@@ -458,8 +464,6 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   
   }
 
-  q->pkts_total++;
-
   if (found_dci == 1 && ret == SRSLTE_SUCCESS) { 
     return q->pdsch_cfg.grant.mcs[0].tbs;
   } else {
@@ -485,10 +489,11 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
     }
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
-    for (uint32_t nof_layers = 1; nof_layers <= q->pdsch_cfg.nof_layers; nof_layers++ ) {
-      if (q->sinr[nof_layers][q->pmi[nof_layers]] > best_sinr + 0.1) {
-        best_sinr = q->sinr[nof_layers][q->pmi[nof_layers]]*nof_layers;
-        best_pmi = q->pmi[nof_layers];
+    for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
+      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers;
+      if (_sinr > best_sinr + 0.1) {
+        best_sinr = _sinr;
+        best_pmi = q->pmi[nof_layers - 1];
         best_ri = nof_layers;
       }
     }

From cf4a601ea385ce382569766dbc51eb9e1bfa6b03 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 16:56:55 +0200
Subject: [PATCH 108/269] pdsch_ue can change the Tx mode dynamically

(cherry picked from commit fb36014)
---
 lib/examples/pdsch_enodeb.c | 111 ++++++++++++++++++++++++++++++------
 1 file changed, 92 insertions(+), 19 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index a4558ff30..4ded08fdf 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -202,23 +202,15 @@ void base_init() {
   switch(pdsch_cfg.mimo_type) {
     case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
       cell.nof_ports = 1;
-      pdsch_cfg.nof_layers = 1;
-      nof_tb = 1;
       break;
     case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
       cell.nof_ports = 2;
-      pdsch_cfg.nof_layers = 2;
-      nof_tb = 1;
       break;
     case SRSLTE_MIMO_TYPE_CDD:
       cell.nof_ports = 2;
-      pdsch_cfg.nof_layers = 2;
-      nof_tb = 2;
       break;
     case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
       cell.nof_ports = 2;
-      pdsch_cfg.nof_layers = multiplex_nof_layers;
-      nof_tb = multiplex_nof_layers;
       break;
     default:
       ERROR("Transmission mode not implemented.");
@@ -226,7 +218,7 @@ void base_init() {
   }
 
   /* Allocate memory */
-  for(i = 0; i < nof_tb; i++) {
+  for(i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     data[i] = srslte_vec_malloc(sizeof(uint8_t) * SOFTBUFFER_SIZE);
     if (!data[i]) {
       perror("malloc");
@@ -236,7 +228,7 @@ void base_init() {
   }
 
   /* init memory */
-  for (i = 0; i < cell.nof_ports; i++) {
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     sf_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
     if (!sf_buffer[i]) {
       perror("malloc");
@@ -333,7 +325,13 @@ void base_init() {
 
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
 
-  for (i = 0; i < nof_tb; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    softbuffers[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
+    if (!softbuffers[i]) {
+      fprintf(stderr, "Error allocating soft buffer\n");
+      exit(-1);
+    }
+
     if (srslte_softbuffer_tx_init(softbuffers[i], cell.nof_prb)) {
       fprintf(stderr, "Error initiating soft buffer\n");
       exit(-1);
@@ -343,11 +341,11 @@ void base_init() {
 
 void base_free() {
   int i;
-  for (i = 0; i < nof_tb; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    srslte_softbuffer_tx_free(softbuffers[i]);
     if (softbuffers[i]) {
       free(softbuffers[i]);
     }
-    srslte_softbuffer_tx_free(softbuffers[i]);
   }
   srslte_pdsch_free(&pdsch);
   srslte_pdcch_free(&pdcch);
@@ -422,7 +420,30 @@ uint32_t prbset_to_bitmask() {
 }
 
 int update_radl() {
-  
+
+  /* Configure cell and PDSCH in function of the transmission mode */
+  switch(pdsch_cfg.mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      pdsch_cfg.nof_layers = 1;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 2;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      pdsch_cfg.nof_layers = multiplex_nof_layers;
+      nof_tb = multiplex_nof_layers;
+      break;
+    default:
+      ERROR("Transmission mode not implemented.");
+      exit(-1);
+  }
+
   bzero(&ra_dl, sizeof(srslte_ra_dl_dci_t));
   ra_dl.harq_process = 0;
   ra_dl.mcs_idx = mcs_idx;
@@ -445,8 +466,21 @@ int update_radl() {
   srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &dummy_grant);
   srslte_ra_dl_grant_to_nbits(&dummy_grant, cfi, cell, 0, &dummy_nbits);
   srslte_ra_dl_grant_fprint(stdout, &dummy_grant);
-  printf("Type new MCS index and press Enter: "); fflush(stdout);
- 
+
+  if (pdsch_cfg.mimo_type != SRSLTE_MIMO_TYPE_SINGLE_ANTENNA) {
+    printf("\nTransmission mode key table:\n");
+    printf("   Mode   |   1TB   | 2TB |\n");
+    printf("----------+---------+-----+\n");
+    printf("Diversity |    x    |     |\n");
+    printf("      CDD |         |  z  |\n");
+    printf("Multiplex | q,w,e,r | a,s |\n");
+    printf("\n");
+    printf("Type new MCS index (0-28) or mode key and press Enter: ");
+  } else {
+    printf("Type new MCS index (0-28) and press Enter: ");
+  }
+  fflush(stdout);
+
   return 0; 
 }
 
@@ -487,8 +521,47 @@ int update_control() {
             break;          
         }
       } else {
-        last_mcs_idx = mcs_idx; 
-        mcs_idx = atoi(input);          
+        switch (input[0]) {
+          case 'q':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 0;
+            multiplex_nof_layers = 1;
+            break;
+          case 'w':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 1;
+            multiplex_nof_layers = 1;
+            break;
+          case 'e':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 2;
+            multiplex_nof_layers = 1;
+            break;
+          case 'r':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 3;
+            multiplex_nof_layers = 1;
+            break;
+          case 'a':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 0;
+            multiplex_nof_layers = 2;
+            break;
+          case 's':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 1;
+            multiplex_nof_layers = 2;
+            break;
+          case 'z':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_CDD;
+            break;
+          case 'x':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+            break;
+          default:
+            last_mcs_idx = mcs_idx;
+            mcs_idx = atoi(input);
+        }
       }
       bzero(input,sizeof(input));
       if (update_radl()) {
@@ -651,7 +724,7 @@ int main(int argc, char **argv) {
   nf = 0;
   
   bool send_data = false; 
-  for (i = 0; i < nof_tb; i++) {
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     srslte_softbuffer_tx_reset(softbuffers[i]);
   }
 

From 92882e64479f0c74a2d5a35d73d33cab8de9f794 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 29 Aug 2017 16:57:56 +0200
Subject: [PATCH 109/269] Improved MIMO stats display

(cherry picked from commit 877ea6a)
---
 lib/examples/pdsch_ue.c | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index be271049e..1a9f77c6f 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -663,7 +663,7 @@ int main(int argc, char **argv) {
           }
 
           // Plot and Printf
-          if (srslte_ue_sync_get_sfidx(&ue_sync) == 5) {
+          if (srslte_ue_sync_get_sfidx(&ue_sync) == 5 && sfn % 20 == 0) {
             float gain = prog_args.rf_gain; 
             if (gain < 0) {
               gain = 10*log10(srslte_agc_get_gain(&ue_sync.agc)); 
@@ -703,7 +703,6 @@ int main(int argc, char **argv) {
               printf("\033[K           Rb: %6.2f / %6.2f Mbps (net/maximum)\n", uerate, enodebrate);
               printf("\033[K   PDCCH-Miss: %5.2f%%\n", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
               printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
-              printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
               printf("\033[K         TB 0: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[0].idx, ue_dl.pdsch_cfg.grant.mcs[0].tbs);
               printf("\033[K         TB 1: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[1].idx, ue_dl.pdsch_cfg.grant.mcs[1].tbs);
               printf("\033[K            κ: %.1f dB (Condition number, 0 dB => Best)\n", cn);
@@ -715,7 +714,7 @@ int main(int argc, char **argv) {
               printf("\033[K M |  1 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][1]), (ri == 1 && pmi == 1)?'*':' ', 10 * log10(sinr[1][1]), (ri == 2 && pmi == 1)?'*':' ');
               printf("\033[K I |  2 | %5.2f%c|-------+ \n", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2)?'*':' ');
               printf("\033[K   |  3 | %5.2f%c|         \n", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3)?'*':' ');
-              printf("\033[K\n\n");
+              printf("\033[K\nPress enter maximum printing debug log of 1 subframe.\n");
               printf("\033[21A");
             }
           }

From d933f1b8175e00f2330b8052138a9eae79ae365b Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 12:24:26 +0200
Subject: [PATCH 110/269] Renamed module algebra (it is now named mat) and
 moved usefl math SIMD macros to simd.h

---
 lib/include/srslte/phy/utils/algebra.h        | 161 ------------------
 lib/include/srslte/phy/utils/mat.h            | 111 ++++++++++++
 lib/include/srslte/phy/utils/simd.h           |  81 +++++++++
 lib/src/phy/mimo/precoding.c                  |  36 ++--
 lib/src/phy/utils/{algebra.c => mat.c}        |  46 ++---
 lib/src/phy/utils/test/CMakeLists.txt         |   2 +-
 .../utils/test/{algebra_test.c => mat_test.c} |  14 +-
 7 files changed, 241 insertions(+), 210 deletions(-)
 delete mode 100644 lib/include/srslte/phy/utils/algebra.h
 create mode 100644 lib/include/srslte/phy/utils/mat.h
 create mode 100644 lib/include/srslte/phy/utils/simd.h
 rename lib/src/phy/utils/{algebra.c => mat.c} (82%)
 rename lib/src/phy/utils/test/{algebra_test.c => mat_test.c} (96%)

diff --git a/lib/include/srslte/phy/utils/algebra.h b/lib/include/srslte/phy/utils/algebra.h
deleted file mode 100644
index ee681d558..000000000
--- a/lib/include/srslte/phy/utils/algebra.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsLTE library.
- *
- * srsLTE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsLTE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-#ifndef SRSLTE_ALGEBRA_H
-#define SRSLTE_ALGEBRA_H
-
-#include "srslte/config.h"
-
-/*
- * Generic Macros
- */
-#define RANDOM_CF() (((float)rand())/((float)RAND_MAX) + _Complex_I*((float)rand())/((float)RAND_MAX))
-
-/*
- * SSE Macros
- */
-#ifdef LV_HAVE_SSE
-#define _MM_SWAP(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,3,0,1)))
-#define _MM_PERM(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,1,3,0)))
-#define _MM_MULJ_PS(X) _MM_SWAP(_MM_CONJ_PS(X))
-#define _MM_CONJ_PS(X) (_mm_xor_ps(X, _mm_set_ps(-0.0f, 0.0f, -0.0f, 0.0f)))
-#define _MM_SQMOD_PS(X) _MM_PERM(_mm_hadd_ps(_mm_mul_ps(X,X), _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f)))
-#define _MM_PROD_PS(a, b) _mm_addsub_ps(_mm_mul_ps(a,_mm_moveldup_ps(b)),_mm_mul_ps(\
-                            _mm_shuffle_ps(a,a,0xB1),_mm_movehdup_ps(b)))
-
-#endif /* LV_HAVE_SSE */
-
-
-/*
- * AVX Macros
- */
-#ifdef LV_HAVE_AVX
-
-#define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
-#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, _mm256_set_ps(-0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f)))
-
-#ifdef LV_HAVE_FMA
-#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_mul_ps(B,B)), \
-                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
-#define _MM256_PROD_PS(a, b) _mm256_fmaddsub_ps(a,_mm256_moveldup_ps(b),\
-                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
-#else
-#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_add_ps(_mm256_mul_ps(A,A), _mm256_mul_ps(B,B)), \
-                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
-#define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
-                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
-#endif /* LV_HAVE_FMA */
-#endif /* LV_HAVE_AVX */
-
-/*
- * AVX extension with FMA Macros
- */
-#ifdef LV_HAVE_FMA
-
-#define _MM256_SQMOD_ADD_PS(A, B, C) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_fmadd_ps(B, B, C)),\
-                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
-
-#define _MM256_PROD_ADD_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
-                              _mm256_fmaddsub_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
-
-#define _MM256_PROD_SUB_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
-                              _mm256_fmsubadd_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
-#endif /* LV_HAVE_FMA */
-
-/* Generic implementation for complex reciprocal */
-SRSLTE_API cf_t srslte_algebra_cf_recip_gen(cf_t a);
-
-/* Generic implementation for 2x2 determinant */
-SRSLTE_API cf_t srslte_algebra_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11);
-
-/* Generic implementation for 2x2 Matrix Inversion */
-SRSLTE_API void srslte_algebra_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
-                                           cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11);
-
-/* Generic implementation for Zero Forcing (ZF) solver */
-SRSLTE_API void srslte_algebra_2x2_zf_gen(cf_t y0, cf_t y1,
-                                          cf_t h00, cf_t h01, cf_t h10, cf_t h11,
-                                          cf_t *x0, cf_t *x1,
-                                          float norm);
-
-/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
-SRSLTE_API void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1,
-                                            cf_t h00, cf_t h01, cf_t h10, cf_t h11,
-                                            cf_t *x0, cf_t *x1,
-                                            float noise_estimate,
-                                            float norm);
-
-SRSLTE_API float srslte_algebra_2x2_cn(cf_t h00,
-                                       cf_t h01,
-                                       cf_t h10,
-                                       cf_t h11);
-
-
-#ifdef LV_HAVE_SSE
-
-/* SSE implementation for complex reciprocal */
-SRSLTE_API __m128 srslte_algebra_cf_recip_sse(__m128 a);
-
-/* SSE implementation for 2x2 determinant */
-SRSLTE_API __m128 srslte_algebra_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11);
-
-/* SSE implementation for Zero Forcing (ZF) solver */
-SRSLTE_API void srslte_algebra_2x2_zf_sse(__m128 y0, __m128 y1,
-                                          __m128 h00, __m128 h01, __m128 h10, __m128 h11,
-                                          __m128 *x0, __m128 *x1,
-                                          float norm);
-
-/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
-SRSLTE_API void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1,
-                                            __m128 h00, __m128 h01, __m128 h10, __m128 h11,
-                                            __m128 *x0, __m128 *x1,
-                                            float noise_estimate, float norm);
-
-#endif /* LV_HAVE_SSE */
-
-#ifdef LV_HAVE_AVX
-
-/* AVX implementation for complex reciprocal */
-SRSLTE_API __m256 srslte_algebra_cf_recip_avx(__m256 a);
-
-/* AVX implementation for 2x2 determinant */
-SRSLTE_API __m256 srslte_algebra_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11);
-
-/* AVX implementation for Zero Forcing (ZF) solver */
-SRSLTE_API void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1,
-                                          __m256 h00, __m256 h01, __m256 h10, __m256 h11,
-                                          __m256 *x0, __m256 *x1,
-                                          float norm);
-
-/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
-SRSLTE_API void srslte_algebra_2x2_mmse_avx(__m256 y0, __m256 y1,
-                                            __m256 h00, __m256 h01, __m256 h10, __m256 h11,
-                                            __m256 *x0, __m256 *x1,
-                                            float noise_estimate, float norm);
-
-#endif /* LV_HAVE_AVX */
-
-#endif //SRSLTE_ALGEBRA_H
diff --git a/lib/include/srslte/phy/utils/mat.h b/lib/include/srslte/phy/utils/mat.h
new file mode 100644
index 000000000..48d3b776d
--- /dev/null
+++ b/lib/include/srslte/phy/utils/mat.h
@@ -0,0 +1,111 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef SRSLTE_MAT_H
+#define SRSLTE_MAT_H
+
+#include "srslte/phy/utils/simd.h"
+#include "srslte/config.h"
+
+/*
+ * Generic Macros
+ */
+#define RANDOM_CF() (((float)rand())/((float)RAND_MAX) + _Complex_I*((float)rand())/((float)RAND_MAX))
+
+/* Generic implementation for complex reciprocal */
+SRSLTE_API cf_t srslte_mat_cf_recip_gen(cf_t a);
+
+/* Generic implementation for 2x2 determinant */
+SRSLTE_API cf_t srslte_mat_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11);
+
+/* Generic implementation for 2x2 Matrix Inversion */
+SRSLTE_API void srslte_mat_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+                                       cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11);
+
+/* Generic implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_gen(cf_t y0, cf_t y1,
+                                      cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                      cf_t *x0, cf_t *x1,
+                                      float norm);
+
+/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_gen(cf_t y0, cf_t y1,
+                                        cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                        cf_t *x0, cf_t *x1,
+                                        float noise_estimate,
+                                        float norm);
+
+SRSLTE_API float srslte_mat_2x2_cn(cf_t h00,
+                                   cf_t h01,
+                                   cf_t h10,
+                                   cf_t h11);
+
+
+#ifdef LV_HAVE_SSE
+
+/* SSE implementation for complex reciprocal */
+SRSLTE_API __m128 srslte_mat_cf_recip_sse(__m128 a);
+
+/* SSE implementation for 2x2 determinant */
+SRSLTE_API __m128 srslte_mat_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11);
+
+/* SSE implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_sse(__m128 y0, __m128 y1,
+                                      __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                      __m128 *x0, __m128 *x1,
+                                      float norm);
+
+/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1,
+                                        __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                        __m128 *x0, __m128 *x1,
+                                        float noise_estimate, float norm);
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+/* AVX implementation for complex reciprocal */
+SRSLTE_API __m256 srslte_mat_cf_recip_avx(__m256 a);
+
+/* AVX implementation for 2x2 determinant */
+SRSLTE_API __m256 srslte_mat_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11);
+
+/* AVX implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_avx(__m256 y0, __m256 y1,
+                                      __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                      __m256 *x0, __m256 *x1,
+                                      float norm);
+
+/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_avx(__m256 y0, __m256 y1,
+                                        __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                        __m256 *x0, __m256 *x1,
+                                        float noise_estimate, float norm);
+
+#endif /* LV_HAVE_AVX */
+
+#endif /* SRSLTE_MAT_H */
diff --git a/lib/include/srslte/phy/utils/simd.h b/lib/include/srslte/phy/utils/simd.h
new file mode 100644
index 000000000..420d07213
--- /dev/null
+++ b/lib/include/srslte/phy/utils/simd.h
@@ -0,0 +1,81 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef SRSLTE_SIMD_H_H
+#define SRSLTE_SIMD_H_H
+
+/*
+ * SSE Macros
+ */
+#ifdef LV_HAVE_SSE
+#define _MM_SWAP(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,3,0,1)))
+#define _MM_PERM(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,1,3,0)))
+#define _MM_MULJ_PS(X) _MM_SWAP(_MM_CONJ_PS(X))
+#define _MM_CONJ_PS(X) (_mm_xor_ps(X, _mm_set_ps(-0.0f, 0.0f, -0.0f, 0.0f)))
+#define _MM_SQMOD_PS(X) _MM_PERM(_mm_hadd_ps(_mm_mul_ps(X,X), _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f)))
+#define _MM_PROD_PS(a, b) _mm_addsub_ps(_mm_mul_ps(a,_mm_moveldup_ps(b)),_mm_mul_ps(\
+                            _mm_shuffle_ps(a,a,0xB1),_mm_movehdup_ps(b)))
+
+#endif /* LV_HAVE_SSE */
+
+/*
+ * AVX Macros
+ */
+#ifdef LV_HAVE_AVX
+
+#define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
+#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, _mm256_set_ps(-0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f)))
+
+#ifdef LV_HAVE_FMA
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+#define _MM256_PROD_PS(a, b) _mm256_fmaddsub_ps(a,_mm256_moveldup_ps(b),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#else
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_add_ps(_mm256_mul_ps(A,A), _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+#define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#endif /* LV_HAVE_FMA */
+#endif /* LV_HAVE_AVX */
+
+
+/*
+ * AVX extension with FMA Macros
+ */
+#ifdef LV_HAVE_FMA
+
+#define _MM256_SQMOD_ADD_PS(A, B, C) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_fmadd_ps(B, B, C)),\
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+
+#define _MM256_PROD_ADD_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmaddsub_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+
+#define _MM256_PROD_SUB_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmsubadd_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+#endif /* LV_HAVE_FMA */
+
+#endif //SRSLTE_SIMD_H_H
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 0e898448a..02faa6e1c 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -36,14 +36,14 @@
 
 #ifdef LV_HAVE_SSE
 #include <immintrin.h>
-#include "srslte/phy/utils/algebra.h"
+#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 int srslte_predecoding_diversity2_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_symbols);
 #endif
 
 #ifdef LV_HAVE_AVX
 #include <immintrin.h>
-#include "srslte/phy/utils/algebra.h"
+#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 #endif
 
@@ -597,7 +597,7 @@ int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
 
     __m256 x0, x1;
 
-    srslte_algebra_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
+    srslte_mat_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
 
     _mm256_store_ps((float *) &x[0][i], x0);
     _mm256_store_ps((float *) &x[1][i], x1);
@@ -634,7 +634,7 @@ int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
 
     __m128 x0, x1;
 
-    srslte_algebra_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
+    srslte_mat_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
 
     _mm_store_ps((float *) &x[0][i], x0);
     _mm_store_ps((float *) &x[1][i], x1);
@@ -731,7 +731,7 @@ int srslte_predecoding_ccd_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS],
 
     __m256 x0, x1;
 
-    srslte_algebra_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
+    srslte_mat_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
 
     _mm256_store_ps((float *) &x[0][i], x0);
     _mm256_store_ps((float *) &x[1][i], x1);
@@ -768,7 +768,7 @@ int srslte_predecoding_ccd_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS],
 
     __m128 x0, x1;
 
-    srslte_algebra_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
+    srslte_mat_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
 
     _mm_store_ps((float *) &x[0][i], x0);
     _mm_store_ps((float *) &x[1][i], x1);
@@ -789,7 +789,7 @@ int srslte_predecoding_ccd_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
     h10 = +h[0][1][i] + h[1][1][i];
     h01 = +h[0][0][i] - h[1][0][i];
     h11 = +h[0][1][i] - h[1][1][i];
-    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
 
     i++;
 
@@ -798,7 +798,7 @@ int srslte_predecoding_ccd_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLT
     h10 = h[0][1][i] - h[1][1][i];
     h01 = h[0][0][i] + h[1][0][i];
     h11 = h[0][1][i] + h[1][1][i];
-    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
   }
   return SRSLTE_SUCCESS;
 }
@@ -886,7 +886,7 @@ int srslte_predecoding_multiplex_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[S
 
     __m256 x0, x1;
 
-    srslte_algebra_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+    srslte_mat_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
 
     _mm256_store_ps((float *) &x[0][i], x0);
     _mm256_store_ps((float *) &x[1][i], x1);
@@ -954,7 +954,7 @@ int srslte_predecoding_multiplex_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[S
 
     __m128 x0, x1;
 
-    srslte_algebra_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+    srslte_mat_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
 
     _mm_store_ps((float *) &x[0][i], x0);
     _mm_store_ps((float *) &x[1][i], x1);
@@ -1078,7 +1078,7 @@ int srslte_predecoding_multiplex_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h
 
     __m256 x0, x1;
 
-    srslte_algebra_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+    srslte_mat_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
 
     _mm256_store_ps((float *) &x[0][i], x0);
     _mm256_store_ps((float *) &x[1][i], x1);
@@ -1148,7 +1148,7 @@ int srslte_predecoding_multiplex_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h
 
     __m128 x0, x1;
 
-    srslte_algebra_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+    srslte_mat_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
 
     _mm_store_ps((float *) &x[0][i], x0);
     _mm_store_ps((float *) &x[1][i], x1);
@@ -1205,7 +1205,7 @@ int srslte_predecoding_multiplex_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h
         return SRSLTE_ERROR;
     }
 
-    srslte_algebra_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, norm);
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, norm);
   }
   return SRSLTE_SUCCESS;
 }
@@ -2295,8 +2295,8 @@ int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
       c11 = _mm_add_ps(c11, sse_noise_estimate);
 
       /* 5. detC */
-      __m128 detC = srslte_algebra_2x2_det_sse(c00, c01, c10, c11);
-      __m128 inv_detC = srslte_algebra_cf_recip_sse(detC);
+      __m128 detC = srslte_mat_2x2_det_sse(c00, c01, c10, c11);
+      __m128 inv_detC = srslte_mat_cf_recip_sse(detC);
       inv_detC = _mm_mul_ps(sse_noise_estimate, inv_detC);
 
       __m128 den0 = _MM_PROD_PS(c00, inv_detC);
@@ -2442,8 +2442,8 @@ int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORT
       c11 = _mm256_add_ps(c11, avx_noise_estimate);
 
       /* 5. detC */
-      __m256 detC = srslte_algebra_2x2_det_avx(c00, c01, c10, c11);
-      __m256 inv_detC = srslte_algebra_cf_recip_avx(detC);
+      __m256 detC = srslte_mat_2x2_det_avx(c00, c01, c10, c11);
+      __m256 inv_detC = srslte_mat_cf_recip_avx(detC);
       inv_detC = _mm256_mul_ps(avx_noise_estimate, inv_detC);
 
       __m256 den0 = _MM256_PROD_PS(c00, inv_detC);
@@ -2530,7 +2530,7 @@ float srslte_precoding_2x2_cn_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], u
     cf_t h10 = h[0][1][i];
     cf_t h11 = h[1][1][i];
 
-    cn_avg += srslte_algebra_2x2_cn(h00, h01, h10, h11);
+    cn_avg += srslte_mat_2x2_cn(h00, h01, h10, h11);
 
     count++;
   }
diff --git a/lib/src/phy/utils/algebra.c b/lib/src/phy/utils/mat.c
similarity index 82%
rename from lib/src/phy/utils/algebra.c
rename to lib/src/phy/utils/mat.c
index 943ae57dd..3c81d4a13 100644
--- a/lib/src/phy/utils/algebra.c
+++ b/lib/src/phy/utils/mat.c
@@ -28,23 +28,23 @@
 #include <immintrin.h>
 #include <math.h>
 
-#include "srslte/phy/utils/algebra.h"
+#include "srslte/phy/utils/mat.h"
 
 
 /* Generic implementation for complex reciprocal */
-inline cf_t srslte_algebra_cf_recip_gen(cf_t a) {
+inline cf_t srslte_mat_cf_recip_gen(cf_t a) {
   return conjf(a) / (crealf(a) * crealf(a) + cimagf(a) * cimagf(a));
 }
 
 /* Generic implementation for 2x2 determinant */
-inline cf_t srslte_algebra_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11) {
+inline cf_t srslte_mat_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11) {
   return a00 * a11 - a01 * a10;
 }
 
 /* 2x2 Matrix inversion, generic implementation */
-inline void srslte_algebra_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+inline void srslte_mat_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
                                        cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11) {
-  cf_t div = srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(a00, a01, a10, a11));
+  cf_t div = srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(a00, a01, a10, a11));
   *r00 = a11 * div;
   *r01 = -a01 * div;
   *r10 = -a10 * div;
@@ -52,15 +52,15 @@ inline void srslte_algebra_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
 }
 
 /* Generic implementation for Zero Forcing (ZF) solver */
-inline void srslte_algebra_2x2_zf_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+inline void srslte_mat_2x2_zf_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
                                       cf_t *x0, cf_t *x1, float norm) {
-  cf_t _norm = srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(h00, h01, h10, h11)) * norm;
+  cf_t _norm = srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(h00, h01, h10, h11)) * norm;
   *x0 = (y0 * h11 - h01 * y1) * _norm;
   *x1 = (y1 * h00 - h10 * y0) * _norm;
 }
 
 /* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
-inline void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+inline void srslte_mat_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
                                         cf_t *x0, cf_t *x1, float noise_estimate, float norm) {
   /* Create conjugated matrix */
   cf_t _h00 = conjf(h00);
@@ -79,7 +79,7 @@ inline void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf
   cf_t b01 = -a01;
   cf_t b10 = -a10;
   cf_t b11 = a00;
-  cf_t _norm = norm * srslte_algebra_cf_recip_gen(srslte_algebra_2x2_det_gen(a00, a01, a10, a11));
+  cf_t _norm = norm * srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(a00, a01, a10, a11));
 
 
   /* 3. W = inv(H' x H + No) x H' = B x H' */
@@ -93,7 +93,7 @@ inline void srslte_algebra_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf
   *x1 = (y0 * w10 + y1 * w11) * _norm;
 }
 
-inline float srslte_algebra_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
+inline float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
   /* 1. A = H * H' (A = A') */
   float a00 =
       crealf(h00) * crealf(h00) + crealf(h01) * crealf(h01) + cimagf(h00) * cimagf(h00) + cimagf(h01) * cimagf(h01);
@@ -118,7 +118,7 @@ inline float srslte_algebra_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
 #ifdef LV_HAVE_SSE
 
 /* SSE implementation for complex reciprocal */
-inline __m128 srslte_algebra_cf_recip_sse(__m128 a) {
+inline __m128 srslte_mat_cf_recip_sse(__m128 a) {
   __m128 conj = _MM_CONJ_PS(a);
   __m128 sqabs = _mm_mul_ps(a, a);
   sqabs = _mm_add_ps(_mm_movehdup_ps(sqabs), _mm_moveldup_ps(sqabs));
@@ -129,25 +129,25 @@ inline __m128 srslte_algebra_cf_recip_sse(__m128 a) {
 }
 
 /* SSE implementation for 2x2 determinant */
-inline __m128 srslte_algebra_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11) {
+inline __m128 srslte_mat_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11) {
   return _mm_sub_ps(_MM_PROD_PS(a00, a11), _MM_PROD_PS(a01, a10));
 }
 
 /* SSE implementation for Zero Forcing (ZF) solver */
-inline void srslte_algebra_2x2_zf_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+inline void srslte_mat_2x2_zf_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
                                       __m128 *x0, __m128 *x1, float norm) {
   __m128 detmult1 = _MM_PROD_PS(h00, h11);
   __m128 detmult2 = _MM_PROD_PS(h01, h10);
 
   __m128 det = _mm_sub_ps(detmult1, detmult2);
-  __m128 detrec = _mm_mul_ps(srslte_algebra_cf_recip_sse(det), _mm_set1_ps(norm));
+  __m128 detrec = _mm_mul_ps(srslte_mat_cf_recip_sse(det), _mm_set1_ps(norm));
 
   *x0 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h11, y0), _MM_PROD_PS(h01, y1)), detrec);
   *x1 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h00, y1), _MM_PROD_PS(h10, y0)), detrec);
 }
 
 /* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
-inline void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+inline void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
                                         __m128 *x0, __m128 *x1, float noise_estimate, float norm) {
   __m128 _noise_estimate = _mm_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate);
   __m128 _norm = _mm_set1_ps(norm);
@@ -169,7 +169,7 @@ inline void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128
   __m128 b01 = _mm_xor_ps(a01, _mm_set1_ps(-0.0f));
   __m128 b10 = _mm_xor_ps(a10, _mm_set1_ps(-0.0f));
   __m128 b11 = a00;
-  _norm = _mm_mul_ps(_norm, srslte_algebra_cf_recip_sse(srslte_algebra_2x2_det_sse(a00, a01, a10, a11)));
+  _norm = _mm_mul_ps(_norm, srslte_mat_cf_recip_sse(srslte_mat_2x2_det_sse(a00, a01, a10, a11)));
 
 
   /* 3. W = inv(H' x H + No) x H' = B x H' */
@@ -188,7 +188,7 @@ inline void srslte_algebra_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128
 #ifdef LV_HAVE_AVX
 
 /* AVX implementation for complex reciprocal */
-inline __m256 srslte_algebra_cf_recip_avx(__m256 a) {
+inline __m256 srslte_mat_cf_recip_avx(__m256 a) {
   __m256 conj = _MM256_CONJ_PS(a);
   __m256 sqabs = _mm256_mul_ps(a, a);
   sqabs = _mm256_add_ps(_mm256_movehdup_ps(sqabs), _mm256_moveldup_ps(sqabs));
@@ -199,7 +199,7 @@ inline __m256 srslte_algebra_cf_recip_avx(__m256 a) {
 }
 
 /* AVX implementation for 2x2 determinant */
-inline __m256 srslte_algebra_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11) {
+inline __m256 srslte_mat_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11) {
 #ifdef LV_HAVE_FMA
   return _MM256_PROD_SUB_PS(a00, a11, _MM256_PROD_PS(a01, a10));
 #else
@@ -208,11 +208,11 @@ inline __m256 srslte_algebra_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m
 }
 
 /* AVX implementation for Zero Forcing (ZF) solver */
-inline void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+inline void srslte_mat_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
                                       __m256 *x0, __m256 *x1, float norm) {
 
-  __m256 det = srslte_algebra_2x2_det_avx(h00, h01, h10, h11);
-  __m256 detrec = _mm256_mul_ps(srslte_algebra_cf_recip_avx(det), _mm256_set1_ps(norm));
+  __m256 det = srslte_mat_2x2_det_avx(h00, h01, h10, h11);
+  __m256 detrec = _mm256_mul_ps(srslte_mat_cf_recip_avx(det), _mm256_set1_ps(norm));
 
 #ifdef LV_HAVE_FMA
   *x0 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h11, y0, _MM256_PROD_PS(h01, y1)), detrec);
@@ -224,7 +224,7 @@ inline void srslte_algebra_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h
 }
 
 /* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
-inline void srslte_algebra_2x2_mmse_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+inline void srslte_mat_2x2_mmse_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
                                         __m256 *x0, __m256 *x1, float noise_estimate, float norm) {
   __m256 _noise_estimate = _mm256_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate,
                                          0.0f, noise_estimate, 0.0f, noise_estimate);
@@ -254,7 +254,7 @@ inline void srslte_algebra_2x2_mmse_avx(__m256 y0, __m256 y1, __m256 h00, __m256
   __m256 b01 = _mm256_xor_ps(a01, _mm256_set1_ps(-0.0f));
   __m256 b10 = _mm256_xor_ps(a10, _mm256_set1_ps(-0.0f));
   __m256 b11 = a00;
-  _norm = _mm256_mul_ps(_norm, srslte_algebra_cf_recip_avx(srslte_algebra_2x2_det_avx(a00, a01, a10, a11)));
+  _norm = _mm256_mul_ps(_norm, srslte_mat_cf_recip_avx(srslte_mat_2x2_det_avx(a00, a01, a10, a11)));
 
 
   /* 3. W = inv(H' x H + No) x H' = B x H' */
diff --git a/lib/src/phy/utils/test/CMakeLists.txt b/lib/src/phy/utils/test/CMakeLists.txt
index 1f516da8f..4dccbf2a0 100644
--- a/lib/src/phy/utils/test/CMakeLists.txt
+++ b/lib/src/phy/utils/test/CMakeLists.txt
@@ -37,7 +37,7 @@ add_test(dft_odd_dc dft_test -N 255 -b -d) # Odd-length, backwards first, handle
 # Algebra TEST
 ########################################################################
 
-add_executable(algebra_test algebra_test.c)
+add_executable(algebra_test mat_test.c)
 target_link_libraries(algebra_test srslte_phy)
 
 add_test(algebra_2x2_zf_solver_test algebra_test -z)
diff --git a/lib/src/phy/utils/test/algebra_test.c b/lib/src/phy/utils/test/mat_test.c
similarity index 96%
rename from lib/src/phy/utils/test/algebra_test.c
rename to lib/src/phy/utils/test/mat_test.c
index a5bc7e773..49be5c9ae 100644
--- a/lib/src/phy/utils/test/algebra_test.c
+++ b/lib/src/phy/utils/test/mat_test.c
@@ -32,7 +32,7 @@
 #include <immintrin.h>
 #include <sys/time.h>
 
-#include "srslte/phy/utils/algebra.h"
+#include "srslte/phy/utils/mat.h"
 
 
 bool zf_solver = false;
@@ -104,7 +104,7 @@ bool test_zf_solver_gen(void) {
   cf_t y0 = x0_gold * h00 + x1_gold * h01;
   cf_t y1 = x0_gold * h10 + x1_gold * h11;
 
-  srslte_algebra_2x2_zf_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 1.0f);
+  srslte_mat_2x2_zf_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 1.0f);
 
   cf_error0 = x0 - x0_gold;
   cf_error1 = x1 - x1_gold;
@@ -127,7 +127,7 @@ bool test_mmse_solver_gen(void) {
   cf_t y0 = x0_gold * h00 + x1_gold * h01;
   cf_t y1 = x0_gold * h10 + x1_gold * h11;
 
-  srslte_algebra_2x2_mmse_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 0.0f, 1.0f);
+  srslte_mat_2x2_mmse_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 0.0f, 1.0f);
 
   cf_error0 = x0 - x0_gold;
   cf_error1 = x1 - x1_gold;
@@ -171,7 +171,7 @@ bool test_zf_solver_sse(void) {
 
   __m128 _x0, _x1;
 
-  srslte_algebra_2x2_zf_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+  srslte_mat_2x2_zf_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
 
 
   __attribute__((aligned(128))) cf_t x0[2];
@@ -225,7 +225,7 @@ bool test_mmse_solver_sse(void) {
 
   __m128 _x0, _x1;
 
-  srslte_algebra_2x2_mmse_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+  srslte_mat_2x2_mmse_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
 
 
   __attribute__((aligned(128))) cf_t x0[2];
@@ -289,7 +289,7 @@ bool test_zf_solver_avx(void) {
 
   __m256 _x0, _x1;
 
-  srslte_algebra_2x2_zf_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+  srslte_mat_2x2_zf_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
 
 
   __attribute__((aligned(256))) cf_t x0[4];
@@ -349,7 +349,7 @@ bool test_mmse_solver_avx(void) {
 
   __m256 _x0, _x1;
 
-  srslte_algebra_2x2_mmse_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+  srslte_mat_2x2_mmse_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
 
 
   __attribute__((aligned(256))) cf_t x0[4];

From c2b5499284e00087e2faa1d6cc58a9604e01996a Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 12:35:27 +0200
Subject: [PATCH 111/269] Removed SCH object vector from PDSCH object (only SCH
 object from now on)

---
 lib/include/srslte/phy/phch/pdsch.h |  2 +-
 lib/src/phy/phch/pdsch.c            | 33 ++++++++++++-----------------
 2 files changed, 15 insertions(+), 20 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index a1016aaf5..a30851648 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -74,7 +74,7 @@ typedef struct SRSLTE_API {
   // This is to generate the scrambling seq for multiple CRNTIs
   srslte_pdsch_user_t **users;
   
-  srslte_sch_t dl_sch[SRSLTE_MAX_CODEWORDS];
+  srslte_sch_t dl_sch;
 
 } srslte_pdsch_t;
 
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 8a1d66be1..018fb96e5 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -227,12 +227,12 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       srslte_modem_table_bytes(&q->mod[i]);
     }
 
-    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
-      if (srslte_sch_init(&q->dl_sch[i])) {
-        ERROR("Initiating DL SCH");
-        goto clean;
-      }
+    if (srslte_sch_init(&q->dl_sch)) {
+      ERROR("Initiating DL SCH");
+      goto clean;
+    }
 
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
       // Allocate int16_t for reception (LLRs)
       q->e[i] = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
       if (!q->e[i]) {
@@ -313,12 +313,11 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     if (q->d[i]) {
       free(q->d[i]);
     }
-
-    /* Free sch objects */
-    srslte_sch_free(&q->dl_sch[i]);
-
   }
 
+  /* Free sch objects */
+  srslte_sch_free(&q->dl_sch);
+
   for (i = 0; i < q->cell.nof_ports; i++) {
     if (q->x[i]) {
       free(q->x[i]);
@@ -452,7 +451,6 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
 static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
-  srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
   srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
   uint32_t rv = cfg->rv[codeword_idx];
@@ -463,7 +461,7 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
          nbits->nof_re, nbits->nof_bits, rv);
 
     /* Channel coding */
-    if (srslte_dlsch_encode2(dl_sch, cfg, softbuffer, data, pdsch->e[codeword_idx], codeword_idx)) {
+    if (srslte_dlsch_encode2(&pdsch->dl_sch, cfg, softbuffer, data, pdsch->e[codeword_idx], codeword_idx)) {
       ERROR("Error encoding TB %d", codeword_idx);
       return SRSLTE_ERROR;
     }
@@ -497,7 +495,6 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
 static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
-  srslte_sch_t *dl_sch = &pdsch->dl_sch[codeword_idx];
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
   srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
   uint32_t rv = cfg->rv[codeword_idx];
@@ -526,7 +523,7 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
       srslte_sequence_free(&seq);
     }
 
-    return srslte_dlsch_decode2(dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
+    return srslte_dlsch_decode2(&pdsch->dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
   }
 
   return SRSLTE_SUCCESS;
@@ -741,7 +738,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
     }
     memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
 
-    if (srslte_dlsch_encode(&q->dl_sch[0], cfg, softbuffer, data, q->e[0])) {
+    if (srslte_dlsch_encode(&q->dl_sch, cfg, softbuffer, data, q->e[0])) {
       fprintf(stderr, "Error encoding TB\n");
       return SRSLTE_ERROR;
     }
@@ -852,17 +849,15 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
 }
 
 void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter) {
-  for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
-    srslte_sch_set_max_noi(&q->dl_sch[cw], max_iter);
-  }
+  srslte_sch_set_max_noi(&q->dl_sch, max_iter);
 }
 
 float srslte_pdsch_average_noi(srslte_pdsch_t *q) {
-  return q->dl_sch[0].average_nof_iterations;
+  return q->dl_sch.average_nof_iterations;
 }
 
 uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q) {
-  return q->dl_sch[0].nof_iterations;
+  return q->dl_sch.nof_iterations;
 }
 
 

From fee8337c67ff57cabfdfce6ee01a3748f96e124e Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 14:22:42 +0200
Subject: [PATCH 112/269] Removed unused function entries

---
 lib/include/srslte/phy/phch/ra.h | 23 +----------------------
 lib/include/srslte/phy/rf/rf.h   | 17 +----------------
 2 files changed, 2 insertions(+), 38 deletions(-)

diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 58e9585a5..9f31a0218 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -208,28 +208,7 @@ SRSLTE_API void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant,
                                             uint32_t sf_idx,
                                             srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS]);
 
-SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
-                                                  uint32_t cfi,
-                                                  srslte_cell_t cell,
-                                                  uint32_t sf_idx,
-                                                  srslte_ra_nbits_t *nbits,
-                                                  srslte_ra_nbits_t *nbits2);
-
-SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
-                                                  uint32_t cfi,
-                                                  srslte_cell_t cell,
-                                                  uint32_t sf_idx,
-                                                  srslte_ra_nbits_t *nbits,
-                                                  srslte_ra_nbits_t *nbits2);
-
-SRSLTE_API void srslte_ra_dl_grant_to_nbits_multi(srslte_ra_dl_grant_t *grant,
-                                                  uint32_t cfi,
-                                                  srslte_cell_t cell,
-                                                  uint32_t sf_idx,
-                                                  srslte_ra_nbits_t *nbits,
-                                                  srslte_ra_nbits_t *nbits2);
-
-SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, 
+SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell,
                                                uint32_t nof_prb, 
                                                uint32_t nof_ctrl_symbols); 
 
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index 50ced76d9..860e0a257 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -75,22 +75,7 @@ SRSLTE_API int srslte_rf_open_multi(srslte_rf_t *h,
                                     char *args, 
                                     uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
-                                     char *args,
-                                     uint32_t nof_tx_antennas,
-                                     uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
-                                     char *args,
-                                     uint32_t nof_tx_antennas,
-                                     uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_rf_open_multi2(srslte_rf_t *h,
-                                     char *args,
-                                     uint32_t nof_tx_antennas,
-                                     uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h, 
+SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h,
                                       char *devname, 
                                       char *args);
 

From 25a239753438decd2534a1a49e899a0a57d23820 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 14:23:59 +0200
Subject: [PATCH 113/269] Removed .gitignore

---
 .gitignore | 21 ---------------------
 1 file changed, 21 deletions(-)
 delete mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 694c93182..000000000
--- a/.gitignore
+++ /dev/null
@@ -1,21 +0,0 @@
-# Ignore build directories
-build
-debug
-cmake-build-debug
-
-# Ignore CLion IDE project files
-.idea
-
-# Generated data files
-*.dat
-
-# Temporal files
-*~
-
-# Internal development
-matlab
-mex
-cmake/modules/BuildMex.cmake
-cmake/modules/FindMATLAB.cmake
-cmake/modules/FindOCTAVE.cmake
-

From b027ec3bda16182da9b0fab88a8b12fff6209dfc Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 14:32:50 +0200
Subject: [PATCH 114/269] srslte_str2 functions convert to lower or capital
 letters

---
 lib/src/phy/common/phy_common.c | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index cfb6d717b..977ebc5b1 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -125,6 +125,11 @@ bool srslte_N_id_1_isvalid(uint32_t N_id_1) {
 }
 
 srslte_mod_t srslte_str2mod (char * mod_str) {
+  int i = 0;
+
+  /* Upper case */
+  while (mod_str[i] &= (~' '), mod_str[++i]);
+
   if (!strcmp(mod_str, "QPSK")) {
     return SRSLTE_MOD_QPSK;
   } else if (!strcmp(mod_str, "16QAM")) {
@@ -437,13 +442,18 @@ struct lte_band lte_bands[SRSLTE_NOF_LTE_BANDS] = {
 
 
 int srslte_str2mimotype(char *mimo_type_str, srslte_mimo_type_t *type) {
-  if (!strcmp(mimo_type_str, "single") || !strcmp(mimo_type_str, "Port0")) {
+  int i = 0;
+
+  /* Low case */
+  while (mimo_type_str[i] |= ' ', mimo_type_str[++i]);
+
+  if (!strcmp(mimo_type_str, "single") || !strcmp(mimo_type_str, "port0")) {
     *type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-  } else if (!strcmp(mimo_type_str, "diversity") || !strcmp(mimo_type_str, "TxDiversity")) {
+  } else if (!strcmp(mimo_type_str, "diversity") || !strcmp(mimo_type_str, "txdiversity")) {
     *type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-  } else if (!strcmp(mimo_type_str, "multiplex") || !strcmp(mimo_type_str, "SpatialMux")) {
+  } else if (!strcmp(mimo_type_str, "multiplex") || !strcmp(mimo_type_str, "spatialmux")) {
     *type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-  } else if (!strcmp(mimo_type_str, "cdd") || !strcmp(mimo_type_str, "CDD")) {
+  } else if (!strcmp(mimo_type_str, "cdd")) {
     *type = SRSLTE_MIMO_TYPE_CDD;
   } else {
     return SRSLTE_ERROR;

From c52023e4cc1e5a83077e91d97699d6f98be58899 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 31 Aug 2017 16:40:49 +0200
Subject: [PATCH 115/269] Solved PCH downlink bug

---
 srsue/src/mac/mac.cc | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 1bc335bb1..7c569358a 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -282,6 +282,9 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     action->generate_ack = false; 
     action->decode_enabled = true; 
+    srslte_softbuffer_rx_reset_cb(&pch_softbuffer, 1);
+    action->payload_ptr[0] = pch_payload_buffer;
+    action->softbuffers[0]  = &pch_softbuffer;
     action->rnti = grant.rnti;
     action->rv[0]   = grant.rv[0];
     if (grant.n_bytes[0] > pch_payload_buffer_sz) {

From c5bc3b11b70dea91f7f4faa819b15d9736cfebbd Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 31 Aug 2017 18:07:54 +0200
Subject: [PATCH 116/269] remove spaces

---
 srsue/hdr/upper/nas.h      |  192 +--
 srsue/hdr/upper/rrc.h      |  326 ++---
 srsue/src/phy/phch_recv.cc |  932 ++++++-------
 srsue/src/upper/nas.cc     | 1004 +++++++-------
 srsue/src/upper/rrc.cc     | 2588 ++++++++++++++++++------------------
 5 files changed, 2521 insertions(+), 2521 deletions(-)

diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index edf5dcf27..1640c9d8a 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -39,145 +39,145 @@ using srslte::byte_buffer_t;
 namespace srsue {
 
 // EMM states (3GPP 24.302 v10.0.0)
-  typedef enum {
-    EMM_STATE_NULL = 0,
-    EMM_STATE_DEREGISTERED,
-    EMM_STATE_REGISTERED_INITIATED,
-    EMM_STATE_REGISTERED,
-    EMM_STATE_SERVICE_REQUEST_INITIATED,
-    EMM_STATE_DEREGISTERED_INITIATED,
-    EMM_STATE_TAU_INITIATED,
-    EMM_STATE_N_ITEMS,
-  } emm_state_t;
-  static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
-                                                              "DEREGISTERED",
-                                                              "REGISTERED INITIATED",
-                                                              "REGISTERED",
-                                                              "SERVICE REQUEST INITIATED",
-                                                              "DEREGISTERED INITIATED",
-                                                              "TRACKING AREA UPDATE INITIATED"};
+typedef enum {
+  EMM_STATE_NULL = 0,
+  EMM_STATE_DEREGISTERED,
+  EMM_STATE_REGISTERED_INITIATED,
+  EMM_STATE_REGISTERED,
+  EMM_STATE_SERVICE_REQUEST_INITIATED,
+  EMM_STATE_DEREGISTERED_INITIATED,
+  EMM_STATE_TAU_INITIATED,
+  EMM_STATE_N_ITEMS,
+} emm_state_t;
+static const char emm_state_text[EMM_STATE_N_ITEMS][100] = {"NULL",
+                                                            "DEREGISTERED",
+                                                            "REGISTERED INITIATED",
+                                                            "REGISTERED",
+                                                            "SERVICE REQUEST INITIATED",
+                                                            "DEREGISTERED INITIATED",
+                                                            "TRACKING AREA UPDATE INITIATED"};
 
-  typedef enum {
-    PLMN_NOT_SELECTED = 0,
-    PLMN_SELECTED
-  } plmn_selection_state_t;
+typedef enum {
+  PLMN_NOT_SELECTED = 0,
+  PLMN_SELECTED
+} plmn_selection_state_t;
 
-  class nas
-    : public nas_interface_rrc, public nas_interface_ue {
-  public:
-    nas();
+class nas
+  : public nas_interface_rrc, public nas_interface_ue {
+public:
+  nas();
 
-    void init(usim_interface_nas *usim_,
-              rrc_interface_nas *rrc_,
-              gw_interface_nas *gw_,
-              srslte::log *nas_log_);
+  void init(usim_interface_nas *usim_,
+            rrc_interface_nas *rrc_,
+            gw_interface_nas *gw_,
+            srslte::log *nas_log_);
 
-    void stop();
+  void stop();
 
-    emm_state_t get_state();
+  emm_state_t get_state();
 
-    // RRC interface
-    void notify_connection_setup();
+  // RRC interface
+  void notify_connection_setup();
 
-    void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+  void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
 
-    uint32_t get_ul_count();
+  uint32_t get_ul_count();
 
-    bool is_attached();
+  bool is_attached();
 
-    bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
+  bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
 
-    void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
+  void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
 
-    void cell_selected();
+  void cell_selected();
 
-    // UE interface
-    void attach_request();
+  // UE interface
+  void attach_request();
 
-    void deattach_request();
+  void deattach_request();
 
-  private:
-    srslte::byte_buffer_pool *pool;
-    srslte::log *nas_log;
-    rrc_interface_nas *rrc;
-    usim_interface_nas *usim;
-    gw_interface_nas *gw;
+private:
+  srslte::byte_buffer_pool *pool;
+  srslte::log *nas_log;
+  rrc_interface_nas *rrc;
+  usim_interface_nas *usim;
+  gw_interface_nas *gw;
 
-    emm_state_t state;
+  emm_state_t state;
 
-    plmn_selection_state_t plmn_selection;
-    LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
-    LIBLTE_RRC_PLMN_IDENTITY_STRUCT home_plmn;
+  plmn_selection_state_t plmn_selection;
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT home_plmn;
 
-    std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
+  std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
 
-    // Save short MAC
+  // Save short MAC
 
-    // Identifiers
-    LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
-    bool is_guti_set;
+  // Identifiers
+  LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT guti;
+  bool is_guti_set;
 
-    uint32_t ip_addr;
-    uint8_t eps_bearer_id;
+  uint32_t ip_addr;
+  uint8_t eps_bearer_id;
 
-    uint8_t transaction_id;
+  uint8_t transaction_id;
 
-    // NAS counters - incremented for each security-protected message recvd/sent
-    uint32_t count_ul;
-    uint32_t count_dl;
+  // NAS counters - incremented for each security-protected message recvd/sent
+  uint32_t count_ul;
+  uint32_t count_dl;
 
-    // Security
-    uint8_t ksi;
-    uint8_t k_nas_enc[32];
-    uint8_t k_nas_int[32];
+  // Security
+  uint8_t ksi;
+  uint8_t k_nas_enc[32];
+  uint8_t k_nas_int[32];
 
-    srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
-    srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
+  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
+  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
-    void integrity_generate(uint8_t *key_128,
-                            uint32_t count,
-                            uint8_t rb_id,
-                            uint8_t direction,
-                            uint8_t *msg,
-                            uint32_t msg_len,
-                            uint8_t *mac);
+  void integrity_generate(uint8_t *key_128,
+                          uint32_t count,
+                          uint8_t rb_id,
+                          uint8_t direction,
+                          uint8_t *msg,
+                          uint32_t msg_len,
+                          uint8_t *mac);
 
-    void integrity_check();
+  void integrity_check();
 
-    void cipher_encrypt();
+  void cipher_encrypt();
 
-    void cipher_decrypt();
+  void cipher_decrypt();
 
-    // Parsers
-    void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
+  // Parsers
+  void parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_identity_request(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_service_reject(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_emm_information(uint32_t lcid, byte_buffer_t *pdu);
 
-    // Senders
-    void send_attach_request();
+  // Senders
+  void send_attach_request();
 
-    void send_identity_response();
+  void send_identity_response();
 
-    void send_service_request();
+  void send_service_request();
 
-    void send_esm_information_response();
+  void send_esm_information_response();
 
-    void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
-  };
+  void gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg);
+};
 
 } // namespace srsue
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 225659d6f..793c2ee4f 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -43,230 +43,230 @@ using srslte::byte_buffer_t;
 namespace srsue {
 
 // RRC states (3GPP 36.331 v10.0.0)
-  typedef enum {
-    RRC_STATE_IDLE = 0,
-    RRC_STATE_PLMN_SELECTION,
-    RRC_STATE_CELL_SELECTING,
-    RRC_STATE_CELL_SELECTED,
-    RRC_STATE_CONNECTING,
-    RRC_STATE_CONNECTED,
-    RRC_STATE_N_ITEMS,
-  } rrc_state_t;
-  static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
-                                                              "PLMN SELECTION",
-                                                              "CELL SELECTION",
-                                                              "CONNECTING",
-                                                              "CONNECTED",
-                                                              "RRC CONNECTED"};
-  typedef enum {
-    SI_ACQUIRE_IDLE = 0,
-    SI_ACQUIRE_SIB1,
-    SI_ACQUIRE_SIB2
-  } si_acquire_state_t;
-
-
-  class rrc
-    : public rrc_interface_nas,
-      public rrc_interface_phy,
-      public rrc_interface_mac,
-      public rrc_interface_gw,
-      public rrc_interface_pdcp,
-      public rrc_interface_rlc,
-      public srslte::timer_callback,
-      public thread
-  {
-  public:
-    rrc();
-
-    void init(phy_interface_rrc *phy_,
-              mac_interface_rrc *mac_,
-              rlc_interface_rrc *rlc_,
-              pdcp_interface_rrc *pdcp_,
-              nas_interface_rrc *nas_,
-              usim_interface_rrc *usim_,
-              srslte::mac_interface_timers *mac_timers_,
-              srslte::log *rrc_log_);
-
-    void stop();
-
-    rrc_state_t get_state();
-
-    void set_ue_category(int category);
-
-    // Timeout callback interface
-    void timer_expired(uint32_t timeout_id);
-
-    void test_con_restablishment();
+typedef enum {
+  RRC_STATE_IDLE = 0,
+  RRC_STATE_PLMN_SELECTION,
+  RRC_STATE_CELL_SELECTING,
+  RRC_STATE_CELL_SELECTED,
+  RRC_STATE_CONNECTING,
+  RRC_STATE_CONNECTED,
+  RRC_STATE_N_ITEMS,
+} rrc_state_t;
+static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
+                                                            "PLMN SELECTION",
+                                                            "CELL SELECTION",
+                                                            "CONNECTING",
+                                                            "CONNECTED",
+                                                            "RRC CONNECTED"};
+typedef enum {
+  SI_ACQUIRE_IDLE = 0,
+  SI_ACQUIRE_SIB1,
+  SI_ACQUIRE_SIB2
+} si_acquire_state_t;
+
+
+class rrc
+  : public rrc_interface_nas,
+    public rrc_interface_phy,
+    public rrc_interface_mac,
+    public rrc_interface_gw,
+    public rrc_interface_pdcp,
+    public rrc_interface_rlc,
+    public srslte::timer_callback,
+    public thread
+{
+public:
+  rrc();
+
+  void init(phy_interface_rrc *phy_,
+            mac_interface_rrc *mac_,
+            rlc_interface_rrc *rlc_,
+            pdcp_interface_rrc *pdcp_,
+            nas_interface_rrc *nas_,
+            usim_interface_rrc *usim_,
+            srslte::mac_interface_timers *mac_timers_,
+            srslte::log *rrc_log_);
+
+  void stop();
+
+  rrc_state_t get_state();
+
+  void set_ue_category(int category);
+
+  // Timeout callback interface
+  void timer_expired(uint32_t timeout_id);
+
+  void test_con_restablishment();
 
-    void liblte_rrc_log(char *str);
+  void liblte_rrc_log(char *str);
 
-  private:
-    srslte::byte_buffer_pool *pool;
-    srslte::log *rrc_log;
-    phy_interface_rrc *phy;
-    mac_interface_rrc *mac;
-    rlc_interface_rrc *rlc;
-    pdcp_interface_rrc *pdcp;
-    nas_interface_rrc *nas;
-    usim_interface_rrc *usim;
+private:
+  srslte::byte_buffer_pool *pool;
+  srslte::log *rrc_log;
+  phy_interface_rrc *phy;
+  mac_interface_rrc *mac;
+  rlc_interface_rrc *rlc;
+  pdcp_interface_rrc *pdcp;
+  nas_interface_rrc *nas;
+  usim_interface_rrc *usim;
 
-    srslte::bit_buffer_t bit_buf;
+  srslte::bit_buffer_t bit_buf;
 
-    pthread_mutex_t mutex;
+  pthread_mutex_t mutex;
 
-    rrc_state_t state;
-    uint8_t transaction_id;
-    bool drb_up;
+  rrc_state_t state;
+  uint8_t transaction_id;
+  bool drb_up;
 
 
-    uint8_t k_rrc_enc[32];
-    uint8_t k_rrc_int[32];
-    uint8_t k_up_enc[32];
-    uint8_t k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
+  uint8_t k_rrc_enc[32];
+  uint8_t k_rrc_int[32];
+  uint8_t k_up_enc[32];
+  uint8_t k_up_int[32];   // Not used: only for relay nodes (3GPP 33.401 Annex A.7)
 
-    srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
-    srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
+  srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
+  srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
-    std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT> srbs;
-    std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT> drbs;
+  std::map<uint32_t, LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT> srbs;
+  std::map<uint32_t, LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT> drbs;
 
-    LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
-    LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
+  LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
+  LIBLTE_RRC_DL_DCCH_MSG_STRUCT dl_dcch_msg;
 
-    // RRC constants and timers
-    srslte::mac_interface_timers *mac_timers;
-    uint32_t n310_cnt, N310;
-    uint32_t n311_cnt, N311;
-    uint32_t t301, t310, t311;
-    uint32_t safe_reset_timer;
-    int ue_category;
+  // RRC constants and timers
+  srslte::mac_interface_timers *mac_timers;
+  uint32_t n310_cnt, N310;
+  uint32_t n311_cnt, N311;
+  uint32_t t301, t310, t311;
+  uint32_t safe_reset_timer;
+  int ue_category;
 
-    typedef struct {
-      uint32_t earfcn;
-      srslte_cell_t phy_cell;
-      float    rsrp;
-      bool     has_valid_sib1;
-      bool     has_valid_sib2;
-      LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
-      LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
-    } cell_t;
+  typedef struct {
+    uint32_t earfcn;
+    srslte_cell_t phy_cell;
+    float    rsrp;
+    bool     has_valid_sib1;
+    bool     has_valid_sib2;
+    LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
+    LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
+  } cell_t;
 
-    std::vector<cell_t> known_cells;
-    cell_t *current_cell;
+  std::vector<cell_t> known_cells;
+  cell_t *current_cell;
 
-    si_acquire_state_t si_acquire_state;
+  si_acquire_state_t si_acquire_state;
 
-    void select_next_cell_in_plmn();
-    LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
-    int last_selected_cell;
+  void select_next_cell_in_plmn();
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
+  int last_selected_cell;
 
-    bool thread_running;
-    void run_thread();
+  bool thread_running;
+  void run_thread();
 
-    // NAS interface
-    void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
+  // NAS interface
+  void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
 
-    uint16_t get_mcc();
+  uint16_t get_mcc();
 
-    uint16_t get_mnc();
+  uint16_t get_mnc();
 
-    void enable_capabilities();
-    void plmn_search();
-    void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
-    void connect();
+  void enable_capabilities();
+  void plmn_search();
+  void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
+  void connect();
 
-    // PHY interface
-    void in_sync();
+  // PHY interface
+  void in_sync();
 
-    void out_of_sync();
-    void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
+  void out_of_sync();
+  void cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp);
 
-    // MAC interface
-    void release_pucch_srs();
+  // MAC interface
+  void release_pucch_srs();
 
-    void ra_problem();
+  void ra_problem();
 
-    // GW interface
-    bool is_connected();
+  // GW interface
+  bool is_connected();
 
-    bool have_drb();
+  bool have_drb();
 
-    // PDCP interface
-    void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+  // PDCP interface
+  void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
 
-    void write_pdu_bcch_bch(byte_buffer_t *pdu);
+  void write_pdu_bcch_bch(byte_buffer_t *pdu);
 
-    void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
+  void write_pdu_bcch_dlsch(byte_buffer_t *pdu);
 
-    void write_pdu_pcch(byte_buffer_t *pdu);
+  void write_pdu_pcch(byte_buffer_t *pdu);
 
-    // RLC interface
-    void max_retx_attempted();
+  // RLC interface
+  void max_retx_attempted();
 
-    // Senders
-    void send_con_request();
+  // Senders
+  void send_con_request();
 
-    void send_con_restablish_request();
+  void send_con_restablish_request();
 
-    void send_con_restablish_complete();
+  void send_con_restablish_complete();
 
-    void send_con_setup_complete(byte_buffer_t *nas_msg);
+  void send_con_setup_complete(byte_buffer_t *nas_msg);
 
-    void send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
+  void send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu);
 
-    void send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
+  void send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu);
 
-    void send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
+  void send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu);
 
-    void send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
+  void send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu);
 
-    // Parsers
-    void parse_dl_ccch(byte_buffer_t *pdu);
+  // Parsers
+  void parse_dl_ccch(byte_buffer_t *pdu);
 
-    void parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
 
-    void parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
+  void parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
 
-    // Helpers
-    void reset_ue();
+  // Helpers
+  void reset_ue();
 
-    void rrc_connection_release();
+  void rrc_connection_release();
 
-    void radio_link_failure();
+  void radio_link_failure();
 
-    uint32_t sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
+  uint32_t sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
 
-    void apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
+  void apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
 
-    void handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
+  void handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
 
-    void handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
+  void handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
 
-    void
-    handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
+  void
+  handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
 
-    void add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
+  void add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
 
-    void add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
+  void add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg);
 
-    void release_drb(uint8_t lcid);
+  void release_drb(uint8_t lcid);
 
-    void apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg);
+  void apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg);
 
-    void apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults);
+  void apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults);
 
-    void apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cfg, bool apply_defaults);
+  void apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cfg, bool apply_defaults);
 
-    // Helpers for setting default values
-    void set_phy_default_pucch_srs();
+  // Helpers for setting default values
+  void set_phy_default_pucch_srs();
 
-    void set_phy_default();
+  void set_phy_default();
 
-    void set_mac_default();
+  void set_mac_default();
 
-    void set_rrc_default();
+  void set_rrc_default();
 
-  };
+};
 
 } // namespace srsue
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 3dedcb606..3662f4d24 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -40,576 +40,576 @@
 namespace srsue {
 
 
-  phch_recv::phch_recv() {
-    running = false;
-  }
+phch_recv::phch_recv() {
+  running = false;
+}
 
-  void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
-                       prach *_prach_buffer, srslte::thread_pool *_workers_pool,
-                       phch_common *_worker_com, srslte::log *_log_h, uint32_t nof_rx_antennas_, uint32_t prio,
-                       int sync_cpu_affinity) {
-    radio_h = _radio_handler;
-    log_h = _log_h;
-    mac = _mac;
-    rrc = _rrc;
-    workers_pool = _workers_pool;
-    worker_com = _worker_com;
-    prach_buffer = _prach_buffer;
-    nof_rx_antennas = nof_rx_antennas_;
-
-    tx_mutex_cnt = 0;
-    running = true;
-    phy_state = IDLE;
-    time_adv_sec = 0;
-    cell_is_set = false;
-    sync_sfn_cnt = 0;
-    srate_mode = SRATE_NONE;
-    cell_search_in_progress = false;
+void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
+                     prach *_prach_buffer, srslte::thread_pool *_workers_pool,
+                     phch_common *_worker_com, srslte::log *_log_h, uint32_t nof_rx_antennas_, uint32_t prio,
+                     int sync_cpu_affinity) {
+  radio_h = _radio_handler;
+  log_h = _log_h;
+  mac = _mac;
+  rrc = _rrc;
+  workers_pool = _workers_pool;
+  worker_com = _worker_com;
+  prach_buffer = _prach_buffer;
+  nof_rx_antennas = nof_rx_antennas_;
+
+  tx_mutex_cnt = 0;
+  running = true;
+  phy_state = IDLE;
+  time_adv_sec = 0;
+  cell_is_set = false;
+  sync_sfn_cnt = 0;
+  srate_mode = SRATE_NONE;
+  cell_search_in_progress = false;
+
+  for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+    sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
+  }
 
-    for (uint32_t i = 0; i < nof_rx_antennas; i++) {
-      sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
-    }
+  nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
+  worker_com->set_nof_mutex(nof_tx_mutex);
+  if (sync_cpu_affinity < 0) {
+    start(prio);
+  } else {
+    start_cpu(prio, sync_cpu_affinity);
+  }
 
-    nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
-    worker_com->set_nof_mutex(nof_tx_mutex);
-    if (sync_cpu_affinity < 0) {
-      start(prio);
-    } else {
-      start_cpu(prio, sync_cpu_affinity);
-    }
 
+}
 
+void phch_recv::stop() {
+  running = false;
+  wait_thread_finish();
+  for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
   }
+}
 
-  void phch_recv::stop() {
-    running = false;
-    wait_thread_finish();
-    for (uint32_t i = 0; i < nof_rx_antennas; i++) {
-      if (sf_buffer[i]) {
-        free(sf_buffer[i]);
-      }
+void phch_recv::set_agc_enable(bool enable) {
+  do_agc = enable;
+}
+
+int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
+  srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
+  if (radio_h->rx_now(data, nsamples, rx_time)) {
+    int offset = nsamples - radio_h->get_tti_len();
+    if (abs(offset) < 10 && offset != 0) {
+      radio_h->tx_offset(offset);
+    } else if (nsamples < 10) {
+      radio_h->tx_offset(nsamples);
     }
+    return nsamples;
+  } else {
+    return -1;
   }
+}
 
-  void phch_recv::set_agc_enable(bool enable) {
-    do_agc = enable;
-  }
+double callback_set_rx_gain(void *h, double gain) {
+  srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
+  return radio_handler->set_rx_gain_th(gain);
+}
 
-  int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
-    srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
-    if (radio_h->rx_now(data, nsamples, rx_time)) {
-      int offset = nsamples - radio_h->get_tti_len();
-      if (abs(offset) < 10 && offset != 0) {
-        radio_h->tx_offset(offset);
-      } else if (nsamples < 10) {
-        radio_h->tx_offset(nsamples);
-      }
-      return nsamples;
-    } else {
-      return -1;
-    }
+void phch_recv::set_time_adv_sec(float _time_adv_sec) {
+  time_adv_sec = _time_adv_sec;
+}
+
+void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
+  if (worker_com->args->cfo_integer_enabled) {
+    srslte_ue_sync_cfo_i_detec_en(q, true);
   }
 
-  double callback_set_rx_gain(void *h, double gain) {
-    srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
-    return radio_handler->set_rx_gain_th(gain);
+  float cfo_tol = worker_com->args->cfo_correct_tol_hz;
+  srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol / (15000 * q->fft_size));
+  srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol / (15000 * q->fft_size));
+
+  int time_correct_period = worker_com->args->time_correct_period;
+  if (time_correct_period > 0) {
+    srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);
   }
 
-  void phch_recv::set_time_adv_sec(float _time_adv_sec) {
-    time_adv_sec = _time_adv_sec;
+  sss_alg_t sss_alg = SSS_FULL;
+  if (!worker_com->args->sss_algorithm.compare("diff")) {
+    sss_alg = SSS_DIFF;
+  } else if (!worker_com->args->sss_algorithm.compare("partial")) {
+    sss_alg = SSS_PARTIAL_3;
+  } else if (!worker_com->args->sss_algorithm.compare("full")) {
+    sss_alg = SSS_FULL;
+  } else {
+    Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
   }
+  srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg);
+  srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
+}
 
-  void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
-    if (worker_com->args->cfo_integer_enabled) {
-      srslte_ue_sync_cfo_i_detec_en(q, true);
-    }
+bool phch_recv::init_cell() {
+  cell_is_set = false;
+  if (!srslte_ue_mib_init(&ue_mib, cell)) {
+    if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
 
-    float cfo_tol = worker_com->args->cfo_correct_tol_hz;
-    srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol / (15000 * q->fft_size));
-    srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol / (15000 * q->fft_size));
+      // Set options defined in expert section
+      set_ue_sync_opts(&ue_sync);
 
-    int time_correct_period = worker_com->args->time_correct_period;
-    if (time_correct_period > 0) {
-      srslte_ue_sync_set_sample_offset_correct_period(q, time_correct_period);
-    }
+      if (srslte_ue_dl_init_multi(&ue_dl_measure, cell, nof_rx_antennas)) {
+        Error("Setting cell: initiating ue_dl_measure\n");
+        return false;
+      }
 
-    sss_alg_t sss_alg = SSS_FULL;
-    if (!worker_com->args->sss_algorithm.compare("diff")) {
-      sss_alg = SSS_DIFF;
-    } else if (!worker_com->args->sss_algorithm.compare("partial")) {
-      sss_alg = SSS_PARTIAL_3;
-    } else if (!worker_com->args->sss_algorithm.compare("full")) {
-      sss_alg = SSS_FULL;
+      for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+        if (!((phch_worker *) workers_pool->get_worker(i))->init_cell(cell)) {
+          Error("Setting cell: initiating PHCH worker\n");
+          return false;
+        }
+      }
+      radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
+      if (do_agc) {
+        srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
+      }
+      srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
+      cell_is_set = true;
     } else {
-      Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
+      Error("Error setting cell: initiating ue_sync");
     }
-    srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg);
-    srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
+  } else {
+    Error("Error setting cell: initiating ue_mib\n");
   }
+  return cell_is_set;
+}
 
-  bool phch_recv::init_cell() {
-    cell_is_set = false;
-    if (!srslte_ue_mib_init(&ue_mib, cell)) {
-      if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+void phch_recv::free_cell() {
+  if (phy_state != IDLE) {
+    phy_state = IDLE;
+    usleep(2000);
+  }
 
-        // Set options defined in expert section
-        set_ue_sync_opts(&ue_sync);
+  srslte_ue_sync_free(&ue_sync);
 
-        if (srslte_ue_dl_init_multi(&ue_dl_measure, cell, nof_rx_antennas)) {
-          Error("Setting cell: initiating ue_dl_measure\n");
-          return false;
-        }
+  srslte_ue_dl_free(&ue_dl_measure);
 
-        for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
-          if (!((phch_worker *) workers_pool->get_worker(i))->init_cell(cell)) {
-            Error("Setting cell: initiating PHCH worker\n");
-            return false;
-          }
-        }
-        radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        if (do_agc) {
-          srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
-        }
-        srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
-        cell_is_set = true;
-      } else {
-        Error("Error setting cell: initiating ue_sync");
-      }
-    } else {
-      Error("Error setting cell: initiating ue_mib\n");
+  if (cell_is_set) {
+    for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+      ((phch_worker *) workers_pool->get_worker(i))->free_cell();
     }
-    return cell_is_set;
+    prach_buffer->free_cell();
+    cell_is_set = false;
   }
+}
 
-  void phch_recv::free_cell() {
-    if (phy_state != IDLE) {
-      phy_state = IDLE;
-      usleep(2000);
-    }
 
-    srslte_ue_sync_free(&ue_sync);
+bool phch_recv::cell_search(int force_N_id_2) {
+  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+  uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN / 8];
 
-    srslte_ue_dl_free(&ue_dl_measure);
+  srslte_ue_cellsearch_result_t found_cells[3];
+  srslte_ue_cellsearch_t cs;
 
-    if (cell_is_set) {
-      for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
-        ((phch_worker *) workers_pool->get_worker(i))->free_cell();
-      }
-      prach_buffer->free_cell();
-      cell_is_set = false;
-    }
+  bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
+
+  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
+                                      radio_h)) {
+    Error("Initiating UE cell search\n");
+    return false;
   }
 
+  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
 
-  bool phch_recv::cell_search(int force_N_id_2) {
-    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
-    uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN / 8];
+  // Set options defined in expert section
+  set_ue_sync_opts(&cs.ue_sync);
 
-    srslte_ue_cellsearch_result_t found_cells[3];
-    srslte_ue_cellsearch_t cs;
+  if (do_agc) {
+    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
+  }
 
-    bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
+  if (srate_mode != SRATE_FIND) {
+    srate_mode = SRATE_FIND;
+    radio_h->set_rx_srate(1.92e6);
+  }
+  radio_h->start_rx();
 
-    if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
-                                        radio_h)) {
-      Error("Initiating UE cell search\n");
-      return false;
-    }
+  /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
+  uint32_t max_peak_cell = 0;
+  int ret = SRSLTE_ERROR;
 
-    srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
+  if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
+    ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
+    max_peak_cell = force_N_id_2;
+  } else {
+    ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell);
+  }
 
-    // Set options defined in expert section
-    set_ue_sync_opts(&cs.ue_sync);
+  last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
-    if (do_agc) {
-      srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
-    }
+  radio_h->stop_rx();
+  srslte_ue_cellsearch_free(&cs);
 
-    if (srate_mode != SRATE_FIND) {
-      srate_mode = SRATE_FIND;
-      radio_h->set_rx_srate(1.92e6);
-    }
-    radio_h->start_rx();
+  if (ret < 0) {
+    Error("Error decoding MIB: Error searching PSS\n");
+    return false;
+  } else if (ret == 0) {
+    Error("Error decoding MIB: Could not find any PSS in this frequency\n");
+    return false;
+  }
 
-    /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
-    uint32_t max_peak_cell = 0;
-    int ret = SRSLTE_ERROR;
+  // Save result
+  cell.id = found_cells[max_peak_cell].cell_id;
+  cell.cp = found_cells[max_peak_cell].cp;
+  cellsearch_cfo = found_cells[max_peak_cell].cfo;
 
-    if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
-      ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
-      max_peak_cell = force_N_id_2;
-    } else {
-      ret = srslte_ue_cellsearch_scan(&cs, found_cells, &max_peak_cell);
-    }
+  srslte_ue_mib_sync_t ue_mib_sync;
 
-    last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
+  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas,
+                                    radio_h)) {
+    Error("Initiating UE MIB synchronization\n");
+    return false;
+  }
 
-    radio_h->stop_rx();
-    srslte_ue_cellsearch_free(&cs);
+  // Set options defined in expert section
+  set_ue_sync_opts(&ue_mib_sync.ue_sync);
 
-    if (ret < 0) {
-      Error("Error decoding MIB: Error searching PSS\n");
-      return false;
-    } else if (ret == 0) {
-      Error("Error decoding MIB: Could not find any PSS in this frequency\n");
-      return false;
-    }
+  if (do_agc) {
+    srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
+  }
 
-    // Save result
-    cell.id = found_cells[max_peak_cell].cell_id;
-    cell.cp = found_cells[max_peak_cell].cp;
-    cellsearch_cfo = found_cells[max_peak_cell].cfo;
+  srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
+
+  /* Find and decode MIB */
+  uint32_t sfn;
+  int sfn_offset;
+  radio_h->start_rx();
+  ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
+                                  SRSLTE_DEFAULT_MAX_FRAMES_PBCH,
+                                  bch_payload, &cell.nof_ports, &sfn_offset);
+  radio_h->stop_rx();
+  last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
+  cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
+  srslte_ue_mib_sync_free(&ue_mib_sync);
+
+  if (ret == 1) {
+    srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
+    worker_com->set_cell(cell);
+    return true;
+  } else {
+    Warning("Error decoding MIB: Error decoding PBCH\n");
+    return false;
+  }
+}
 
-    srslte_ue_mib_sync_t ue_mib_sync;
 
-    if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas,
-                                      radio_h)) {
-      Error("Initiating UE MIB synchronization\n");
-      return false;
-    }
+void phch_recv::resync_sfn() {
+  sync_sfn_cnt = 0;
+  phy_state = CELL_SELECT;
+}
 
-    // Set options defined in expert section
-    set_ue_sync_opts(&ue_mib_sync.ue_sync);
+void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
+  this->earfcn = earfcn;
+}
 
-    if (do_agc) {
-      srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
+void phch_recv::cell_search_next() {
+  cell_search_in_progress = true;
+  cur_earfcn_index++;
+  if (cur_earfcn_index >= 0) {
+    if (cur_earfcn_index >= (int) earfcn.size() - 1) {
+      cur_earfcn_index = 0;
     }
+    // If PHY is running, stop and free resources
+    free_cell();
 
-    srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
-
-    /* Find and decode MIB */
-    uint32_t sfn;
-    int sfn_offset;
-    radio_h->start_rx();
-    ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
-                                    SRSLTE_DEFAULT_MAX_FRAMES_PBCH,
-                                    bch_payload, &cell.nof_ports, &sfn_offset);
-    radio_h->stop_rx();
-    last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
-    cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
-    srslte_ue_mib_sync_free(&ue_mib_sync);
-
-    if (ret == 1) {
-      srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
-      worker_com->set_cell(cell);
-      return true;
+    float dl_freq = 1e6*srslte_band_fd(earfcn[cur_earfcn_index]);
+    if (dl_freq >= 0) {
+      log_h->info("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz, channel_index=%d\n", earfcn[cur_earfcn_index],
+                  dl_freq / 1e6, cur_earfcn_index);
+      radio_h->set_rx_freq(dl_freq);
+
+      // Start PHY cell search (finds maximum cell in frequency)
+      phy_state = CELL_SEARCH;
     } else {
-      Warning("Error decoding MIB: Error decoding PBCH\n");
-      return false;
+      log_h->error("Cell Search: Invalid EARFCN=%d, channel_index=%d\n", earfcn[cur_earfcn_index], cur_earfcn_index);
     }
   }
+}
 
-
-  void phch_recv::resync_sfn() {
-    sync_sfn_cnt = 0;
-    phy_state = CELL_SELECT;
+void phch_recv::cell_search_start() {
+  if (earfcn.size() > 0) {
+    cur_earfcn_index = -1;
+    log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+    log_h->info("Cell Search: Starting procedure...\n");
+    cell_search_next();
+  } else {
+    log_h->info("Empty EARFCN list. Stopping cell search...\n");
+    log_h->console("Empty EARFCN list. Stopping cell search...\n");
   }
+}
 
-  void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
-    this->earfcn = earfcn;
-  }
+bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
+  free_cell();
 
-  void phch_recv::cell_search_next() {
-    cell_search_in_progress = true;
-    cur_earfcn_index++;
-    if (cur_earfcn_index >= 0) {
-      if (cur_earfcn_index >= (int) earfcn.size() - 1) {
-        cur_earfcn_index = 0;
-      }
-      // If PHY is running, stop and free resources
-      free_cell();
-
-      float dl_freq = 1e6*srslte_band_fd(earfcn[cur_earfcn_index]);
-      if (dl_freq >= 0) {
-        log_h->info("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz, channel_index=%d\n", earfcn[cur_earfcn_index],
-                    dl_freq / 1e6, cur_earfcn_index);
-        radio_h->set_rx_freq(dl_freq);
-
-        // Start PHY cell search (finds maximum cell in frequency)
-        phy_state = CELL_SEARCH;
-      } else {
-        log_h->error("Cell Search: Invalid EARFCN=%d, channel_index=%d\n", earfcn[cur_earfcn_index], cur_earfcn_index);
-      }
-    }
+  int cnt=0;
+  while(phy_state == CELL_SEARCH && cnt<100) {
+    usleep(10000);
+    log_h->info("PHY in CELL_SEARCH. Waiting...\n");
+  }
+  if (phy_state==CELL_SEARCH) {
+    log_h->warning("PHY still in CELL_SEARCH, forcing CELL_SELECT...\n");
   }
+  float dl_freq = 1e6*srslte_band_fd(earfcn);
+  float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(earfcn));
+  if (dl_freq >= 0 || ul_freq <= 0) {
+    log_h->info("Cell Select: Set EARFCN=%d, frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn, dl_freq / 1e6,
+                ul_freq / 1e6);
+    radio_h->set_rx_freq(dl_freq);
+    radio_h->set_tx_freq(ul_freq);
+
+    ul_dl_factor = ul_freq/dl_freq;
 
-  void phch_recv::cell_search_start() {
-    if (earfcn.size() > 0) {
-      cur_earfcn_index = -1;
-      log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
-      log_h->info("Cell Search: Starting procedure...\n");
-      cell_search_next();
+    cell_search_in_progress = false;
+    this->cell = cell;
+    if (init_cell()) {
+      phy_state = CELL_SELECT;
+      return true;
     } else {
-      log_h->info("Empty EARFCN list. Stopping cell search...\n");
-      log_h->console("Empty EARFCN list. Stopping cell search...\n");
+      log_h->error("Cell Select: Initializing cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
     }
+  } else {
+    log_h->error("Cell Select: Invalid EARFCN=%d\n", earfcn);
   }
+  return false;
+}
 
-  bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
-    free_cell();
 
-    int cnt=0;
-    while(phy_state == CELL_SEARCH && cnt<100) {
-      usleep(10000);
-      log_h->info("PHY in CELL_SEARCH. Waiting...\n");
-    }
-    if (phy_state==CELL_SEARCH) {
-      log_h->warning("PHY still in CELL_SEARCH, forcing CELL_SELECT...\n");
-    }
-    float dl_freq = 1e6*srslte_band_fd(earfcn);
-    float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(earfcn));
-    if (dl_freq >= 0 || ul_freq <= 0) {
-      log_h->info("Cell Select: Set EARFCN=%d, frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn, dl_freq / 1e6,
-                  ul_freq / 1e6);
-      radio_h->set_rx_freq(dl_freq);
-      radio_h->set_tx_freq(ul_freq);
+int phch_recv::cell_sync_sfn(void) {
+
+  int ret = SRSLTE_ERROR;
+  uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+
+  srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+  ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
+  if (ret < 0) {
+    Error("Error calling ue_sync_get_buffer");
+    return -1;
+  }
+
+  if (ret == 1) {
+    if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
+      int sfn_offset = 0;
+      Info("SYNC:  Decoding MIB...\n");
+      int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
+      if (n < 0) {
+        Error("SYNC:  Error decoding MIB while synchronising SFN");
+        return -1;
+      } else if (n == SRSLTE_UE_MIB_FOUND) {
+        uint32_t sfn;
+        srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
 
-      ul_dl_factor = ul_freq/dl_freq;
+        sfn = (sfn+sfn_offset)%1024;
+        tti = sfn * 10;
 
-      cell_search_in_progress = false;
-      this->cell = cell;
-      if (init_cell()) {
-        phy_state = CELL_SELECT;
-        return true;
-      } else {
-        log_h->error("Cell Select: Initializing cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
+        srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+        Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
+        srslte_ue_mib_reset(&ue_mib);
+        return 1;
       }
-    } else {
-      log_h->error("Cell Select: Invalid EARFCN=%d\n", earfcn);
     }
-    return false;
+  } else {
+    Debug("SYNC:  PSS/SSS not found...\n");
   }
+  return 0;
+}
 
+int phch_recv::cell_meas_rsrp() {
 
-  int phch_recv::cell_sync_sfn(void) {
+  uint32_t cfi = 0;
 
-    int ret = SRSLTE_ERROR;
-    uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
+  tti = (tti+1) % 10240;
+  log_h->step(tti);
 
-    srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-    ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
-    if (ret < 0) {
-      Error("Error calling ue_sync_get_buffer");
+  uint32_t sf_idx = tti%10;
+
+  int sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
+  if (sync_res == 1) {
+    if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
+      log_h->error("SYNC:  Measuring RSRP: Estimating channel\n");
       return -1;
     }
-
-    if (ret == 1) {
-      if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
-        int sfn_offset = 0;
-        Info("SYNC:  Decoding MIB...\n");
-        int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
-        if (n < 0) {
-          Error("SYNC:  Error decoding MIB while synchronising SFN");
-          return -1;
-        } else if (n == SRSLTE_UE_MIB_FOUND) {
-          uint32_t sfn;
-          srslte_pbch_mib_unpack(bch_payload, &cell, &sfn);
-
-          sfn = (sfn+sfn_offset)%1024;
-          tti = sfn * 10;
-
-          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
-          Info("SYNC:  DONE, TTI=%d, sfn_offset=%d\n", tti, sfn_offset);
-          srslte_ue_mib_reset(&ue_mib);
-          return 1;
-        }
-      }
-    } else {
-      Debug("SYNC:  PSS/SSS not found...\n");
+    float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl_measure.chest);
+    measure_rsrp = SRSLTE_VEC_CMA(rsrp, measure_rsrp, measure_cnt);
+    measure_cnt++;
+    log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm\n",
+                measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000));
+    if (measure_cnt >= RSRP_MEASURE_NOF_FRAMES) {
+      return 1;
     }
-    return 0;
+  } else {
+    log_h->error("SYNC:  Measuring RSRP: Sync error\n");
+    return -1;
   }
 
-  int phch_recv::cell_meas_rsrp() {
+  return 0;
+}
 
-    uint32_t cfi = 0;
+void phch_recv::run_thread() {
+  int sync_res;
+  phch_worker *worker = NULL;
+  cf_t *buffer[SRSLTE_MAX_PORTS];
+  while (running) {
+    switch (phy_state) {
+      case CELL_SEARCH:
+        if (cell_search()) {
+          init_cell();
+          float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+
+          if (30720 % ((int) srate / 1000) == 0) {
+            radio_h->set_master_clock_rate(30.72e6);
+          } else {
+            radio_h->set_master_clock_rate(23.04e6);
+          }
 
-    tti = (tti+1) % 10240;
-    log_h->step(tti);
+          log_h->info("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
+          srate_mode = SRATE_CAMP;
+          radio_h->set_rx_srate(srate);
+          radio_h->set_tx_srate(srate);
 
-    uint32_t sf_idx = tti%10;
+          Info("SYNC:  Cell found. Synchronizing...\n");
+          phy_state = CELL_SELECT;
+          sync_sfn_cnt = 0;
+          srslte_ue_mib_reset(&ue_mib);
+        }
+        break;
+      case CELL_SELECT:
 
-    int sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
-    if (sync_res == 1) {
-      if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
-        log_h->error("SYNC:  Measuring RSRP: Estimating channel\n");
-        return -1;
-      }
-      float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl_measure.chest);
-      measure_rsrp = SRSLTE_VEC_CMA(rsrp, measure_rsrp, measure_cnt);
-      measure_cnt++;
-      log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm\n",
-                  measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000));
-      if (measure_cnt >= RSRP_MEASURE_NOF_FRAMES) {
-        return 1;
-      }
-    } else {
-      log_h->error("SYNC:  Measuring RSRP: Sync error\n");
-      return -1;
-    }
+        srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
 
-    return 0;
-  }
+        if (!radio_is_streaming) {
+          // Start streaming
+          radio_h->start_rx();
+          radio_is_streaming = true;
+        }
 
-  void phch_recv::run_thread() {
-    int sync_res;
-    phch_worker *worker = NULL;
-    cf_t *buffer[SRSLTE_MAX_PORTS];
-    while (running) {
-      switch (phy_state) {
-        case CELL_SEARCH:
-          if (cell_search()) {
-            init_cell();
-            float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
-
-            if (30720 % ((int) srate / 1000) == 0) {
-              radio_h->set_master_clock_rate(30.72e6);
+        switch (cell_sync_sfn()) {
+          default:
+            log_h->console("Going IDLE\n");
+            phy_state = IDLE;
+            break;
+          case 1:
+            srslte_ue_sync_set_agc_period(&ue_sync, 20);
+            if (!cell_search_in_progress) {
+              phy_state = CELL_CAMP;
+              log_h->console("Sync OK. Camping on cell PCI=%d...\n", cell.id);
             } else {
-              radio_h->set_master_clock_rate(23.04e6);
+              measure_cnt  = 0;
+              measure_rsrp = 0;
+              phy_state    = CELL_MEASURE;
             }
+            break;
+          case 0:
+            break;
+        }
+        sync_sfn_cnt++;
+        if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
+          sync_sfn_cnt = 0;
+          radio_h->stop_rx();
+          radio_is_streaming = false;
+          log_h->console("Timeout while synchronizing SFN\n");
+          log_h->warning("Timeout while synchronizing SFN\n");
+        }
+        break;
+      case CELL_MEASURE:
+        switch(cell_meas_rsrp()) {
+          case 1:
+            rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
+            phy_state = CELL_CAMP;
+          case 0:
+            break;
+          default:
+            log_h->error("SYNC:  Getting RSRP cell measurement.\n");
+            cell_search_next();
+        }
+        break;
+      case CELL_CAMP:
+        tti = (tti+1) % 10240;
+        worker = (phch_worker *) workers_pool->wait_worker(tti);
+        if (worker) {
+          for (uint32_t i = 0; i < nof_rx_antennas; i++) {
+            buffer[i] = worker->get_buffer(i);
+          }
 
-            log_h->info("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
-            srate_mode = SRATE_CAMP;
-            radio_h->set_rx_srate(srate);
-            radio_h->set_tx_srate(srate);
+          sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer);
+          if (sync_res == 1) {
 
-            Info("SYNC:  Cell found. Synchronizing...\n");
-            phy_state = CELL_SELECT;
-            sync_sfn_cnt = 0;
-            srslte_ue_mib_reset(&ue_mib);
-          }
-          break;
-        case CELL_SELECT:
+            log_h->step(tti);
 
-          srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
+            Debug("Worker %d synchronized\n", worker->get_id());
 
-          if (!radio_is_streaming) {
-            // Start streaming
-            radio_h->start_rx();
-            radio_is_streaming = true;
-          }
+            metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
+            metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
+            worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
+            worker_com->set_sync_metrics(metrics);
 
-          switch (cell_sync_sfn()) {
-            default:
-              log_h->console("Going IDLE\n");
-              phy_state = IDLE;
-              break;
-            case 1:
-              srslte_ue_sync_set_agc_period(&ue_sync, 20);
-              if (!cell_search_in_progress) {
-                phy_state = CELL_CAMP;
-                log_h->console("Sync OK. Camping on cell PCI=%d...\n", cell.id);
-              } else {
-                measure_cnt  = 0;
-                measure_rsrp = 0;
-                phy_state    = CELL_MEASURE;
-              }
-              break;
-            case 0:
-              break;
-          }
-          sync_sfn_cnt++;
-          if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
-            sync_sfn_cnt = 0;
-            radio_h->stop_rx();
-            radio_is_streaming = false;
-            log_h->console("Timeout while synchronizing SFN\n");
-            log_h->warning("Timeout while synchronizing SFN\n");
-          }
-          break;
-        case CELL_MEASURE:
-          switch(cell_meas_rsrp()) {
-            case 1:
-              rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
-              phy_state = CELL_CAMP;
-            case 0:
-              break;
-            default:
-              log_h->error("SYNC:  Getting RSRP cell measurement.\n");
-              cell_search_next();
-          }
-          break;
-        case CELL_CAMP:
-          tti = (tti+1) % 10240;
-          worker = (phch_worker *) workers_pool->wait_worker(tti);
-          if (worker) {
-            for (uint32_t i = 0; i < nof_rx_antennas; i++) {
-              buffer[i] = worker->get_buffer(i);
-            }
+            float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync) / 1000;
+            worker->set_sample_offset(sample_offset);
 
-            sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer);
-            if (sync_res == 1) {
-
-              log_h->step(tti);
-
-              Debug("Worker %d synchronized\n", worker->get_id());
-
-              metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
-              metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
-              worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
-              worker_com->set_sync_metrics(metrics);
-
-              float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync) / 1000;
-              worker->set_sample_offset(sample_offset);
-
-              /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
-              srslte_timestamp_t rx_time, tx_time, tx_time_prach;
-              srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
-              srslte_timestamp_copy(&tx_time, &rx_time);
-              srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
-              worker->set_tx_time(tx_time);
-
-              Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
-              worker->set_tti(tti, tx_mutex_cnt);
-              tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
-
-              // Check if we need to TX a PRACH
-              if (prach_buffer->is_ready_to_send(tti)) {
-                srslte_timestamp_copy(&tx_time_prach, &rx_time);
-                srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
-                prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
-                radio_h->tx_end();
-                worker_com->p0_preamble = prach_buffer->get_p0_preamble();
-                worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
-              }
-              workers_pool->start_worker(worker);
-              // Notify RRC in-sync every 1 frame
-              if ((tti % 10) == 0) {
-                rrc->in_sync();
-                log_h->debug("Sending in-sync to RRC\n");
-              }
-            } else {
-              log_h->console("Sync error.\n");
-              log_h->error("Sync error. Sending out-of-sync to RRC\n");
-              // Notify RRC of out-of-sync frame
-              rrc->out_of_sync();
-              worker->release();
-              worker_com->reset_ul();
-              phy_state = CELL_SELECT;
+            /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
+            srslte_timestamp_t rx_time, tx_time, tx_time_prach;
+            srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
+            srslte_timestamp_copy(&tx_time, &rx_time);
+            srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
+            worker->set_tx_time(tx_time);
+
+            Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+            worker->set_tti(tti, tx_mutex_cnt);
+            tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
+
+            // Check if we need to TX a PRACH
+            if (prach_buffer->is_ready_to_send(tti)) {
+              srslte_timestamp_copy(&tx_time_prach, &rx_time);
+              srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
+              prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
+              radio_h->tx_end();
+              worker_com->p0_preamble = prach_buffer->get_p0_preamble();
+              worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
+            }
+            workers_pool->start_worker(worker);
+            // Notify RRC in-sync every 1 frame
+            if ((tti % 10) == 0) {
+              rrc->in_sync();
+              log_h->debug("Sending in-sync to RRC\n");
             }
           } else {
-            // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
-            running = false;
+            log_h->console("Sync error.\n");
+            log_h->error("Sync error. Sending out-of-sync to RRC\n");
+            // Notify RRC of out-of-sync frame
+            rrc->out_of_sync();
+            worker->release();
+            worker_com->reset_ul();
+            phy_state = CELL_SELECT;
           }
-          break;
-        case IDLE:
-          usleep(1000);
-          break;
-      }
+        } else {
+          // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
+          running = false;
+        }
+        break;
+      case IDLE:
+        usleep(1000);
+        break;
     }
   }
+}
 
-  uint32_t phch_recv::get_current_tti() {
-    return tti;
-  }
+uint32_t phch_recv::get_current_tti() {
+  return tti;
+}
 
-  bool phch_recv::status_is_sync() {
-    return phy_state == CELL_CAMP;
-  }
+bool phch_recv::status_is_sync() {
+  return phy_state == CELL_CAMP;
+}
 
-  void phch_recv::get_current_cell(srslte_cell_t *cell_) {
-    if (cell_) {
-      memcpy(cell_, &cell, sizeof(srslte_cell_t));
-    }
+void phch_recv::get_current_cell(srslte_cell_t *cell_) {
+  if (cell_) {
+    memcpy(cell_, &cell, sizeof(srslte_cell_t));
   }
 }
+}
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index f9bc88064..11372e198 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -33,280 +33,280 @@ using namespace srslte;
 
 namespace srsue {
 
-  nas::nas()
-    : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), is_guti_set(false), ip_addr(0), eps_bearer_id(0),
-      count_ul(0), count_dl(0) {}
-
-  void nas::init(usim_interface_nas *usim_,
-                 rrc_interface_nas *rrc_,
-                 gw_interface_nas *gw_,
-                 srslte::log *nas_log_) {
-    pool = byte_buffer_pool::get_instance();
-    usim = usim_;
-    rrc = rrc_;
-    gw = gw_;
-    nas_log = nas_log_;
-    state = EMM_STATE_DEREGISTERED;
-    plmn_selection = PLMN_NOT_SELECTED;
-    home_plmn.mcc = 61441; // This is 001
-    home_plmn.mnc = 65281; // This is 01
-  }
-
-  void nas::stop() {}
-
-  emm_state_t nas::get_state() {
-    return state;
-  }
+nas::nas()
+  : state(EMM_STATE_DEREGISTERED), plmn_selection(PLMN_SELECTED), is_guti_set(false), ip_addr(0), eps_bearer_id(0),
+    count_ul(0), count_dl(0) {}
+
+void nas::init(usim_interface_nas *usim_,
+               rrc_interface_nas *rrc_,
+               gw_interface_nas *gw_,
+               srslte::log *nas_log_) {
+  pool = byte_buffer_pool::get_instance();
+  usim = usim_;
+  rrc = rrc_;
+  gw = gw_;
+  nas_log = nas_log_;
+  state = EMM_STATE_DEREGISTERED;
+  plmn_selection = PLMN_NOT_SELECTED;
+  home_plmn.mcc = 61441; // This is 001
+  home_plmn.mnc = 65281; // This is 01
+}
+
+void nas::stop() {}
+
+emm_state_t nas::get_state() {
+  return state;
+}
 
 /*******************************************************************************
-  UE interface
+UE interface
 *******************************************************************************/
-  void nas::attach_request() {
-    nas_log->info("Attach Request\n");
-    if (state == EMM_STATE_DEREGISTERED) {
-      state = EMM_STATE_REGISTERED_INITIATED;
-      if (plmn_selection == PLMN_NOT_SELECTED) {
-        nas_log->info("Starting PLMN Search...\n");
-        rrc->plmn_search();
-      } else if (plmn_selection == PLMN_SELECTED) {
-        nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
-        rrc->plmn_select(current_plmn);
-      }
-    } else {
-      nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
+void nas::attach_request() {
+  nas_log->info("Attach Request\n");
+  if (state == EMM_STATE_DEREGISTERED) {
+    state = EMM_STATE_REGISTERED_INITIATED;
+    if (plmn_selection == PLMN_NOT_SELECTED) {
+      nas_log->info("Starting PLMN Search...\n");
+      rrc->plmn_search();
+    } else if (plmn_selection == PLMN_SELECTED) {
+      nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
+      rrc->plmn_select(current_plmn);
     }
+  } else {
+    nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
   }
+}
 
-  void nas::deattach_request() {
-    state = EMM_STATE_DEREGISTERED_INITIATED;
-    nas_log->info("Dettach request not supported\n");
-  }
+void nas::deattach_request() {
+  state = EMM_STATE_DEREGISTERED_INITIATED;
+  nas_log->info("Dettach request not supported\n");
+}
 
 /*******************************************************************************
-  RRC interface
+RRC interface
 *******************************************************************************/
 
-  void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
+void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
 
-    // Store PLMN if not registered
-    for (uint32_t i=0;i<known_plmns.size();i++) {
-      if (plmn_id.mcc == known_plmns[i].mcc && plmn_id.mnc == known_plmns[i].mnc) {
-        nas_log->info("Detected known PLMN %s\n", plmn_id_to_c_str(plmn_id).c_str());
-        if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
-          rrc->plmn_select(plmn_id);
-        }
-        return;
+  // Store PLMN if not registered
+  for (uint32_t i=0;i<known_plmns.size();i++) {
+    if (plmn_id.mcc == known_plmns[i].mcc && plmn_id.mnc == known_plmns[i].mnc) {
+      nas_log->info("Detected known PLMN %s\n", plmn_id_to_c_str(plmn_id).c_str());
+      if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+        rrc->plmn_select(plmn_id);
       }
-    }
-    nas_log->info("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
-                  tracking_area_code);
-    nas_log->console("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
-                  tracking_area_code);
-    if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
-      rrc->plmn_select(plmn_id);
+      return;
     }
   }
-
-  void nas::cell_selected() {
-    if (state == EMM_STATE_REGISTERED_INITIATED) {
-      rrc->connect();
-    } else {
-      nas_log->info("Cell selected in invalid state = %s\n", emm_state_text[state]);
-    }
-  }
-
-  bool nas::is_attached() {
-    return state == EMM_STATE_REGISTERED;
+  nas_log->info("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                tracking_area_code);
+  nas_log->console("Found PLMN:  Id=%s, TAC=%d\n", plmn_id_to_c_str(plmn_id).c_str(),
+                tracking_area_code);
+  if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
+    rrc->plmn_select(plmn_id);
   }
+}
 
-  void nas::notify_connection_setup() {
-    nas_log->debug("State = %s\n", emm_state_text[state]);
-    if (EMM_STATE_REGISTERED_INITIATED == state) {
-      send_attach_request();
-    } else {
-      send_service_request();
-    }
+void nas::cell_selected() {
+  if (state == EMM_STATE_REGISTERED_INITIATED) {
+    rrc->connect();
+  } else {
+    nas_log->info("Cell selected in invalid state = %s\n", emm_state_text[state]);
   }
-
-  void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
-    uint8 pd;
-    uint8 msg_type;
-
-    nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rb_id_text[lcid]);
-
-    // Parse the message
-    liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT *) pdu, &pd, &msg_type);
-    switch (msg_type) {
-      case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
-        parse_attach_accept(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
-        parse_attach_reject(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
-        parse_authentication_request(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
-        parse_authentication_reject(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
-        parse_identity_request(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
-        parse_security_mode_command(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
-        parse_service_reject(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
-        parse_esm_information_request(lcid, pdu);
-        break;
-      case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
-        parse_emm_information(lcid, pdu);
-        break;
-      default:
-        nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n", msg_type);
-        pool->deallocate(pdu);
-        break;
-    }
+}
+
+bool nas::is_attached() {
+  return state == EMM_STATE_REGISTERED;
+}
+
+void nas::notify_connection_setup() {
+  nas_log->debug("State = %s\n", emm_state_text[state]);
+  if (EMM_STATE_REGISTERED_INITIATED == state) {
+    send_attach_request();
+  } else {
+    send_service_request();
   }
-
-  uint32_t nas::get_ul_count() {
-    return count_ul;
+}
+
+void nas::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+  uint8 pd;
+  uint8 msg_type;
+
+  nas_log->info_hex(pdu->msg, pdu->N_bytes, "DL %s PDU", rb_id_text[lcid]);
+
+  // Parse the message
+  liblte_mme_parse_msg_header((LIBLTE_BYTE_MSG_STRUCT *) pdu, &pd, &msg_type);
+  switch (msg_type) {
+    case LIBLTE_MME_MSG_TYPE_ATTACH_ACCEPT:
+      parse_attach_accept(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_ATTACH_REJECT:
+      parse_attach_reject(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REQUEST:
+      parse_authentication_request(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_AUTHENTICATION_REJECT:
+      parse_authentication_reject(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_IDENTITY_REQUEST:
+      parse_identity_request(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_SECURITY_MODE_COMMAND:
+      parse_security_mode_command(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_SERVICE_REJECT:
+      parse_service_reject(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_ESM_INFORMATION_REQUEST:
+      parse_esm_information_request(lcid, pdu);
+      break;
+    case LIBLTE_MME_MSG_TYPE_EMM_INFORMATION:
+      parse_emm_information(lcid, pdu);
+      break;
+    default:
+      nas_log->error("Not handling NAS message with MSG_TYPE=%02X\n", msg_type);
+      pool->deallocate(pdu);
+      break;
   }
-
-  bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
-    if (is_guti_set) {
-      s_tmsi->mmec = guti.mme_code;
-      s_tmsi->m_tmsi = guti.m_tmsi;
-      return true;
-    } else {
-      return false;
-    }
+}
+
+uint32_t nas::get_ul_count() {
+  return count_ul;
+}
+
+bool nas::get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) {
+  if (is_guti_set) {
+    s_tmsi->mmec = guti.mme_code;
+    s_tmsi->m_tmsi = guti.m_tmsi;
+    return true;
+  } else {
+    return false;
   }
+}
 
 /*******************************************************************************
-  Security
+Security
 *******************************************************************************/
 
-  void nas::integrity_generate(uint8_t *key_128,
-                               uint32_t count,
-                               uint8_t rb_id,
-                               uint8_t direction,
-                               uint8_t *msg,
-                               uint32_t msg_len,
-                               uint8_t *mac) {
-    switch (integ_algo) {
-      case INTEGRITY_ALGORITHM_ID_EIA0:
-        break;
-      case INTEGRITY_ALGORITHM_ID_128_EIA1:
-        security_128_eia1(key_128,
-                          count,
-                          rb_id,
-                          direction,
-                          msg,
-                          msg_len,
-                          mac);
-        break;
-      case INTEGRITY_ALGORITHM_ID_128_EIA2:
-        security_128_eia2(key_128,
-                          count,
-                          rb_id,
-                          direction,
-                          msg,
-                          msg_len,
-                          mac);
-        break;
-      default:
-        break;
-    }
+void nas::integrity_generate(uint8_t *key_128,
+                             uint32_t count,
+                             uint8_t rb_id,
+                             uint8_t direction,
+                             uint8_t *msg,
+                             uint32_t msg_len,
+                             uint8_t *mac) {
+  switch (integ_algo) {
+    case INTEGRITY_ALGORITHM_ID_EIA0:
+      break;
+    case INTEGRITY_ALGORITHM_ID_128_EIA1:
+      security_128_eia1(key_128,
+                        count,
+                        rb_id,
+                        direction,
+                        msg,
+                        msg_len,
+                        mac);
+      break;
+    case INTEGRITY_ALGORITHM_ID_128_EIA2:
+      security_128_eia2(key_128,
+                        count,
+                        rb_id,
+                        direction,
+                        msg,
+                        msg_len,
+                        mac);
+      break;
+    default:
+      break;
   }
+}
 
-  void nas::integrity_check() {
+void nas::integrity_check() {
 
-  }
+}
 
-  void nas::cipher_encrypt() {
+void nas::cipher_encrypt() {
 
-  }
+}
 
-  void nas::cipher_decrypt() {
+void nas::cipher_decrypt() {
 
-  }
+}
 
 
 /*******************************************************************************
-  Parsers
+Parsers
 *******************************************************************************/
 
-  void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
-    LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
-    LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT act_def_eps_bearer_context_req;
-    LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT attach_complete;
-    LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT act_def_eps_bearer_context_accept;
+void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
+  LIBLTE_MME_ATTACH_ACCEPT_MSG_STRUCT attach_accept;
+  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_REQUEST_MSG_STRUCT act_def_eps_bearer_context_req;
+  LIBLTE_MME_ATTACH_COMPLETE_MSG_STRUCT attach_complete;
+  LIBLTE_MME_ACTIVATE_DEFAULT_EPS_BEARER_CONTEXT_ACCEPT_MSG_STRUCT act_def_eps_bearer_context_accept;
 
-    nas_log->info("Received Attach Accept\n");
-    count_dl++;
+  nas_log->info("Received Attach Accept\n");
+  count_dl++;
 
-    liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_accept);
+  liblte_mme_unpack_attach_accept_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_accept);
 
-    if (attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY) {
-      //FIXME: Handle t3412.unit
-      //FIXME: Handle tai_list
-      if (attach_accept.guti_present) {
-        memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
-        is_guti_set = true;
-        // TODO: log message to console
-      }
-      if (attach_accept.lai_present) {
-      }
-      if (attach_accept.ms_id_present) {}
-      if (attach_accept.emm_cause_present) {}
-      if (attach_accept.t3402_present) {}
-      if (attach_accept.t3423_present) {}
-      if (attach_accept.equivalent_plmns_present) {}
-      if (attach_accept.emerg_num_list_present) {}
-      if (attach_accept.eps_network_feature_support_present) {}
-      if (attach_accept.additional_update_result_present) {}
-      if (attach_accept.t3412_ext_present) {}
-
-      liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg,
-                                                                        &act_def_eps_bearer_context_req);
-
-      if (LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type) {
-        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[0] << 24;
-        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[1] << 16;
-        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
-        ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[3];
-
-        nas_log->info("IP allocated by network %u.%u.%u.%u\n",
-                      act_def_eps_bearer_context_req.pdn_addr.addr[0],
-                      act_def_eps_bearer_context_req.pdn_addr.addr[1],
-                      act_def_eps_bearer_context_req.pdn_addr.addr[2],
-                      act_def_eps_bearer_context_req.pdn_addr.addr[3]);
-
-        nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
-                         act_def_eps_bearer_context_req.pdn_addr.addr[0],
-                         act_def_eps_bearer_context_req.pdn_addr.addr[1],
-                         act_def_eps_bearer_context_req.pdn_addr.addr[2],
-                         act_def_eps_bearer_context_req.pdn_addr.addr[3]);
-
-        // Setup GW
-        char *err_str = NULL;
-        if (gw->setup_if_addr(ip_addr, err_str)) {
-          nas_log->error("Failed to set gateway address - %s\n", err_str);
-        }
-      } else {
-        nas_log->error("Not handling IPV6 or IPV4V6\n");
-        pool->deallocate(pdu);
-        return;
-      }
-      eps_bearer_id = act_def_eps_bearer_context_req.eps_bearer_id;
-      if (act_def_eps_bearer_context_req.transaction_id_present) {
-        transaction_id = act_def_eps_bearer_context_req.proc_transaction_id;
+  if (attach_accept.eps_attach_result == LIBLTE_MME_EPS_ATTACH_RESULT_EPS_ONLY) {
+    //FIXME: Handle t3412.unit
+    //FIXME: Handle tai_list
+    if (attach_accept.guti_present) {
+      memcpy(&guti, &attach_accept.guti.guti, sizeof(LIBLTE_MME_EPS_MOBILE_ID_GUTI_STRUCT));
+      is_guti_set = true;
+      // TODO: log message to console
+    }
+    if (attach_accept.lai_present) {
+    }
+    if (attach_accept.ms_id_present) {}
+    if (attach_accept.emm_cause_present) {}
+    if (attach_accept.t3402_present) {}
+    if (attach_accept.t3423_present) {}
+    if (attach_accept.equivalent_plmns_present) {}
+    if (attach_accept.emerg_num_list_present) {}
+    if (attach_accept.eps_network_feature_support_present) {}
+    if (attach_accept.additional_update_result_present) {}
+    if (attach_accept.t3412_ext_present) {}
+
+    liblte_mme_unpack_activate_default_eps_bearer_context_request_msg(&attach_accept.esm_msg,
+                                                                      &act_def_eps_bearer_context_req);
+
+    if (LIBLTE_MME_PDN_TYPE_IPV4 == act_def_eps_bearer_context_req.pdn_addr.pdn_type) {
+      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[0] << 24;
+      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[1] << 16;
+      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[2] << 8;
+      ip_addr |= act_def_eps_bearer_context_req.pdn_addr.addr[3];
+
+      nas_log->info("IP allocated by network %u.%u.%u.%u\n",
+                    act_def_eps_bearer_context_req.pdn_addr.addr[0],
+                    act_def_eps_bearer_context_req.pdn_addr.addr[1],
+                    act_def_eps_bearer_context_req.pdn_addr.addr[2],
+                    act_def_eps_bearer_context_req.pdn_addr.addr[3]);
+
+      nas_log->console("Network attach successful.   IP: %u.%u.%u.%u\n",
+                       act_def_eps_bearer_context_req.pdn_addr.addr[0],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[1],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[2],
+                       act_def_eps_bearer_context_req.pdn_addr.addr[3]);
+
+      // Setup GW
+      char *err_str = NULL;
+      if (gw->setup_if_addr(ip_addr, err_str)) {
+        nas_log->error("Failed to set gateway address - %s\n", err_str);
       }
+    } else {
+      nas_log->error("Not handling IPV6 or IPV4V6\n");
+      pool->deallocate(pdu);
+      return;
+    }
+    eps_bearer_id = act_def_eps_bearer_context_req.eps_bearer_id;
+    if (act_def_eps_bearer_context_req.transaction_id_present) {
+      transaction_id = act_def_eps_bearer_context_req.proc_transaction_id;
+    }
 
-      //FIXME: Handle the following parameters
+    //FIXME: Handle the following parameters
 //    act_def_eps_bearer_context_req.eps_qos.qci
 //    act_def_eps_bearer_context_req.eps_qos.br_present
 //    act_def_eps_bearer_context_req.eps_qos.br_ext_present
@@ -319,315 +319,315 @@ namespace srsue {
 //    act_def_eps_bearer_context_req.protocol_cnfg_opts_present
 //    act_def_eps_bearer_context_req.connectivity_type_present
 
-      // FIXME: Setup the default EPS bearer context
-
-      state = EMM_STATE_REGISTERED;
-
-      // Send EPS bearer context accept and attach complete
-      count_ul++;
-      act_def_eps_bearer_context_accept.eps_bearer_id = eps_bearer_id;
-      act_def_eps_bearer_context_accept.proc_transaction_id = transaction_id;
-      act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
-      liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept,
-                                                                     &attach_complete.esm_msg);
-      liblte_mme_pack_attach_complete_msg(&attach_complete,
-                                          LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                          count_ul,
-                                          (LIBLTE_BYTE_MSG_STRUCT *) pdu);
-      integrity_generate(&k_nas_int[16],
-                         count_ul,
-                         lcid - 1,
-                         SECURITY_DIRECTION_UPLINK,
-                         &pdu->msg[5],
-                         pdu->N_bytes - 5,
-                         &pdu->msg[1]);
+    // FIXME: Setup the default EPS bearer context
 
-      // Instruct RRC to enable capabilities
-      rrc->enable_capabilities();
+    state = EMM_STATE_REGISTERED;
 
-      nas_log->info("Sending Attach Complete\n");
-      rrc->write_sdu(lcid, pdu);
+    // Send EPS bearer context accept and attach complete
+    count_ul++;
+    act_def_eps_bearer_context_accept.eps_bearer_id = eps_bearer_id;
+    act_def_eps_bearer_context_accept.proc_transaction_id = transaction_id;
+    act_def_eps_bearer_context_accept.protocol_cnfg_opts_present = false;
+    liblte_mme_pack_activate_default_eps_bearer_context_accept_msg(&act_def_eps_bearer_context_accept,
+                                                                   &attach_complete.esm_msg);
+    liblte_mme_pack_attach_complete_msg(&attach_complete,
+                                        LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
+                                        count_ul,
+                                        (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+    integrity_generate(&k_nas_int[16],
+                       count_ul,
+                       lcid - 1,
+                       SECURITY_DIRECTION_UPLINK,
+                       &pdu->msg[5],
+                       pdu->N_bytes - 5,
+                       &pdu->msg[1]);
 
-    } else {
-      nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
-      state = EMM_STATE_DEREGISTERED;
-      pool->deallocate(pdu);
-    }
-  }
+    // Instruct RRC to enable capabilities
+    rrc->enable_capabilities();
 
-  void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu) {
-    LIBLTE_MME_ATTACH_REJECT_MSG_STRUCT attach_rej;
+    nas_log->info("Sending Attach Complete\n");
+    rrc->write_sdu(lcid, pdu);
 
-    liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_rej);
-    nas_log->warning("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
-    nas_log->console("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
+  } else {
+    nas_log->info("Not handling attach type %u\n", attach_accept.eps_attach_result);
     state = EMM_STATE_DEREGISTERED;
     pool->deallocate(pdu);
-    // FIXME: Command RRC to release?
   }
+}
 
-  void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
-    LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT auth_req;
-    LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
+void nas::parse_attach_reject(uint32_t lcid, byte_buffer_t *pdu) {
+  LIBLTE_MME_ATTACH_REJECT_MSG_STRUCT attach_rej;
 
-    nas_log->info("Received Authentication Request\n");;
-    liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &auth_req);
+  liblte_mme_unpack_attach_reject_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &attach_rej);
+  nas_log->warning("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
+  nas_log->console("Received Attach Reject. Cause= %02X\n", attach_rej.emm_cause);
+  state = EMM_STATE_DEREGISTERED;
+  pool->deallocate(pdu);
+  // FIXME: Command RRC to release?
+}
 
-    // Reuse the pdu for the response message
-    pdu->reset();
+void nas::parse_authentication_request(uint32_t lcid, byte_buffer_t *pdu) {
+  LIBLTE_MME_AUTHENTICATION_REQUEST_MSG_STRUCT auth_req;
+  LIBLTE_MME_AUTHENTICATION_RESPONSE_MSG_STRUCT auth_res;
 
-    // Generate authentication response using RAND, AUTN & KSI-ASME
-    uint16 mcc, mnc;
-    mcc = rrc->get_mcc();
-    mnc = rrc->get_mnc();
+  nas_log->info("Received Authentication Request\n");;
+  liblte_mme_unpack_authentication_request_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &auth_req);
 
-    nas_log->info("MCC=%d, MNC=%d\n", mcc, mnc);
+  // Reuse the pdu for the response message
+  pdu->reset();
 
-    bool net_valid;
-    uint8_t res[16];
-    usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
+  // Generate authentication response using RAND, AUTN & KSI-ASME
+  uint16 mcc, mnc;
+  mcc = rrc->get_mcc();
+  mnc = rrc->get_mnc();
 
-    if (net_valid) {
-      nas_log->info("Network authentication successful\n");
-      for (int i = 0; i < 8; i++) {
-        auth_res.res[i] = res[i];
-      }
-      liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+  nas_log->info("MCC=%d, MNC=%d\n", mcc, mnc);
 
-      nas_log->info("Sending Authentication Response\n");
-      rrc->write_sdu(lcid, pdu);
-    } else {
-      nas_log->warning("Network authentication failure\n");
-      nas_log->console("Warning: Network authentication failure\n");
-      pool->deallocate(pdu);
+  bool net_valid;
+  uint8_t res[16];
+  usim->generate_authentication_response(auth_req.rand, auth_req.autn, mcc, mnc, &net_valid, res);
+
+  if (net_valid) {
+    nas_log->info("Network authentication successful\n");
+    for (int i = 0; i < 8; i++) {
+      auth_res.res[i] = res[i];
     }
-  }
+    liblte_mme_pack_authentication_response_msg(&auth_res, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
 
-  void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
-    nas_log->warning("Received Authentication Reject\n");
+    nas_log->info("Sending Authentication Response\n");
+    rrc->write_sdu(lcid, pdu);
+  } else {
+    nas_log->warning("Network authentication failure\n");
+    nas_log->console("Warning: Network authentication failure\n");
     pool->deallocate(pdu);
-    state = EMM_STATE_DEREGISTERED;
-    // FIXME: Command RRC to release?
-  }
-
-  void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu) {
-    nas_log->error("TODO:parse_identity_request\n");
   }
-
-  void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
-    bool success;
-    LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT sec_mode_cmd;
-    LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT sec_mode_comp;
-    LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
-
-    nas_log->info("Received Security Mode Command\n");
-    liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &sec_mode_cmd);
-
-    ksi = sec_mode_cmd.nas_ksi.nas_ksi;
-    cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
-    integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
-    // FIXME: Handle nonce_ue, nonce_mme
-    // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
-    // FIXME: Use selected_nas_sec_algs to choose correct algos
-
-    nas_log->debug("Security details: ksi=%d, eea=%s, eia=%s\n",
-                   ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
-
-
-    if (CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
-        (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
-         INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
-        sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
-      sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
-      nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
+}
+
+void nas::parse_authentication_reject(uint32_t lcid, byte_buffer_t *pdu) {
+  nas_log->warning("Received Authentication Reject\n");
+  pool->deallocate(pdu);
+  state = EMM_STATE_DEREGISTERED;
+  // FIXME: Command RRC to release?
+}
+
+void nas::parse_identity_request(uint32_t lcid, byte_buffer_t *pdu) {
+  nas_log->error("TODO:parse_identity_request\n");
+}
+
+void nas::parse_security_mode_command(uint32_t lcid, byte_buffer_t *pdu) {
+  bool success;
+  LIBLTE_MME_SECURITY_MODE_COMMAND_MSG_STRUCT sec_mode_cmd;
+  LIBLTE_MME_SECURITY_MODE_COMPLETE_MSG_STRUCT sec_mode_comp;
+  LIBLTE_MME_SECURITY_MODE_REJECT_MSG_STRUCT sec_mode_rej;
+
+  nas_log->info("Received Security Mode Command\n");
+  liblte_mme_unpack_security_mode_command_msg((LIBLTE_BYTE_MSG_STRUCT *) pdu, &sec_mode_cmd);
+
+  ksi = sec_mode_cmd.nas_ksi.nas_ksi;
+  cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eea;
+  integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) sec_mode_cmd.selected_nas_sec_algs.type_of_eia;
+  // FIXME: Handle nonce_ue, nonce_mme
+  // FIXME: Currently only handling ciphering EEA0 (null) and integrity EIA1,EIA2
+  // FIXME: Use selected_nas_sec_algs to choose correct algos
+
+  nas_log->debug("Security details: ksi=%d, eea=%s, eia=%s\n",
+                 ksi, ciphering_algorithm_id_text[cipher_algo], integrity_algorithm_id_text[integ_algo]);
+
+
+  if (CIPHERING_ALGORITHM_ID_EEA0 != cipher_algo ||
+      (INTEGRITY_ALGORITHM_ID_128_EIA2 != integ_algo &&
+       INTEGRITY_ALGORITHM_ID_128_EIA1 != integ_algo) ||
+      sec_mode_cmd.nas_ksi.tsc_flag != LIBLTE_MME_TYPE_OF_SECURITY_CONTEXT_FLAG_NATIVE) {
+    sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_UE_SECURITY_CAPABILITIES_MISMATCH;
+    nas_log->warning("Sending Security Mode Reject due to security capabilities mismatch\n");
+    success = false;
+  } else {
+    // Generate NAS encryption key and integrity protection key
+    usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
+    nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
+    nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
+
+    // Check incoming MAC
+    uint8_t *inMAC = &pdu->msg[1];
+    uint8_t genMAC[4];
+    integrity_generate(&k_nas_int[16],
+                       count_dl,
+                       lcid - 1,
+                       SECURITY_DIRECTION_DOWNLINK,
+                       &pdu->msg[5],
+                       pdu->N_bytes - 5,
+                       genMAC);
+
+    nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
+    nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
+
+    bool match = true;
+    for (int i = 0; i < 4; i++) {
+      if (inMAC[i] != genMAC[i]) {
+        match = false;
+      }
+    }
+    if (!match) {
+      sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
+      nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
       success = false;
     } else {
-      // Generate NAS encryption key and integrity protection key
-      usim->generate_nas_keys(k_nas_enc, k_nas_int, cipher_algo, integ_algo);
-      nas_log->debug_hex(k_nas_enc, 32, "NAS encryption key - k_nas_enc");
-      nas_log->debug_hex(k_nas_int, 32, "NAS integrity key - k_nas_int");
-
-      // Check incoming MAC
-      uint8_t *inMAC = &pdu->msg[1];
-      uint8_t genMAC[4];
-      integrity_generate(&k_nas_int[16],
-                         count_dl,
-                         lcid - 1,
-                         SECURITY_DIRECTION_DOWNLINK,
-                         &pdu->msg[5],
-                         pdu->N_bytes - 5,
-                         genMAC);
-
-      nas_log->info_hex(inMAC, 4, "Incoming PDU MAC:");
-      nas_log->info_hex(genMAC, 4, "Generated PDU MAC:");
 
-      bool match = true;
-      for (int i = 0; i < 4; i++) {
-        if (inMAC[i] != genMAC[i]) {
-          match = false;
-        }
-      }
-      if (!match) {
-        sec_mode_rej.emm_cause = LIBLTE_MME_EMM_CAUSE_SECURITY_MODE_REJECTED_UNSPECIFIED;
-        nas_log->warning("Sending Security Mode Reject due to integrity check failure\n");
-        success = false;
+      if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
+        sec_mode_comp.imeisv_present = true;
+        sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
+        usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
+        sec_mode_comp.imeisv.imeisv[14] = 5;
+        sec_mode_comp.imeisv.imeisv[15] = 3;
       } else {
-
-        if (sec_mode_cmd.imeisv_req_present && LIBLTE_MME_IMEISV_REQUESTED == sec_mode_cmd.imeisv_req) {
-          sec_mode_comp.imeisv_present = true;
-          sec_mode_comp.imeisv.type_of_id = LIBLTE_MME_MOBILE_ID_TYPE_IMEISV;
-          usim->get_imei_vec(sec_mode_comp.imeisv.imeisv, 15);
-          sec_mode_comp.imeisv.imeisv[14] = 5;
-          sec_mode_comp.imeisv.imeisv[15] = 3;
-        } else {
-          sec_mode_comp.imeisv_present = false;
-        }
-
-        // Reuse pdu for response
-        pdu->reset();
-        liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
-                                                   LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
-                                                   count_ul,
-                                                   (LIBLTE_BYTE_MSG_STRUCT *) pdu);
-        integrity_generate(&k_nas_int[16],
-                           count_ul,
-                           lcid - 1,
-                           SECURITY_DIRECTION_UPLINK,
-                           &pdu->msg[5],
-                           pdu->N_bytes - 5,
-                           &pdu->msg[1]);
-        nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
-                      count_ul,
-                      rb_id_text[lcid]);
-        success = true;
+        sec_mode_comp.imeisv_present = false;
       }
-    }
 
-    if (!success) {
       // Reuse pdu for response
       pdu->reset();
-      liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+      liblte_mme_pack_security_mode_complete_msg(&sec_mode_comp,
+                                                 LIBLTE_MME_SECURITY_HDR_TYPE_INTEGRITY_AND_CIPHERED,
+                                                 count_ul,
+                                                 (LIBLTE_BYTE_MSG_STRUCT *) pdu);
+      integrity_generate(&k_nas_int[16],
+                         count_ul,
+                         lcid - 1,
+                         SECURITY_DIRECTION_UPLINK,
+                         &pdu->msg[5],
+                         pdu->N_bytes - 5,
+                         &pdu->msg[1]);
+      nas_log->info("Sending Security Mode Complete nas_count_ul=%d, RB=%s\n",
+                    count_ul,
+                    rb_id_text[lcid]);
+      success = true;
     }
-
-    rrc->write_sdu(lcid, pdu);
-  }
-
-  void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
-    nas_log->error("TODO:parse_service_reject\n");
   }
 
-  void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
-    nas_log->error("TODO:parse_esm_information_request\n");
-  }
-
-  void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
-    nas_log->error("TODO:parse_emm_information\n");
-  }
-
-/*******************************************************************************
-  Senders
-*******************************************************************************/
-
-  void nas::send_attach_request() {
-    LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
-    byte_buffer_t *msg = pool_allocate;
-    u_int32_t i;
-
-    attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
-
-    for (i = 0; i < 8; i++) {
-      attach_req.ue_network_cap.eea[i] = false;
-      attach_req.ue_network_cap.eia[i] = false;
-    }
-    attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
-    attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
-    attach_req.ue_network_cap.eia[1] = true; // EIA1 supported
-    attach_req.ue_network_cap.eia[2] = true; // EIA2 supported
-
-    attach_req.ue_network_cap.uea_present = false; // UMTS encryption algos
-    attach_req.ue_network_cap.uia_present = false; // UMTS integrity algos
-
-    attach_req.ms_network_cap_present = false; // A/Gb mode (2G) or Iu mode (3G)
-
-    attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
-    usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
-
-    // ESM message (PDN connectivity request) for first default bearer
-    gen_pdn_connectivity_request(&attach_req.esm_msg);
-
-    attach_req.old_p_tmsi_signature_present = false;
-    attach_req.additional_guti_present = false;
-    attach_req.last_visited_registered_tai_present = false;
-    attach_req.drx_param_present = false;
-    attach_req.ms_network_cap_present = false;
-    attach_req.old_lai_present = false;
-    attach_req.tmsi_status_present = false;
-    attach_req.ms_cm2_present = false;
-    attach_req.ms_cm3_present = false;
-    attach_req.supported_codecs_present = false;
-    attach_req.additional_update_type_present = false;
-    attach_req.voice_domain_pref_and_ue_usage_setting_present = false;
-    attach_req.device_properties_present = false;
-    attach_req.old_guti_type_present = false;
-
-    // Pack the message
-    liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
-
-    nas_log->info("Sending attach request\n");
-    rrc->write_sdu(RB_ID_SRB1, msg);
+  if (!success) {
+    // Reuse pdu for response
+    pdu->reset();
+    liblte_mme_pack_security_mode_reject_msg(&sec_mode_rej, (LIBLTE_BYTE_MSG_STRUCT *) pdu);
   }
 
-  void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
-    LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT pdn_con_req;
+  rrc->write_sdu(lcid, pdu);
+}
 
-    nas_log->info("Generating PDN Connectivity Request\n");
+void nas::parse_service_reject(uint32_t lcid, byte_buffer_t *pdu) {
+  nas_log->error("TODO:parse_service_reject\n");
+}
 
-    // Set the PDN con req parameters
-    pdn_con_req.eps_bearer_id = 0x00; // Unassigned bearer ID
-    pdn_con_req.proc_transaction_id = 0x01; // First transaction ID
-    pdn_con_req.pdn_type = LIBLTE_MME_PDN_TYPE_IPV4;
-    pdn_con_req.request_type = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
+void nas::parse_esm_information_request(uint32_t lcid, byte_buffer_t *pdu) {
+  nas_log->error("TODO:parse_esm_information_request\n");
+}
 
-    // Set the optional flags
-    pdn_con_req.esm_info_transfer_flag_present = false; //FIXME: Check if this is needed
-    pdn_con_req.apn_present = false;
-    pdn_con_req.protocol_cnfg_opts_present = false;
-    pdn_con_req.device_properties_present = false;
+void nas::parse_emm_information(uint32_t lcid, byte_buffer_t *pdu) {
+  nas_log->error("TODO:parse_emm_information\n");
+}
 
-    // Pack the message
-    liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
-  }
-
-  void nas::send_identity_response() {}
+/*******************************************************************************
+Senders
+*******************************************************************************/
 
-  void nas::send_service_request() {
-    byte_buffer_t *msg = pool_allocate;
-    count_ul++;
+void nas::send_attach_request() {
+  LIBLTE_MME_ATTACH_REQUEST_MSG_STRUCT attach_req;
+  byte_buffer_t *msg = pool_allocate;
+  u_int32_t i;
 
-    // Pack the service request message directly
-    msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
-    msg->N_bytes++;
-    msg->msg[1] = (ksi & 0x07) << 5;
-    msg->msg[1] |= count_ul & 0x1F;
-    msg->N_bytes++;
+  attach_req.eps_attach_type = LIBLTE_MME_EPS_ATTACH_TYPE_EPS_ATTACH;
 
-    uint8_t mac[4];
-    integrity_generate(&k_nas_int[16],
-                       count_ul,
-                       RB_ID_SRB1 - 1,
-                       SECURITY_DIRECTION_UPLINK,
-                       &msg->msg[0],
-                       2,
-                       &mac[0]);
-    // Set the short MAC
-    msg->msg[2] = mac[2];
-    msg->N_bytes++;
-    msg->msg[3] = mac[3];
-    msg->N_bytes++;
-    nas_log->info("Sending service request\n");
-    rrc->write_sdu(RB_ID_SRB1, msg);
+  for (i = 0; i < 8; i++) {
+    attach_req.ue_network_cap.eea[i] = false;
+    attach_req.ue_network_cap.eia[i] = false;
   }
-
-  void nas::send_esm_information_response() {}
+  attach_req.ue_network_cap.eea[0] = true; // EEA0 supported
+  attach_req.ue_network_cap.eia[0] = true; // EIA0 supported
+  attach_req.ue_network_cap.eia[1] = true; // EIA1 supported
+  attach_req.ue_network_cap.eia[2] = true; // EIA2 supported
+
+  attach_req.ue_network_cap.uea_present = false; // UMTS encryption algos
+  attach_req.ue_network_cap.uia_present = false; // UMTS integrity algos
+
+  attach_req.ms_network_cap_present = false; // A/Gb mode (2G) or Iu mode (3G)
+
+  attach_req.eps_mobile_id.type_of_id = LIBLTE_MME_EPS_MOBILE_ID_TYPE_IMSI;
+  usim->get_imsi_vec(attach_req.eps_mobile_id.imsi, 15);
+
+  // ESM message (PDN connectivity request) for first default bearer
+  gen_pdn_connectivity_request(&attach_req.esm_msg);
+
+  attach_req.old_p_tmsi_signature_present = false;
+  attach_req.additional_guti_present = false;
+  attach_req.last_visited_registered_tai_present = false;
+  attach_req.drx_param_present = false;
+  attach_req.ms_network_cap_present = false;
+  attach_req.old_lai_present = false;
+  attach_req.tmsi_status_present = false;
+  attach_req.ms_cm2_present = false;
+  attach_req.ms_cm3_present = false;
+  attach_req.supported_codecs_present = false;
+  attach_req.additional_update_type_present = false;
+  attach_req.voice_domain_pref_and_ue_usage_setting_present = false;
+  attach_req.device_properties_present = false;
+  attach_req.old_guti_type_present = false;
+
+  // Pack the message
+  liblte_mme_pack_attach_request_msg(&attach_req, (LIBLTE_BYTE_MSG_STRUCT *) msg);
+
+  nas_log->info("Sending attach request\n");
+  rrc->write_sdu(RB_ID_SRB1, msg);
+}
+
+void nas::gen_pdn_connectivity_request(LIBLTE_BYTE_MSG_STRUCT *msg) {
+  LIBLTE_MME_PDN_CONNECTIVITY_REQUEST_MSG_STRUCT pdn_con_req;
+
+  nas_log->info("Generating PDN Connectivity Request\n");
+
+  // Set the PDN con req parameters
+  pdn_con_req.eps_bearer_id = 0x00; // Unassigned bearer ID
+  pdn_con_req.proc_transaction_id = 0x01; // First transaction ID
+  pdn_con_req.pdn_type = LIBLTE_MME_PDN_TYPE_IPV4;
+  pdn_con_req.request_type = LIBLTE_MME_REQUEST_TYPE_INITIAL_REQUEST;
+
+  // Set the optional flags
+  pdn_con_req.esm_info_transfer_flag_present = false; //FIXME: Check if this is needed
+  pdn_con_req.apn_present = false;
+  pdn_con_req.protocol_cnfg_opts_present = false;
+  pdn_con_req.device_properties_present = false;
+
+  // Pack the message
+  liblte_mme_pack_pdn_connectivity_request_msg(&pdn_con_req, msg);
+}
+
+void nas::send_identity_response() {}
+
+void nas::send_service_request() {
+  byte_buffer_t *msg = pool_allocate;
+  count_ul++;
+
+  // Pack the service request message directly
+  msg->msg[0] = (LIBLTE_MME_SECURITY_HDR_TYPE_SERVICE_REQUEST << 4) | (LIBLTE_MME_PD_EPS_MOBILITY_MANAGEMENT);
+  msg->N_bytes++;
+  msg->msg[1] = (ksi & 0x07) << 5;
+  msg->msg[1] |= count_ul & 0x1F;
+  msg->N_bytes++;
+
+  uint8_t mac[4];
+  integrity_generate(&k_nas_int[16],
+                     count_ul,
+                     RB_ID_SRB1 - 1,
+                     SECURITY_DIRECTION_UPLINK,
+                     &msg->msg[0],
+                     2,
+                     &mac[0]);
+  // Set the short MAC
+  msg->msg[2] = mac[2];
+  msg->N_bytes++;
+  msg->msg[3] = mac[3];
+  msg->N_bytes++;
+  nas_log->info("Sending service request\n");
+  rrc->write_sdu(RB_ID_SRB1, msg);
+}
+
+void nas::send_esm_information_response() {}
 
 } // namespace srsue
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 3e8bfcab8..81577cce4 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -46,388 +46,388 @@ namespace srsue {
   Base functions 
 *******************************************************************************/
 
-  rrc::rrc()
-    : state(RRC_STATE_IDLE), drb_up(false) {}
-
-  static void liblte_rrc_handler(void *ctx, char *str) {
-    rrc *r = (rrc *) ctx;
-    r->liblte_rrc_log(str);
-  }
-
-  void rrc::liblte_rrc_log(char *str) {
-    if (rrc_log) {
-      rrc_log->warning("[ASN]: %s\n", str);
-    } else {
-      printf("[ASN]: %s\n", str);
-    }
-  }
-
-  void rrc::init(phy_interface_rrc *phy_,
-                 mac_interface_rrc *mac_,
-                 rlc_interface_rrc *rlc_,
-                 pdcp_interface_rrc *pdcp_,
-                 nas_interface_rrc *nas_,
-                 usim_interface_rrc *usim_,
-                 mac_interface_timers *mac_timers_,
-                 srslte::log *rrc_log_) {
-    pool = byte_buffer_pool::get_instance();
-    phy = phy_;
-    mac = mac_;
-    rlc = rlc_;
-    pdcp = pdcp_;
-    nas = nas_;
-    usim = usim_;
-    rrc_log = rrc_log_;
-    mac_timers = mac_timers_;
-    state = RRC_STATE_IDLE;
-    si_acquire_state = SI_ACQUIRE_IDLE;
-
-    thread_running = true;
-    start();
-
-    pthread_mutex_init(&mutex, NULL);
-
-    ue_category = SRSLTE_UE_CATEGORY;
-
-    transaction_id = 0;
-
-    // Register logging handler with liblte_rrc
-    liblte_rrc_log_register_handler(this, liblte_rrc_handler);
-
-    // Set default values for all layers
-    set_rrc_default();
-    set_phy_default();
-    set_mac_default();
-  }
-
-  void rrc::stop() {
-    thread_running = false;
-    wait_thread_finish();
+rrc::rrc()
+  : state(RRC_STATE_IDLE), drb_up(false) {}
+
+static void liblte_rrc_handler(void *ctx, char *str) {
+  rrc *r = (rrc *) ctx;
+  r->liblte_rrc_log(str);
+}
+
+void rrc::liblte_rrc_log(char *str) {
+  if (rrc_log) {
+    rrc_log->warning("[ASN]: %s\n", str);
+  } else {
+    printf("[ASN]: %s\n", str);
   }
-
-  rrc_state_t rrc::get_state() {
-    return state;
-  }
-
-  void rrc::set_ue_category(int category) {
-    if (category >= 1 && category <= 5) {
-      ue_category = category;
-    } else {
-      rrc_log->error("Unsupported UE category %d\n", category);
-    }
+}
+
+void rrc::init(phy_interface_rrc *phy_,
+               mac_interface_rrc *mac_,
+               rlc_interface_rrc *rlc_,
+               pdcp_interface_rrc *pdcp_,
+               nas_interface_rrc *nas_,
+               usim_interface_rrc *usim_,
+               mac_interface_timers *mac_timers_,
+               srslte::log *rrc_log_) {
+  pool = byte_buffer_pool::get_instance();
+  phy = phy_;
+  mac = mac_;
+  rlc = rlc_;
+  pdcp = pdcp_;
+  nas = nas_;
+  usim = usim_;
+  rrc_log = rrc_log_;
+  mac_timers = mac_timers_;
+  state = RRC_STATE_IDLE;
+  si_acquire_state = SI_ACQUIRE_IDLE;
+
+  thread_running = true;
+  start();
+
+  pthread_mutex_init(&mutex, NULL);
+
+  ue_category = SRSLTE_UE_CATEGORY;
+
+  transaction_id = 0;
+
+  // Register logging handler with liblte_rrc
+  liblte_rrc_log_register_handler(this, liblte_rrc_handler);
+
+  // Set default values for all layers
+  set_rrc_default();
+  set_phy_default();
+  set_mac_default();
+}
+
+void rrc::stop() {
+  thread_running = false;
+  wait_thread_finish();
+}
+
+rrc_state_t rrc::get_state() {
+  return state;
+}
+
+void rrc::set_ue_category(int category) {
+  if (category >= 1 && category <= 5) {
+    ue_category = category;
+  } else {
+    rrc_log->error("Unsupported UE category %d\n", category);
   }
+}
 
 
 
 /*******************************************************************************
- * 
- * 
- * 
- * PLMN selection, cell selection/reselection and acquisition of SI procedures
- * 
- * 
- * 
+*
+*
+*
+* PLMN selection, cell selection/reselection and acquisition of SI procedures
+*
+*
+*
 *******************************************************************************/
 
 
 /*******************************************************************************
-  NAS interface
+NAS interface
 *******************************************************************************/
 
-  uint16_t rrc::get_mcc() {
-    if (current_cell) {
-      if (current_cell->sib1.N_plmn_ids > 0) {
-        return current_cell->sib1.plmn_id[0].id.mcc;
-      }
+uint16_t rrc::get_mcc() {
+  if (current_cell) {
+    if (current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mcc;
     }
-    return 0;
   }
+  return 0;
+}
 
-  uint16_t rrc::get_mnc() {
-    if (current_cell) {
-      if (current_cell->sib1.N_plmn_ids > 0) {
-        return current_cell->sib1.plmn_id[0].id.mnc;
-      }
+uint16_t rrc::get_mnc() {
+  if (current_cell) {
+    if (current_cell->sib1.N_plmn_ids > 0) {
+      return current_cell->sib1.plmn_id[0].id.mnc;
     }
-    return 0;
   }
+  return 0;
+}
 
-  void rrc::plmn_search() {
-    rrc_log->info("Starting PLMN search procedure\n");
-    state = RRC_STATE_PLMN_SELECTION;
-    phy->cell_search_start();
-  }
+void rrc::plmn_search() {
+  rrc_log->info("Starting PLMN search procedure\n");
+  state = RRC_STATE_PLMN_SELECTION;
+  phy->cell_search_start();
+}
 
-  void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
-    rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
+void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
+  rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
 
-    state = RRC_STATE_CELL_SELECTING;
+  state = RRC_STATE_CELL_SELECTING;
 
-    // Sort cells according to RSRP
+  // Sort cells according to RSRP
 
-    selected_plmn_id = plmn_id;
-    last_selected_cell = -1;
+  selected_plmn_id = plmn_id;
+  last_selected_cell = -1;
 
-    select_next_cell_in_plmn();
-  }
+  select_next_cell_in_plmn();
+}
 
-  void rrc::connect() {
-    pthread_mutex_lock(&mutex);
-    if (RRC_STATE_CELL_SELECTED == state) {
-      rrc_log->info("RRC in IDLE state - sending connection request.\n");
-      state = RRC_STATE_CONNECTING;
-      send_con_request();
-    } else {
-      rrc_log->warning("Received connect() but cell is not selected\n");
-    }
-    pthread_mutex_unlock(&mutex);
+void rrc::connect() {
+  pthread_mutex_lock(&mutex);
+  if (RRC_STATE_CELL_SELECTED == state) {
+    rrc_log->info("RRC in IDLE state - sending connection request.\n");
+    state = RRC_STATE_CONNECTING;
+    send_con_request();
+  } else {
+    rrc_log->warning("Received connect() but cell is not selected\n");
   }
-
-  void rrc::select_next_cell_in_plmn() {
-    for (uint32_t i = last_selected_cell + 1; i < known_cells.size(); i++) {
-      for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
-        if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc ||
-            known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) {
-          rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
-                        known_cells[i].phy_cell.id, known_cells[i].earfcn,
-                        known_cells[i].sib1.cell_id);
-          rrc_log->console("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
-                        known_cells[i].phy_cell.id, known_cells[i].earfcn,
-                        known_cells[i].sib1.cell_id);
-          // Check that cell satisfies S criteria
-          if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
-            // Give time to the PHY to sync on the new cell
-            int cnt=0;
-            while(!phy->sync_status() && cnt<100) {
-              usleep(1000);
-              cnt++;
-            }
-            if (phy->sync_status()) {
-              if (!known_cells[i].has_valid_sib1) {
-                si_acquire_state = SI_ACQUIRE_SIB1;
-              } else if (!known_cells[i].has_valid_sib2) {
-                si_acquire_state = SI_ACQUIRE_SIB2;
-              } else {
-                si_acquire_state = SI_ACQUIRE_IDLE;
-              }
-              last_selected_cell = i;
+  pthread_mutex_unlock(&mutex);
+}
+
+void rrc::select_next_cell_in_plmn() {
+  for (uint32_t i = last_selected_cell + 1; i < known_cells.size(); i++) {
+    for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
+      if (known_cells[i].sib1.plmn_id[j].id.mcc == selected_plmn_id.mcc ||
+          known_cells[i].sib1.plmn_id[j].id.mnc == selected_plmn_id.mnc) {
+        rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                      known_cells[i].sib1.cell_id);
+        rrc_log->console("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+                      known_cells[i].phy_cell.id, known_cells[i].earfcn,
+                      known_cells[i].sib1.cell_id);
+        // Check that cell satisfies S criteria
+        if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
+          // Give time to the PHY to sync on the new cell
+          int cnt=0;
+          while(!phy->sync_status() && cnt<100) {
+            usleep(1000);
+            cnt++;
+          }
+          if (phy->sync_status()) {
+            if (!known_cells[i].has_valid_sib1) {
+              si_acquire_state = SI_ACQUIRE_SIB1;
+            } else if (!known_cells[i].has_valid_sib2) {
+              si_acquire_state = SI_ACQUIRE_SIB2;
             } else {
-              rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x: Could not synchronize\n",
-                             known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+              si_acquire_state = SI_ACQUIRE_IDLE;
             }
-            return;
+            last_selected_cell = i;
           } else {
-            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x: Could not synchronize\n",
                            known_cells[i].earfcn, known_cells[i].sib1.cell_id);
           }
+          return;
+        } else {
+          rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+                         known_cells[i].earfcn, known_cells[i].sib1.cell_id);
         }
       }
     }
   }
+}
 
 /*******************************************************************************
-  PHY interface
+PHY interface
 *******************************************************************************/
 
-  void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
+void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
-    // find if cell_id-earfcn combination already exists
-    for (uint32_t i = 0; i < known_cells.size(); i++) {
-      if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
-        known_cells[i].rsrp = rsrp;
-        current_cell = &known_cells[i];
-        rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
-                      known_cells[i].phy_cell.id, known_cells[i].rsrp);
-        return;
-      }
+  // find if cell_id-earfcn combination already exists
+  for (uint32_t i = 0; i < known_cells.size(); i++) {
+    if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
+      known_cells[i].rsrp = rsrp;
+      current_cell = &known_cells[i];
+      rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
+                    known_cells[i].phy_cell.id, known_cells[i].rsrp);
+      return;
     }
-    // add to list of known cells
-    cell_t cell;
-    cell.phy_cell = phy_cell;
-    cell.rsrp = rsrp;
-    cell.earfcn = earfcn;
-    cell.has_valid_sib1 = false;
-    cell.has_valid_sib2 = false;
-    known_cells.push_back(cell);
+  }
+  // add to list of known cells
+  cell_t cell;
+  cell.phy_cell = phy_cell;
+  cell.rsrp = rsrp;
+  cell.earfcn = earfcn;
+  cell.has_valid_sib1 = false;
+  cell.has_valid_sib2 = false;
+  known_cells.push_back(cell);
 
-    // save current cell
-    current_cell = &known_cells.back();
+  // save current cell
+  current_cell = &known_cells.back();
 
-    si_acquire_state = SI_ACQUIRE_SIB1;
+  si_acquire_state = SI_ACQUIRE_SIB1;
 
-    rrc_log->info("Found Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
-                  cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
-                  cell.earfcn, cell.rsrp);
+  rrc_log->info("Found Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+                cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
+                cell.earfcn, cell.rsrp);
 
-    rrc_log->console("Found Cell:  PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
-                  cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
-                  cell.earfcn, cell.rsrp);
+  rrc_log->console("Found Cell:  PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+                cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
+                cell.earfcn, cell.rsrp);
 
-  }
+}
 
 // Detection of physical layer problems (5.3.11.1)
-  void rrc::out_of_sync() {
-    if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
-      n310_cnt++;
-      if (n310_cnt == N310) {
-        mac_timers->get(t310)->reset();
-        mac_timers->get(t310)->run();
-        n310_cnt = 0;
-        rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
-      }
+void rrc::out_of_sync() {
+  if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
+    n310_cnt++;
+    if (n310_cnt == N310) {
+      mac_timers->get(t310)->reset();
+      mac_timers->get(t310)->run();
+      n310_cnt = 0;
+      rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
     }
   }
+}
 
 // Recovery of physical layer problems (5.3.11.2)
-  void rrc::in_sync() {
-    if (mac_timers->get(t310)->is_running()) {
-      n311_cnt++;
-      if (n311_cnt == N311) {
-        mac_timers->get(t310)->stop();
-        n311_cnt = 0;
-        rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
-      }
+void rrc::in_sync() {
+  if (mac_timers->get(t310)->is_running()) {
+    n311_cnt++;
+    if (n311_cnt == N311) {
+      mac_timers->get(t310)->stop();
+      n311_cnt = 0;
+      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
     }
   }
+}
 
 /*******************************************************************************
-  PDCP interface
+PDCP interface
 *******************************************************************************/
-  void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
-    pool->deallocate(pdu);
-    if (state == RRC_STATE_PLMN_SELECTION) {
-      // Do we need to do something with BCH?
-      rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
-    } else {
-      rrc_log->warning("Received BCCH BCH in incorrect state\n");
-    }
+void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
+  pool->deallocate(pdu);
+  if (state == RRC_STATE_PLMN_SELECTION) {
+    // Do we need to do something with BCH?
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
+  } else {
+    rrc_log->warning("Received BCCH BCH in incorrect state\n");
   }
+}
 
-  void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-    rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-    LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-    bit_buf.N_bits = pdu->N_bytes * 8;
-    pool->deallocate(pdu);
-    liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
-
-    if (dlsch_msg.N_sibs > 0) {
-      if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
-        mac->bcch_stop_rx();
+void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
+  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
+  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  pool->deallocate(pdu);
+  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
 
-        // Handle SIB1
-        memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+  if (dlsch_msg.N_sibs > 0) {
+    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
+      mac->bcch_stop_rx();
 
-        rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                      current_cell->sib1.cell_id & 0xfff,
-                      liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
-                      liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+      // Handle SIB1
+      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
 
+      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
+                    current_cell->sib1.cell_id & 0xfff,
+                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
 
-        // Set TDD Config
-        if (current_cell->sib1.tdd) {
-          phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
-        }
 
-        current_cell->has_valid_sib1 = true;
+      // Set TDD Config
+      if (current_cell->sib1.tdd) {
+        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+      }
 
-        // Jump to next state
-        switch(state) {
-          case RRC_STATE_CELL_SELECTING:
-            si_acquire_state = SI_ACQUIRE_SIB2;
-            break;
-          case RRC_STATE_PLMN_SELECTION:
-            si_acquire_state = SI_ACQUIRE_IDLE;
-            rrc_log->info("SI Acquisition done. Searching next cell...\n");
-            usleep(5000);
-            phy->cell_search_next();
-            break;
-          default:
-            si_acquire_state = SI_ACQUIRE_IDLE;
-        }
+      current_cell->has_valid_sib1 = true;
 
-        // Send PLMN and TAC to NAS
-        std::stringstream ss;
-        for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
-          nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
-        }
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_SIB2;
+          break;
+        case RRC_STATE_PLMN_SELECTION:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          rrc_log->info("SI Acquisition done. Searching next cell...\n");
+          usleep(5000);
+          phy->cell_search_next();
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+      }
 
-      } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
-                 SI_ACQUIRE_SIB2 == si_acquire_state) {
-        mac->bcch_stop_rx();
+      // Send PLMN and TAC to NAS
+      std::stringstream ss;
+      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+      }
 
-        // Handle SIB2
-        memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-        rrc_log->info("SIB2 received\n");
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
+               SI_ACQUIRE_SIB2 == si_acquire_state) {
+      mac->bcch_stop_rx();
 
-        apply_sib2_configs(&current_cell->sib2);
+      // Handle SIB2
+      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+      rrc_log->info("SIB2 received\n");
 
-        current_cell->has_valid_sib2 = true;
+      apply_sib2_configs(&current_cell->sib2);
 
-        // Jump to next state
-        switch(state) {
-          case RRC_STATE_CELL_SELECTING:
-            si_acquire_state = SI_ACQUIRE_IDLE;
-            state = RRC_STATE_CELL_SELECTED;
-            nas->cell_selected();
-            break;
-          default:
-            si_acquire_state = SI_ACQUIRE_IDLE;
-        }
+      current_cell->has_valid_sib2 = true;
 
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          state = RRC_STATE_CELL_SELECTED;
+          nas->cell_selected();
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
       }
+
     }
   }
+}
 
 
 // Right now, this thread only controls System Information acquisition procedure
-  void rrc::run_thread() {
-    uint32_t tti;
-    uint32_t si_win_start, si_win_len;
-    uint16_t period;
-    uint32_t nof_sib1_trials = 0;
-    const int SIB1_SEARCH_TIMEOUT = 30;
-
-    while (thread_running) {
-      switch (si_acquire_state) {
-        case SI_ACQUIRE_SIB1:
-          // Instruct MAC to look for SIB1
-          tti = mac->get_current_tti();
-          si_win_start = sib_start_tti(tti, 2, 5);
-          mac->bcch_start_rx(si_win_start, 1);
-          rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                         si_win_start, 1);
-          nof_sib1_trials++;
-          if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-            if (state == RRC_STATE_CELL_SELECTING) {
-              select_next_cell_in_plmn();
-              si_acquire_state = SI_ACQUIRE_IDLE;
-            } else if (state == RRC_STATE_PLMN_SELECTION) {
-              phy->cell_search_next();
-            }
-            nof_sib1_trials = 0;
+void rrc::run_thread() {
+  uint32_t tti;
+  uint32_t si_win_start, si_win_len;
+  uint16_t period;
+  uint32_t nof_sib1_trials = 0;
+  const int SIB1_SEARCH_TIMEOUT = 30;
+
+  while (thread_running) {
+    switch (si_acquire_state) {
+      case SI_ACQUIRE_SIB1:
+        // Instruct MAC to look for SIB1
+        tti = mac->get_current_tti();
+        si_win_start = sib_start_tti(tti, 2, 5);
+        mac->bcch_start_rx(si_win_start, 1);
+        rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                       si_win_start, 1);
+        nof_sib1_trials++;
+        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+          if (state == RRC_STATE_CELL_SELECTING) {
+            select_next_cell_in_plmn();
+            si_acquire_state = SI_ACQUIRE_IDLE;
+          } else if (state == RRC_STATE_PLMN_SELECTION) {
+            phy->cell_search_next();
           }
-          usleep(20000);
-          break;
-        case SI_ACQUIRE_SIB2:
-          // Instruct MAC to look for SIB2 only when selecting a cell
-          tti = mac->get_current_tti();
-          period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
-          si_win_start = sib_start_tti(tti, period, 0);
-          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
-
-          mac->bcch_start_rx(si_win_start, si_win_len);
-          rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                         si_win_start, si_win_len);
-          usleep(current_cell->sib1.si_window_length*1000);
-          break;
-        default:
-          break;
-      }
-      usleep(10000);
+          nof_sib1_trials = 0;
+        }
+        usleep(20000);
+        break;
+      case SI_ACQUIRE_SIB2:
+        // Instruct MAC to look for SIB2 only when selecting a cell
+        tti = mac->get_current_tti();
+        period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
+        si_win_start = sib_start_tti(tti, period, 0);
+        si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+
+        mac->bcch_start_rx(si_win_start, si_win_len);
+        rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                       si_win_start, si_win_len);
+        usleep(current_cell->sib1.si_window_length*1000);
+        break;
+      default:
+        break;
     }
+    usleep(10000);
   }
+}
 
 
 
@@ -448,13 +448,13 @@ namespace srsue {
 
 
 /*******************************************************************************
- * 
- * 
- * 
- * Connection control and establishment/reestablishment procedures 
- * 
- * 
- * 
+*
+*
+*
+* Connection control and establishment/reestablishment procedures
+*
+*
+*
 *******************************************************************************/
 
 
@@ -463,1135 +463,1135 @@ namespace srsue {
 
 
 /*******************************************************************************
-  NAS interface
+NAS interface
 *******************************************************************************/
 
-  void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
-    rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
-
-    switch (state) {
-      case RRC_STATE_CONNECTING:
-        send_con_setup_complete(sdu);
-        break;
-      case RRC_STATE_CONNECTED:
-        send_ul_info_transfer(lcid, sdu);
-        break;
-      default:
-        rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
-        break;
-    }
+void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_text[lcid]);
+
+  switch (state) {
+    case RRC_STATE_CONNECTING:
+      send_con_setup_complete(sdu);
+      break;
+    case RRC_STATE_CONNECTED:
+      send_ul_info_transfer(lcid, sdu);
+      break;
+    default:
+      rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
+      break;
   }
+}
 
 
 
 /*******************************************************************************
-  MAC interface
+MAC interface
 *******************************************************************************/
 /* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
-  void rrc::release_pucch_srs() {
-    // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
-    set_phy_default_pucch_srs();
+void rrc::release_pucch_srs() {
+  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
+  set_phy_default_pucch_srs();
 
-    // Configure RX signals without pregeneration because default option is release
-    phy->configure_ul_params(true);
-  }
+  // Configure RX signals without pregeneration because default option is release
+  phy->configure_ul_params(true);
+}
 
-  void rrc::ra_problem() {
-    radio_link_failure();
-  }
+void rrc::ra_problem() {
+  radio_link_failure();
+}
 
 
 /*******************************************************************************
-  GW interface
+GW interface
 *******************************************************************************/
 
-  bool rrc::is_connected() {
-    return (RRC_STATE_CONNECTED == state);
-  }
+bool rrc::is_connected() {
+  return (RRC_STATE_CONNECTED == state);
+}
 
-  bool rrc::have_drb() {
-    return drb_up;
-  }
+bool rrc::have_drb() {
+  return drb_up;
+}
 
 /*******************************************************************************
-  PDCP interface
+PDCP interface
 *******************************************************************************/
 
-  void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_text[lcid]);
-    rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
-
-    switch (lcid) {
-      case RB_ID_SRB0:
-        parse_dl_ccch(pdu);
-        break;
-      case RB_ID_SRB1:
-      case RB_ID_SRB2:
-        parse_dl_dcch(lcid, pdu);
-        break;
-      default:
-        rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
-        break;
-    }
-
+void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_text[lcid]);
+  rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
+
+  switch (lcid) {
+    case RB_ID_SRB0:
+      parse_dl_ccch(pdu);
+      break;
+    case RB_ID_SRB1:
+    case RB_ID_SRB2:
+      parse_dl_dcch(lcid, pdu);
+      break;
+    default:
+      rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
+      break;
   }
 
+}
 
-  void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
-    if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
-      rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
-      rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
-      rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
 
-      LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
-      srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-      bit_buf.N_bits = pdu->N_bytes * 8;
-      pool->deallocate(pdu);
-      liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
+void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
+  if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
+    rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
+    rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
 
-      if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
-        pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
-      }
+    LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
+    pool->deallocate(pdu);
+    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
 
-      LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-      if (!nas->get_s_tmsi(&s_tmsi)) {
-        rrc_log->info("No S-TMSI present in NAS\n");
-        return;
-      }
+    if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
+      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
+    }
 
-      LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
-      for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
-        s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
-        rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
-                      pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-        rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
-                         pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-        if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
-          rrc_log->info("S-TMSI match in paging message\n");
-          rrc_log->console("S-TMSI match in paging message\n");
-          mac->pcch_stop_rx();
-          if (RRC_STATE_IDLE == state) {
-            rrc_log->info("RRC in IDLE state - sending connection request.\n");
-            state = RRC_STATE_CONNECTING;
-            send_con_request();
-          }
+    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+    if (!nas->get_s_tmsi(&s_tmsi)) {
+      rrc_log->info("No S-TMSI present in NAS\n");
+      return;
+    }
+
+    LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
+    for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
+      s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
+      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                    pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                       pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+      if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
+        rrc_log->info("S-TMSI match in paging message\n");
+        rrc_log->console("S-TMSI match in paging message\n");
+        mac->pcch_stop_rx();
+        if (RRC_STATE_IDLE == state) {
+          rrc_log->info("RRC in IDLE state - sending connection request.\n");
+          state = RRC_STATE_CONNECTING;
+          send_con_request();
         }
       }
     }
   }
+}
 
 /*******************************************************************************
-  RLC interface
+RLC interface
 *******************************************************************************/
 
-  void rrc::max_retx_attempted() {
-    //TODO: Handle the radio link failure
-    rrc_log->warning("Max RLC reTx attempted\n");
-    //radio_link_failure();
-  }
+void rrc::max_retx_attempted() {
+  //TODO: Handle the radio link failure
+  rrc_log->warning("Max RLC reTx attempted\n");
+  //radio_link_failure();
+}
 
 /*******************************************************************************
-  Senders
+Senders
 *******************************************************************************/
 
-  void rrc::send_con_request() {
-    rrc_log->debug("Preparing RRC Connection Request\n");
-    LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-
-    // Prepare ConnectionRequest packet
-    ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
-    if (nas->get_s_tmsi(&s_tmsi)) {
-      ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
-      ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
-    } else {
-      ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_RANDOM_VALUE;
-      ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
-    }
-    ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
-    liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    byte_buffer_t *pdcp_buf = pool_allocate;;
-    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
-    pdcp_buf->set_timestamp();
-
-    // Set UE contention resolution ID in MAC
-    uint64_t uecri = 0;
-    uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
-    uint32_t nbytes = 6;
-    for (uint32_t i = 0; i < nbytes; i++) {
-      ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
-    }
-    rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
-
-    mac->set_contention_id(uecri);
-
-    rrc_log->info("Sending RRC Connection Request on SRB0\n");
-    pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
+void rrc::send_con_request() {
+  rrc_log->debug("Preparing RRC Connection Request\n");
+  LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
+  LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+
+  // Prepare ConnectionRequest packet
+  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REQ;
+  if (nas->get_s_tmsi(&s_tmsi)) {
+    ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI;
+    ul_ccch_msg.msg.rrc_con_req.ue_id.s_tmsi = s_tmsi;
+  } else {
+    ul_ccch_msg.msg.rrc_con_req.ue_id_type = LIBLTE_RRC_CON_REQ_UE_ID_TYPE_RANDOM_VALUE;
+    ul_ccch_msg.msg.rrc_con_req.ue_id.random = 1000;
   }
-
-
-/* RRC connection re-establishment procedure (5.3.7) */
-  void rrc::send_con_restablish_request() {
-
-    srslte_cell_t cell;
-    phy->get_current_cell(&cell);
-
-    LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
-    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-
-    // Compute shortMAC-I
-    uint8_t varShortMAC[128], varShortMAC_packed[16];
-    bzero(varShortMAC, 128);
-    bzero(varShortMAC_packed, 16);
-    uint8_t *msg_ptr = varShortMAC;
-    liblte_rrc_pack_cell_identity_ie(0x1a2d0, &msg_ptr);
-    liblte_rrc_pack_phys_cell_id_ie(cell.id, &msg_ptr);
-    mac_interface_rrc::ue_rnti_t ue_rnti;
-    mac->get_rntis(&ue_rnti);
-    liblte_rrc_pack_c_rnti_ie(ue_rnti.crnti, &msg_ptr);
-    srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, msg_ptr - varShortMAC);
-
-    uint8_t mac_key[4];
-    security_128_eia2(&k_rrc_int[16],
-                      1,
-                      1,
-                      1,
-                      varShortMAC_packed,
-                      7,
-                      mac_key);
-
-    mac_interface_rrc::ue_rnti_t uernti;
-    mac->get_rntis(&uernti);
-
-    // Prepare ConnectionRestalishmentRequest packet
-    ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;
-    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = uernti.crnti;
-    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = cell.id;
-    ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2] << 8 | mac_key[3];
-    ul_ccch_msg.msg.rrc_con_reest_req.cause = LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE;
-    liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
-    rrc_log->console("RRC Connection Reestablishment\n");
-    mac_timers->get(t310)->stop();
-    mac_timers->get(t311)->reset();
-    mac_timers->get(t311)->run();
-
-    set_phy_default();
-    mac->reset();
-
-    // FIXME: Cell selection should be different??
-    phy->resync_sfn();
-
-    // Wait for cell re-synchronization
-    uint32_t timeout_cnt = 0;
-    while (!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH) {
-      usleep(10000);
-      timeout_cnt++;
-    }
-    mac_timers->get(t301)->reset();
-    mac_timers->get(t301)->run();
-    mac_timers->get(t311)->stop();
-    rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    byte_buffer_t *pdcp_buf = pool_allocate;;
-    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
-
-    // Set UE contention resolution ID in MAC
-    uint64_t uecri = 0;
-    uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
-    uint32_t nbytes = 6;
-    for (uint32_t i = 0; i < nbytes; i++) {
-      ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
-    }
-    rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
-    mac->set_contention_id(uecri);
-
-    rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
-    pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
+  ul_ccch_msg.msg.rrc_con_req.cause = LIBLTE_RRC_CON_REQ_EST_CAUSE_MO_SIGNALLING;
+  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+  }
+  byte_buffer_t *pdcp_buf = pool_allocate;;
+  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+  pdcp_buf->set_timestamp();
+
+  // Set UE contention resolution ID in MAC
+  uint64_t uecri = 0;
+  uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
+  uint32_t nbytes = 6;
+  for (uint32_t i = 0; i < nbytes; i++) {
+    ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
   }
+  rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
 
+  mac->set_contention_id(uecri);
 
-  void rrc::send_con_restablish_complete() {
-    rrc_log->debug("Preparing RRC Connection Reestablishment Complete\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+  rrc_log->info("Sending RRC Connection Request on SRB0\n");
+  pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
+}
 
-    // Prepare ConnectionSetupComplete packet
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_REEST_COMPLETE;
-    ul_dcch_msg.msg.rrc_con_reest_complete.rrc_transaction_id = transaction_id;
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    byte_buffer_t *pdcp_buf = pool_allocate;;
-    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
-
-    state = RRC_STATE_CONNECTED;
-    rrc_log->console("RRC Connected\n");
-    rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
-    pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
+/* RRC connection re-establishment procedure (5.3.7) */
+void rrc::send_con_restablish_request() {
+
+  srslte_cell_t cell;
+  phy->get_current_cell(&cell);
+
+  LIBLTE_RRC_UL_CCCH_MSG_STRUCT ul_ccch_msg;
+  LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+
+  // Compute shortMAC-I
+  uint8_t varShortMAC[128], varShortMAC_packed[16];
+  bzero(varShortMAC, 128);
+  bzero(varShortMAC_packed, 16);
+  uint8_t *msg_ptr = varShortMAC;
+  liblte_rrc_pack_cell_identity_ie(0x1a2d0, &msg_ptr);
+  liblte_rrc_pack_phys_cell_id_ie(cell.id, &msg_ptr);
+  mac_interface_rrc::ue_rnti_t ue_rnti;
+  mac->get_rntis(&ue_rnti);
+  liblte_rrc_pack_c_rnti_ie(ue_rnti.crnti, &msg_ptr);
+  srslte_bit_pack_vector(varShortMAC, varShortMAC_packed, msg_ptr - varShortMAC);
+
+  uint8_t mac_key[4];
+  security_128_eia2(&k_rrc_int[16],
+                    1,
+                    1,
+                    1,
+                    varShortMAC_packed,
+                    7,
+                    mac_key);
+
+  mac_interface_rrc::ue_rnti_t uernti;
+  mac->get_rntis(&uernti);
+
+  // Prepare ConnectionRestalishmentRequest packet
+  ul_ccch_msg.msg_type = LIBLTE_RRC_UL_CCCH_MSG_TYPE_RRC_CON_REEST_REQ;
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.c_rnti = uernti.crnti;
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.phys_cell_id = cell.id;
+  ul_ccch_msg.msg.rrc_con_reest_req.ue_id.short_mac_i = mac_key[2] << 8 | mac_key[3];
+  ul_ccch_msg.msg.rrc_con_reest_req.cause = LIBLTE_RRC_CON_REEST_REQ_CAUSE_OTHER_FAILURE;
+  liblte_rrc_pack_ul_ccch_msg(&ul_ccch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
+  rrc_log->console("RRC Connection Reestablishment\n");
+  mac_timers->get(t310)->stop();
+  mac_timers->get(t311)->reset();
+  mac_timers->get(t311)->run();
+
+  set_phy_default();
+  mac->reset();
+
+  // FIXME: Cell selection should be different??
+  phy->resync_sfn();
+
+  // Wait for cell re-synchronization
+  uint32_t timeout_cnt = 0;
+  while (!phy->sync_status() && timeout_cnt < TIMEOUT_RESYNC_REESTABLISH) {
+    usleep(10000);
+    timeout_cnt++;
   }
-
-  void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
-    rrc_log->debug("Preparing RRC Connection Setup Complete\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-    // Prepare ConnectionSetupComplete packet
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_SETUP_COMPLETE;
-    ul_dcch_msg.msg.rrc_con_setup_complete.registered_mme_present = false;
-    ul_dcch_msg.msg.rrc_con_setup_complete.rrc_transaction_id = transaction_id;
-    ul_dcch_msg.msg.rrc_con_setup_complete.selected_plmn_id = 1;
-    memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
-    ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    byte_buffer_t *pdcp_buf = pool_allocate;;
-    srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
-    pdcp_buf->N_bytes = bit_buf.N_bits / 8;
-    pdcp_buf->set_timestamp();
-
-    state = RRC_STATE_CONNECTED;
-    rrc_log->console("RRC Connected\n");
-    rrc_log->info("Sending RRC Connection Setup Complete\n");
-    pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
+  mac_timers->get(t301)->reset();
+  mac_timers->get(t301)->run();
+  mac_timers->get(t311)->stop();
+  rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-
-  void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu) {
-    rrc_log->debug("Preparing RX Info Transfer\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-    // Prepare RX INFO packet
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
-    ul_dcch_msg.msg.ul_info_transfer.dedicated_info_type = LIBLTE_RRC_UL_INFORMATION_TRANSFER_TYPE_NAS;
-    memcpy(ul_dcch_msg.msg.ul_info_transfer.dedicated_info.msg, sdu->msg, sdu->N_bytes);
-    ul_dcch_msg.msg.ul_info_transfer.dedicated_info.N_bytes = sdu->N_bytes;
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    // Reset and reuse sdu buffer
-    byte_buffer_t *pdu = sdu;
-    pdu->reset();
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-    pdu->N_bytes = bit_buf.N_bits / 8;
-    pdu->set_timestamp();
-    pdu->set_timestamp();
-
-    rrc_log->info("Sending RX Info Transfer\n");
-    pdcp->write_sdu(lcid, pdu);
+  byte_buffer_t *pdcp_buf = pool_allocate;;
+  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+
+  // Set UE contention resolution ID in MAC
+  uint64_t uecri = 0;
+  uint8_t *ue_cri_ptr = (uint8_t *) &uecri;
+  uint32_t nbytes = 6;
+  for (uint32_t i = 0; i < nbytes; i++) {
+    ue_cri_ptr[nbytes - i - 1] = pdcp_buf->msg[i];
   }
+  rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
+  mac->set_contention_id(uecri);
 
-  void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu) {
-    rrc_log->debug("Preparing Security Mode Complete\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_SECURITY_MODE_COMPLETE;
-    ul_dcch_msg.msg.security_mode_complete.rrc_transaction_id = transaction_id;
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-    pdu->N_bytes = bit_buf.N_bits / 8;
-    pdu->set_timestamp();
+  rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
+  pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
+}
 
-    rrc_log->info("Sending Security Mode Complete\n");
-    pdcp->write_sdu(lcid, pdu);
-  }
 
-  void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu) {
-    rrc_log->debug("Preparing RRC Connection Reconfig Complete\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+void rrc::send_con_restablish_complete() {
+  rrc_log->debug("Preparing RRC Connection Reestablishment Complete\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_RECONFIG_COMPLETE;
-    ul_dcch_msg.msg.rrc_con_reconfig_complete.rrc_transaction_id = transaction_id;
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+  // Prepare ConnectionSetupComplete packet
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_REEST_COMPLETE;
+  ul_dcch_msg.msg.rrc_con_reest_complete.rrc_transaction_id = transaction_id;
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-    pdu->N_bytes = bit_buf.N_bits / 8;
-    pdu->set_timestamp();
-
-    rrc_log->info("Sending RRC Connection Reconfig Complete\n");
-    pdcp->write_sdu(lcid, pdu);
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-
-  void rrc::enable_capabilities() {
-    bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
-    rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
-    phy->set_config_64qam_en(enable_ul_64);
+  byte_buffer_t *pdcp_buf = pool_allocate;;
+  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+
+  state = RRC_STATE_CONNECTED;
+  rrc_log->console("RRC Connected\n");
+  rrc_log->info("Sending RRC Connection Reestablishment Complete\n");
+  pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
+}
+
+void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
+  rrc_log->debug("Preparing RRC Connection Setup Complete\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+  // Prepare ConnectionSetupComplete packet
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_SETUP_COMPLETE;
+  ul_dcch_msg.msg.rrc_con_setup_complete.registered_mme_present = false;
+  ul_dcch_msg.msg.rrc_con_setup_complete.rrc_transaction_id = transaction_id;
+  ul_dcch_msg.msg.rrc_con_setup_complete.selected_plmn_id = 1;
+  memcpy(ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.msg, nas_msg->msg, nas_msg->N_bytes);
+  ul_dcch_msg.msg.rrc_con_setup_complete.dedicated_info_nas.N_bytes = nas_msg->N_bytes;
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
-
-  void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
-    rrc_log->debug("Preparing UE Capability Info\n");
-    LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
-
-    ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
-    ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
-
-    LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
-    info->N_ue_caps = 1;
-    info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
-
-    LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
-    cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
-    cap->ue_category = ue_category;
-
-    cap->pdcp_params.max_rohc_ctxts_present = false;
-    cap->pdcp_params.supported_rohc_profiles[0] = false;
-    cap->pdcp_params.supported_rohc_profiles[1] = false;
-    cap->pdcp_params.supported_rohc_profiles[2] = false;
-    cap->pdcp_params.supported_rohc_profiles[3] = false;
-    cap->pdcp_params.supported_rohc_profiles[4] = false;
-    cap->pdcp_params.supported_rohc_profiles[5] = false;
-    cap->pdcp_params.supported_rohc_profiles[6] = false;
-    cap->pdcp_params.supported_rohc_profiles[7] = false;
-    cap->pdcp_params.supported_rohc_profiles[8] = false;
-
-    cap->phy_params.specific_ref_sigs_supported = false;
-    cap->phy_params.tx_antenna_selection_supported = false;
-
-    //TODO: Generate this from user input?
-    cap->rf_params.N_supported_band_eutras = 3;
-    cap->rf_params.supported_band_eutra[0].band_eutra = 3;
-    cap->rf_params.supported_band_eutra[0].half_duplex = false;
-    cap->rf_params.supported_band_eutra[1].band_eutra = 7;
-    cap->rf_params.supported_band_eutra[1].half_duplex = false;
-    cap->rf_params.supported_band_eutra[2].band_eutra = 20;
-    cap->rf_params.supported_band_eutra[2].half_duplex = false;
-
-    cap->meas_params.N_band_list_eutra = 3;
-    cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
-    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
-    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
-    cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
-    cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
-    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
-    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
-    cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
-    cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
-    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
-    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
-    cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
-
-    cap->feature_group_indicator_present = true;
-    cap->feature_group_indicator = 0x62001000;
-    cap->inter_rat_params.utra_fdd_present = false;
-    cap->inter_rat_params.utra_tdd128_present = false;
-    cap->inter_rat_params.utra_tdd384_present = false;
-    cap->inter_rat_params.utra_tdd768_present = false;
-    cap->inter_rat_params.geran_present = false;
-    cap->inter_rat_params.cdma2000_hrpd_present = false;
-    cap->inter_rat_params.cdma2000_1xrtt_present = false;
-
-    liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
-
-    // Byte align and pack the message bits for PDCP
-    if ((bit_buf.N_bits % 8) != 0) {
-      for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-        bit_buf.msg[bit_buf.N_bits + i] = 0;
-      bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-    }
-    srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-    pdu->N_bytes = bit_buf.N_bits / 8;
-    pdu->set_timestamp();
-
-    rrc_log->info("Sending UE Capability Info\n");
-    pdcp->write_sdu(lcid, pdu);
+  byte_buffer_t *pdcp_buf = pool_allocate;;
+  srslte_bit_pack_vector(bit_buf.msg, pdcp_buf->msg, bit_buf.N_bits);
+  pdcp_buf->N_bytes = bit_buf.N_bits / 8;
+  pdcp_buf->set_timestamp();
+
+  state = RRC_STATE_CONNECTED;
+  rrc_log->console("RRC Connected\n");
+  rrc_log->info("Sending RRC Connection Setup Complete\n");
+  pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
+}
+
+void rrc::send_ul_info_transfer(uint32_t lcid, byte_buffer_t *sdu) {
+  rrc_log->debug("Preparing RX Info Transfer\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+  // Prepare RX INFO packet
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UL_INFO_TRANSFER;
+  ul_dcch_msg.msg.ul_info_transfer.dedicated_info_type = LIBLTE_RRC_UL_INFORMATION_TRANSFER_TYPE_NAS;
+  memcpy(ul_dcch_msg.msg.ul_info_transfer.dedicated_info.msg, sdu->msg, sdu->N_bytes);
+  ul_dcch_msg.msg.ul_info_transfer.dedicated_info.N_bytes = sdu->N_bytes;
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Reset and reuse sdu buffer
+  byte_buffer_t *pdu = sdu;
+  pdu->reset();
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+  }
+  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+  pdu->N_bytes = bit_buf.N_bits / 8;
+  pdu->set_timestamp();
+  pdu->set_timestamp();
+
+  rrc_log->info("Sending RX Info Transfer\n");
+  pdcp->write_sdu(lcid, pdu);
+}
+
+void rrc::send_security_mode_complete(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->debug("Preparing Security Mode Complete\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_SECURITY_MODE_COMPLETE;
+  ul_dcch_msg.msg.security_mode_complete.rrc_transaction_id = transaction_id;
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
+  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+  pdu->N_bytes = bit_buf.N_bits / 8;
+  pdu->set_timestamp();
+
+  rrc_log->info("Sending Security Mode Complete\n");
+  pdcp->write_sdu(lcid, pdu);
+}
+
+void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->debug("Preparing RRC Connection Reconfig Complete\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_RRC_CON_RECONFIG_COMPLETE;
+  ul_dcch_msg.msg.rrc_con_reconfig_complete.rrc_transaction_id = transaction_id;
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+  }
+  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+  pdu->N_bytes = bit_buf.N_bits / 8;
+  pdu->set_timestamp();
+
+  rrc_log->info("Sending RRC Connection Reconfig Complete\n");
+  pdcp->write_sdu(lcid, pdu);
+}
+
+void rrc::enable_capabilities() {
+  bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
+  phy->set_config_64qam_en(enable_ul_64);
+}
+
+void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->debug("Preparing UE Capability Info\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
+  ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
+
+  LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
+  info->N_ue_caps = 1;
+  info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
+
+  LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
+  cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
+  cap->ue_category = ue_category;
+
+  cap->pdcp_params.max_rohc_ctxts_present = false;
+  cap->pdcp_params.supported_rohc_profiles[0] = false;
+  cap->pdcp_params.supported_rohc_profiles[1] = false;
+  cap->pdcp_params.supported_rohc_profiles[2] = false;
+  cap->pdcp_params.supported_rohc_profiles[3] = false;
+  cap->pdcp_params.supported_rohc_profiles[4] = false;
+  cap->pdcp_params.supported_rohc_profiles[5] = false;
+  cap->pdcp_params.supported_rohc_profiles[6] = false;
+  cap->pdcp_params.supported_rohc_profiles[7] = false;
+  cap->pdcp_params.supported_rohc_profiles[8] = false;
+
+  cap->phy_params.specific_ref_sigs_supported = false;
+  cap->phy_params.tx_antenna_selection_supported = false;
+
+  //TODO: Generate this from user input?
+  cap->rf_params.N_supported_band_eutras = 3;
+  cap->rf_params.supported_band_eutra[0].band_eutra = 3;
+  cap->rf_params.supported_band_eutra[0].half_duplex = false;
+  cap->rf_params.supported_band_eutra[1].band_eutra = 7;
+  cap->rf_params.supported_band_eutra[1].half_duplex = false;
+  cap->rf_params.supported_band_eutra[2].band_eutra = 20;
+  cap->rf_params.supported_band_eutra[2].half_duplex = false;
+
+  cap->meas_params.N_band_list_eutra = 3;
+  cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
+  cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
+  cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
+
+  cap->feature_group_indicator_present = true;
+  cap->feature_group_indicator = 0x62001000;
+  cap->inter_rat_params.utra_fdd_present = false;
+  cap->inter_rat_params.utra_tdd128_present = false;
+  cap->inter_rat_params.utra_tdd384_present = false;
+  cap->inter_rat_params.utra_tdd768_present = false;
+  cap->inter_rat_params.geran_present = false;
+  cap->inter_rat_params.cdma2000_hrpd_present = false;
+  cap->inter_rat_params.cdma2000_1xrtt_present = false;
+
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
+  }
+  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+  pdu->N_bytes = bit_buf.N_bits / 8;
+  pdu->set_timestamp();
+
+  rrc_log->info("Sending UE Capability Info\n");
+  pdcp->write_sdu(lcid, pdu);
+}
 
 /*******************************************************************************
-  Parsers
+Parsers
 *******************************************************************************/
 
-  void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-    bit_buf.N_bits = pdu->N_bytes * 8;
-    pool->deallocate(pdu);
-    bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
-    liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_ccch_msg);
-
-    rrc_log->info("SRB0 - Received %s\n",
-                  liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg.msg_type]);
-
-    switch (dl_ccch_msg.msg_type) {
-      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
-        rrc_log->info("Connection Reject received. Wait time: %d\n",
-                      dl_ccch_msg.msg.rrc_con_rej.wait_time);
-        state = RRC_STATE_IDLE;
-        break;
-      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
-        rrc_log->info("Connection Setup received\n");
-        transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
-        handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
-        rrc_log->info("Notifying NAS of connection setup\n");
-        nas->notify_connection_setup();
-        break;
-      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
-        rrc_log->info("Connection Reestablishment received\n");
-        rrc_log->console("Reestablishment OK\n");
-        transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
-        handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
-        break;
-      case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
-        rrc_log->info("Connection Reestablishment Reject received\n");
-        rrc_log->console("Reestablishment Reject\n");
-        usleep(50000);
-        rrc_connection_release();
-        break;
-      default:
-        break;
-    }
+void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  pool->deallocate(pdu);
+  bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
+  liblte_rrc_unpack_dl_ccch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_ccch_msg);
+
+  rrc_log->info("SRB0 - Received %s\n",
+                liblte_rrc_dl_ccch_msg_type_text[dl_ccch_msg.msg_type]);
+
+  switch (dl_ccch_msg.msg_type) {
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
+      rrc_log->info("Connection Reject received. Wait time: %d\n",
+                    dl_ccch_msg.msg.rrc_con_rej.wait_time);
+      state = RRC_STATE_IDLE;
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
+      rrc_log->info("Connection Setup received\n");
+      transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
+      handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
+      rrc_log->info("Notifying NAS of connection setup\n");
+      nas->notify_connection_setup();
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
+      rrc_log->info("Connection Reestablishment received\n");
+      rrc_log->console("Reestablishment OK\n");
+      transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
+      handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
+      break;
+    case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
+      rrc_log->info("Connection Reestablishment Reject received\n");
+      rrc_log->console("Reestablishment Reject\n");
+      usleep(50000);
+      rrc_connection_release();
+      break;
+    default:
+      break;
   }
-
-  void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-    bit_buf.N_bits = pdu->N_bytes * 8;
-    liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_dcch_msg);
-
-    rrc_log->info("%s - Received %s\n",
-                  rb_id_text[lcid],
-                  liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
-
-    // Reset and reuse pdu buffer if possible
-    pdu->reset();
-
-    switch (dl_dcch_msg.msg_type) {
-      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
-        memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg,
-               dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
-        pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
-        nas->write_pdu(lcid, pdu);
-        break;
-      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
-        transaction_id = dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
-
-        cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
-        integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
-
-        // Configure PDCP for security
-        usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
-        pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
-        send_security_mode_complete(lcid, pdu);
-        break;
-      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
-        transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
-        handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
-        break;
-      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
-        transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
-        for (uint32_t i = 0; i < dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++) {
-          if (LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i]) {
-            send_rrc_ue_cap_info(lcid, pdu);
-            break;
-          }
+}
+
+void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dl_dcch_msg);
+
+  rrc_log->info("%s - Received %s\n",
+                rb_id_text[lcid],
+                liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
+
+  // Reset and reuse pdu buffer if possible
+  pdu->reset();
+
+  switch (dl_dcch_msg.msg_type) {
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_DL_INFO_TRANSFER:
+      memcpy(pdu->msg, dl_dcch_msg.msg.dl_info_transfer.dedicated_info.msg,
+             dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes);
+      pdu->N_bytes = dl_dcch_msg.msg.dl_info_transfer.dedicated_info.N_bytes;
+      nas->write_pdu(lcid, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_SECURITY_MODE_COMMAND:
+      transaction_id = dl_dcch_msg.msg.security_mode_cmd.rrc_transaction_id;
+
+      cipher_algo = (CIPHERING_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.cipher_alg;
+      integ_algo = (INTEGRITY_ALGORITHM_ID_ENUM) dl_dcch_msg.msg.security_mode_cmd.sec_algs.int_alg;
+
+      // Configure PDCP for security
+      usim->generate_as_keys(nas->get_ul_count(), k_rrc_enc, k_rrc_int, k_up_enc, k_up_int, cipher_algo, integ_algo);
+      pdcp->config_security(lcid, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+      send_security_mode_complete(lcid, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RECONFIG:
+      transaction_id = dl_dcch_msg.msg.rrc_con_reconfig.rrc_transaction_id;
+      handle_rrc_con_reconfig(lcid, &dl_dcch_msg.msg.rrc_con_reconfig, pdu);
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_UE_CAPABILITY_ENQUIRY:
+      transaction_id = dl_dcch_msg.msg.ue_cap_enquiry.rrc_transaction_id;
+      for (uint32_t i = 0; i < dl_dcch_msg.msg.ue_cap_enquiry.N_ue_cap_reqs; i++) {
+        if (LIBLTE_RRC_RAT_TYPE_EUTRA == dl_dcch_msg.msg.ue_cap_enquiry.ue_capability_request[i]) {
+          send_rrc_ue_cap_info(lcid, pdu);
+          break;
         }
-        break;
-      case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
-        rrc_connection_release();
-        break;
-      default:
-        break;
-    }
+      }
+      break;
+    case LIBLTE_RRC_DL_DCCH_MSG_TYPE_RRC_CON_RELEASE:
+      rrc_connection_release();
+      break;
+    default:
+      break;
   }
+}
 
 
 /*******************************************************************************
-  Timer expiration callback 
+Timer expiration callback
 *******************************************************************************/
-  void rrc::timer_expired(uint32_t timeout_id) {
-    if (timeout_id == t310) {
-      rrc_log->info("Timer T310 expired: Radio Link Failure\n");
-      radio_link_failure();
-    } else if (timeout_id == t311) {
-      rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
-      rrc_connection_release();
-    } else if (timeout_id == t301) {
-      rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
-      rrc_connection_release();
-    } else if (timeout_id == safe_reset_timer) {
-      reset_ue();
-    } else {
-      rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
-    }
+void rrc::timer_expired(uint32_t timeout_id) {
+  if (timeout_id == t310) {
+    rrc_log->info("Timer T310 expired: Radio Link Failure\n");
+    radio_link_failure();
+  } else if (timeout_id == t311) {
+    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
+    rrc_connection_release();
+  } else if (timeout_id == t301) {
+    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
+    rrc_connection_release();
+  } else if (timeout_id == safe_reset_timer) {
+    reset_ue();
+  } else {
+    rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
   }
+}
 
 /*******************************************************************************
-  Helpers
+Helpers
 *******************************************************************************/
 
-  void rrc::reset_ue() {
-    phy->reset();
-    mac->reset();
-    pdcp->reset();
-    rlc->reset();
-    mac->pcch_start_rx();
-    mac_timers->get(safe_reset_timer)->stop();
-    mac_timers->get(safe_reset_timer)->reset();
-    rrc_log->console("RRC Connection released.\n");
-  }
-
-  void rrc::rrc_connection_release() {
-    pthread_mutex_lock(&mutex);
-    drb_up = false;
-    state = RRC_STATE_IDLE;
-    set_phy_default();
-    set_mac_default();
-    mac_timers->get(t311)->run();
-    mac_timers->get(t310)->stop();
-    mac_timers->get(t311)->stop();
-    mac_timers->get(safe_reset_timer)->stop();
-    mac_timers->get(safe_reset_timer)->reset();
-    mac_timers->get(safe_reset_timer)->run();
-    pthread_mutex_unlock(&mutex);
-  }
-
-  void rrc::test_con_restablishment() {
-    printf("Testing connection Reestablishment\n");
-    send_con_restablish_request();
-  }
+void rrc::reset_ue() {
+  phy->reset();
+  mac->reset();
+  pdcp->reset();
+  rlc->reset();
+  mac->pcch_start_rx();
+  mac_timers->get(safe_reset_timer)->stop();
+  mac_timers->get(safe_reset_timer)->reset();
+  rrc_log->console("RRC Connection released.\n");
+}
+
+void rrc::rrc_connection_release() {
+  pthread_mutex_lock(&mutex);
+  drb_up = false;
+  state = RRC_STATE_IDLE;
+  set_phy_default();
+  set_mac_default();
+  mac_timers->get(t311)->run();
+  mac_timers->get(t310)->stop();
+  mac_timers->get(t311)->stop();
+  mac_timers->get(safe_reset_timer)->stop();
+  mac_timers->get(safe_reset_timer)->reset();
+  mac_timers->get(safe_reset_timer)->run();
+  pthread_mutex_unlock(&mutex);
+}
+
+void rrc::test_con_restablishment() {
+  printf("Testing connection Reestablishment\n");
+  send_con_restablish_request();
+}
 
 /* Detection of radio link failure (5.3.11.3) */
-  void rrc::radio_link_failure() {
-    // TODO: Generate and store failure report
-
-    rrc_log->warning("Detected Radio-Link Failure\n");
-    rrc_log->console("Warning: Detected Radio-Link Failure\n");
-    if (state != RRC_STATE_CONNECTED) {
-      rrc_connection_release();
-    } else {
-      send_con_restablish_request();
-    }
+void rrc::radio_link_failure() {
+  // TODO: Generate and store failure report
+
+  rrc_log->warning("Detected Radio-Link Failure\n");
+  rrc_log->console("Warning: Detected Radio-Link Failure\n");
+  if (state != RRC_STATE_CONNECTED) {
+    rrc_connection_release();
+  } else {
+    send_con_restablish_request();
   }
+}
 
 
 // Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
-  uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
-    return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
+uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
+  return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
+}
+
+void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
+
+  // Apply RACH timeAlginmentTimer configuration
+  mac_interface_rrc::mac_cfg_t cfg;
+  mac->get_config(&cfg);
+  cfg.main.time_alignment_timer = sib2->time_alignment_timer;
+  memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT));
+  cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index;
+  mac->set_config(&cfg);
+
+  rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
+                liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
+                liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
+                liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
+
+  // Apply PHY RR Config Common
+  phy_interface_rrc::phy_cfg_common_t common;
+  memcpy(&common.pdsch_cnfg, &sib2->rr_config_common_sib.pdsch_cnfg, sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pusch_cnfg, &sib2->rr_config_common_sib.pusch_cnfg, sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.pucch_cnfg, &sib2->rr_config_common_sib.pucch_cnfg, sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
+  memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl,
+         sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
+  memcpy(&common.prach_cnfg, &sib2->rr_config_common_sib.prach_cnfg, sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
+  if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
+    memcpy(&common.srs_ul_cnfg, &sib2->rr_config_common_sib.srs_ul_cnfg,
+           sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
+  } else {
+    // default is release
+    common.srs_ul_cnfg.present = false;
   }
-
-  void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
-
-    // Apply RACH timeAlginmentTimer configuration
-    mac_interface_rrc::mac_cfg_t cfg;
-    mac->get_config(&cfg);
-    cfg.main.time_alignment_timer = sib2->time_alignment_timer;
-    memcpy(&cfg.rach, &sib2->rr_config_common_sib.rach_cnfg, sizeof(LIBLTE_RRC_RACH_CONFIG_COMMON_STRUCT));
-    cfg.prach_config_index = sib2->rr_config_common_sib.prach_cnfg.root_sequence_index;
-    mac->set_config(&cfg);
-
-    rrc_log->info("Set RACH ConfigCommon: NofPreambles=%d, ResponseWindow=%d, ContentionResolutionTimer=%d ms\n",
-                  liblte_rrc_number_of_ra_preambles_num[sib2->rr_config_common_sib.rach_cnfg.num_ra_preambles],
-                  liblte_rrc_ra_response_window_size_num[sib2->rr_config_common_sib.rach_cnfg.ra_resp_win_size],
-                  liblte_rrc_mac_contention_resolution_timer_num[sib2->rr_config_common_sib.rach_cnfg.mac_con_res_timer]);
-
-    // Apply PHY RR Config Common
-    phy_interface_rrc::phy_cfg_common_t common;
-    memcpy(&common.pdsch_cnfg, &sib2->rr_config_common_sib.pdsch_cnfg, sizeof(LIBLTE_RRC_PDSCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.pusch_cnfg, &sib2->rr_config_common_sib.pusch_cnfg, sizeof(LIBLTE_RRC_PUSCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.pucch_cnfg, &sib2->rr_config_common_sib.pucch_cnfg, sizeof(LIBLTE_RRC_PUCCH_CONFIG_COMMON_STRUCT));
-    memcpy(&common.ul_pwr_ctrl, &sib2->rr_config_common_sib.ul_pwr_ctrl,
-           sizeof(LIBLTE_RRC_UL_POWER_CONTROL_COMMON_STRUCT));
-    memcpy(&common.prach_cnfg, &sib2->rr_config_common_sib.prach_cnfg, sizeof(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT));
-    if (sib2->rr_config_common_sib.srs_ul_cnfg.present) {
-      memcpy(&common.srs_ul_cnfg, &sib2->rr_config_common_sib.srs_ul_cnfg,
-             sizeof(LIBLTE_RRC_SRS_UL_CONFIG_COMMON_STRUCT));
-    } else {
-      // default is release
-      common.srs_ul_cnfg.present = false;
-    }
-    phy->set_config_common(&common);
-
-    phy->configure_ul_params();
-
-    rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
-                  sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
-                  sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
-                  sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
-                  sib2->rr_config_common_sib.pusch_cnfg.n_sb);
-
-    rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
-                  liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
-                  sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
-                  sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
-                  sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
-
-    rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
-                  sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
-                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag ? "yes" : "no",
-                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
-                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
-                  sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
-
-    rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
-                  liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
-                  liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
-                  sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
-
-    mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
-    mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
-    mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
-    N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
-    N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
-
-    rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n",
-                  N310, N311, mac_timers->get(t301)->get_timeout(),
-                  mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
-
+  phy->set_config_common(&common);
+
+  phy->configure_ul_params();
+
+  rrc_log->info("Set PUSCH ConfigCommon: HopOffset=%d, RSGroup=%d, RSNcs=%d, N_sb=%d\n",
+                sib2->rr_config_common_sib.pusch_cnfg.pusch_hopping_offset,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.group_assignment_pusch,
+                sib2->rr_config_common_sib.pusch_cnfg.ul_rs.cyclic_shift,
+                sib2->rr_config_common_sib.pusch_cnfg.n_sb);
+
+  rrc_log->info("Set PUCCH ConfigCommon: DeltaShift=%d, CyclicShift=%d, N1=%d, NRB=%d\n",
+                liblte_rrc_delta_pucch_shift_num[sib2->rr_config_common_sib.pucch_cnfg.delta_pucch_shift],
+                sib2->rr_config_common_sib.pucch_cnfg.n_cs_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n1_pucch_an,
+                sib2->rr_config_common_sib.pucch_cnfg.n_rb_cqi);
+
+  rrc_log->info("Set PRACH ConfigCommon: SeqIdx=%d, HS=%s, FreqOffset=%d, ZC=%d, ConfigIndex=%d\n",
+                sib2->rr_config_common_sib.prach_cnfg.root_sequence_index,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.high_speed_flag ? "yes" : "no",
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_freq_offset,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.zero_correlation_zone_config,
+                sib2->rr_config_common_sib.prach_cnfg.prach_cnfg_info.prach_config_index);
+
+  rrc_log->info("Set SRS ConfigCommon: BW-Configuration=%d, SF-Configuration=%d, ACKNACK=%s\n",
+                liblte_rrc_srs_bw_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.bw_cnfg],
+                liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
+                sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
+
+  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
+  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
+  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+  N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
+  N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
+
+  rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n",
+                N310, N311, mac_timers->get(t301)->get_timeout(),
+                mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
+
+}
+
+// Go through all information elements and apply defaults (9.2.4) if not defined
+void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) {
+  // Get current configuration
+  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg;
+  phy_interface_rrc::phy_cfg_t c;
+  phy->get_config(&c);
+  current_cfg = &c.dedicated;
+
+  if (phy_cnfg->pucch_cnfg_ded_present) {
+    memcpy(&current_cfg->pucch_cnfg_ded, &phy_cnfg->pucch_cnfg_ded, sizeof(LIBLTE_RRC_PUCCH_CONFIG_DEDICATED_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING;
+    current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false;
   }
-
-  // Go through all information elements and apply defaults (9.2.4) if not defined
-  void rrc::apply_phy_config_dedicated(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *phy_cnfg, bool apply_defaults) {
-    // Get current configuration
-    LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT *current_cfg;
-    phy_interface_rrc::phy_cfg_t c;
-    phy->get_config(&c);
-    current_cfg = &c.dedicated;
-
-    if (phy_cnfg->pucch_cnfg_ded_present) {
-      memcpy(&current_cfg->pucch_cnfg_ded, &phy_cnfg->pucch_cnfg_ded, sizeof(LIBLTE_RRC_PUCCH_CONFIG_DEDICATED_STRUCT));
-    } else if (apply_defaults) {
-      current_cfg->pucch_cnfg_ded.tdd_ack_nack_feedback_mode = LIBLTE_RRC_TDD_ACK_NACK_FEEDBACK_MODE_BUNDLING;
-      current_cfg->pucch_cnfg_ded.ack_nack_repetition_setup_present = false;
-    }
-    if (phy_cnfg->pusch_cnfg_ded_present) {
-      memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT));
-    } else if (apply_defaults) {
-      current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10;
-      current_cfg->pusch_cnfg_ded.beta_offset_ri_idx = 12;
-      current_cfg->pusch_cnfg_ded.beta_offset_cqi_idx = 15;
-    }
-    if (phy_cnfg->ul_pwr_ctrl_ded_present) {
-      memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded,
-             sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT));
-    } else if (apply_defaults) {
-      current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch = 0;
-      current_cfg->ul_pwr_ctrl_ded.delta_mcs_en = LIBLTE_RRC_DELTA_MCS_ENABLED_EN0;
-      current_cfg->ul_pwr_ctrl_ded.accumulation_en = true;
-      current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch = 0;
-      current_cfg->ul_pwr_ctrl_ded.p_srs_offset = 7;
-      current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
-      current_cfg->ul_pwr_ctrl_ded.filter_coeff_present = true;
-    }
-    if (phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
-      memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch,
-             sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
+  if (phy_cnfg->pusch_cnfg_ded_present) {
+    memcpy(&current_cfg->pusch_cnfg_ded, &phy_cnfg->pusch_cnfg_ded, sizeof(LIBLTE_RRC_PUSCH_CONFIG_DEDICATED_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->pusch_cnfg_ded.beta_offset_ack_idx = 10;
+    current_cfg->pusch_cnfg_ded.beta_offset_ri_idx = 12;
+    current_cfg->pusch_cnfg_ded.beta_offset_cqi_idx = 15;
+  }
+  if (phy_cnfg->ul_pwr_ctrl_ded_present) {
+    memcpy(&current_cfg->ul_pwr_ctrl_ded, &phy_cnfg->ul_pwr_ctrl_ded,
+           sizeof(LIBLTE_RRC_UL_POWER_CONTROL_DEDICATED_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->ul_pwr_ctrl_ded.p0_ue_pusch = 0;
+    current_cfg->ul_pwr_ctrl_ded.delta_mcs_en = LIBLTE_RRC_DELTA_MCS_ENABLED_EN0;
+    current_cfg->ul_pwr_ctrl_ded.accumulation_en = true;
+    current_cfg->ul_pwr_ctrl_ded.p0_ue_pucch = 0;
+    current_cfg->ul_pwr_ctrl_ded.p_srs_offset = 7;
+    current_cfg->ul_pwr_ctrl_ded.filter_coeff = LIBLTE_RRC_FILTER_COEFFICIENT_FC4;
+    current_cfg->ul_pwr_ctrl_ded.filter_coeff_present = true;
+  }
+  if (phy_cnfg->tpc_pdcch_cnfg_pucch_present) {
+    memcpy(&current_cfg->tpc_pdcch_cnfg_pucch, &phy_cnfg->tpc_pdcch_cnfg_pucch,
+           sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false;
+  }
+  if (phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
+    memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch,
+           sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
+  } else {
+    current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false;
+  }
+  if (phy_cnfg->cqi_report_cnfg_present) {
+    if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
+      memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic,
+             sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
+      current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present;
     } else if (apply_defaults) {
-      current_cfg->tpc_pdcch_cnfg_pucch.setup_present = false;
-    }
-    if (phy_cnfg->tpc_pdcch_cnfg_pusch_present) {
-      memcpy(&current_cfg->tpc_pdcch_cnfg_pusch, &phy_cnfg->tpc_pdcch_cnfg_pusch,
-             sizeof(LIBLTE_RRC_TPC_PDCCH_CONFIG_STRUCT));
-    } else {
-      current_cfg->tpc_pdcch_cnfg_pusch.setup_present = false;
-    }
-    if (phy_cnfg->cqi_report_cnfg_present) {
-      if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-        memcpy(&current_cfg->cqi_report_cnfg.report_periodic, &phy_cnfg->cqi_report_cnfg.report_periodic,
-               sizeof(LIBLTE_RRC_CQI_REPORT_PERIODIC_STRUCT));
-        current_cfg->cqi_report_cnfg.report_periodic_setup_present = phy_cnfg->cqi_report_cnfg.report_periodic_setup_present;
-      } else if (apply_defaults) {
-        current_cfg->cqi_report_cnfg.report_periodic_setup_present = false;
-      }
-      if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-        current_cfg->cqi_report_cnfg.report_mode_aperiodic = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
-        current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present;
-      } else if (apply_defaults) {
-        current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false;
-      }
-      current_cfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset = phy_cnfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset;
+      current_cfg->cqi_report_cnfg.report_periodic_setup_present = false;
     }
-    if (phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
-      memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded,
-             sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT));
+    if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
+      current_cfg->cqi_report_cnfg.report_mode_aperiodic = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic;
+      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present;
     } else if (apply_defaults) {
-      current_cfg->srs_ul_cnfg_ded.setup_present = false;
+      current_cfg->cqi_report_cnfg.report_mode_aperiodic_present = false;
     }
-    if (phy_cnfg->antenna_info_present) {
-      if (!phy_cnfg->antenna_info_default_value) {
-        if (phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
-            phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
-          rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n",
-                         liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
-        }
-        memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value,
-               sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT));
-      } else if (apply_defaults) {
-        current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
-        current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
-        current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
+    current_cfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset = phy_cnfg->cqi_report_cnfg.nom_pdsch_rs_epre_offset;
+  }
+  if (phy_cnfg->srs_ul_cnfg_ded_present && phy_cnfg->srs_ul_cnfg_ded.setup_present) {
+    memcpy(&current_cfg->srs_ul_cnfg_ded, &phy_cnfg->srs_ul_cnfg_ded,
+           sizeof(LIBLTE_RRC_SRS_UL_CONFIG_DEDICATED_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->srs_ul_cnfg_ded.setup_present = false;
+  }
+  if (phy_cnfg->antenna_info_present) {
+    if (!phy_cnfg->antenna_info_default_value) {
+      if (phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_1 &&
+          phy_cnfg->antenna_info_explicit_value.tx_mode != LIBLTE_RRC_TRANSMISSION_MODE_2) {
+        rrc_log->error("Transmission mode TM%s not currently supported by srsUE\n",
+                       liblte_rrc_transmission_mode_text[phy_cnfg->antenna_info_explicit_value.tx_mode]);
       }
+      memcpy(&current_cfg->antenna_info_explicit_value, &phy_cnfg->antenna_info_explicit_value,
+             sizeof(LIBLTE_RRC_ANTENNA_INFO_DEDICATED_STRUCT));
     } else if (apply_defaults) {
       current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
       current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
       current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
     }
-    if (phy_cnfg->sched_request_cnfg_present) {
-      memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg,
-             sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-    } else if (apply_defaults) {
-      current_cfg->sched_request_cnfg.setup_present = false;
+  } else if (apply_defaults) {
+    current_cfg->antenna_info_explicit_value.tx_mode = LIBLTE_RRC_TRANSMISSION_MODE_2;
+    current_cfg->antenna_info_explicit_value.codebook_subset_restriction_present = false;
+    current_cfg->antenna_info_explicit_value.ue_tx_antenna_selection_setup_present = false;
+  }
+  if (phy_cnfg->sched_request_cnfg_present) {
+    memcpy(&current_cfg->sched_request_cnfg, &phy_cnfg->sched_request_cnfg,
+           sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+  } else if (apply_defaults) {
+    current_cfg->sched_request_cnfg.setup_present = false;
+  }
+  if (phy_cnfg->pdsch_cnfg_ded_present) {
+    current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded;
+  } else if (apply_defaults) {
+    current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0;
+  }
+
+  if (phy_cnfg->cqi_report_cnfg_present) {
+    if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
+      rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n",
+                    current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx,
+                    current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx,
+                    liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]);
     }
-    if (phy_cnfg->pdsch_cnfg_ded_present) {
-      current_cfg->pdsch_cnfg_ded = phy_cnfg->pdsch_cnfg_ded;
-    } else if (apply_defaults) {
-      current_cfg->pdsch_cnfg_ded = LIBLTE_RRC_PDSCH_CONFIG_P_A_DB_0;
+    if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
+      rrc_log->info("Set cqi-ReportModeAperiodic=%s\n",
+                    liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]);
     }
 
-    if (phy_cnfg->cqi_report_cnfg_present) {
-      if (phy_cnfg->cqi_report_cnfg.report_periodic_present) {
-        rrc_log->info("Set cqi-PUCCH-ResourceIndex=%d, cqi-pmi-ConfigIndex=%d, cqi-FormatIndicatorPeriodic=%s\n",
-                      current_cfg->cqi_report_cnfg.report_periodic.pucch_resource_idx,
-                      current_cfg->cqi_report_cnfg.report_periodic.pmi_cnfg_idx,
-                      liblte_rrc_cqi_format_indicator_periodic_text[current_cfg->cqi_report_cnfg.report_periodic.format_ind_periodic]);
-      }
-      if (phy_cnfg->cqi_report_cnfg.report_mode_aperiodic_present) {
-        rrc_log->info("Set cqi-ReportModeAperiodic=%s\n",
-                      liblte_rrc_cqi_report_mode_aperiodic_text[current_cfg->cqi_report_cnfg.report_mode_aperiodic]);
-      }
+  }
 
-    }
+  if (phy_cnfg->sched_request_cnfg_present) {
+    rrc_log->info("Set PHY config ded: SR-n_pucch=%d, SR-ConfigIndex=%d, SR-TransMax=%d\n",
+                  current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
+                  current_cfg->sched_request_cnfg.sr_cnfg_idx,
+                  liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
+  }
 
-    if (phy_cnfg->sched_request_cnfg_present) {
-      rrc_log->info("Set PHY config ded: SR-n_pucch=%d, SR-ConfigIndex=%d, SR-TransMax=%d\n",
-                    current_cfg->sched_request_cnfg.sr_pucch_resource_idx,
-                    current_cfg->sched_request_cnfg.sr_cnfg_idx,
-                    liblte_rrc_dsr_trans_max_num[current_cfg->sched_request_cnfg.dsr_trans_max]);
-    }
+  if (current_cfg->srs_ul_cnfg_ded_present) {
+    rrc_log->info("Set PHY config ded: SRS-ConfigIndex=%d, SRS-bw=%s, SRS-Nrcc=%d, SRS-hop=%s, SRS-Ncs=%s\n",
+                  current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
+                  liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
+                  current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
+                  liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
+                  liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
+  }
 
-    if (current_cfg->srs_ul_cnfg_ded_present) {
-      rrc_log->info("Set PHY config ded: SRS-ConfigIndex=%d, SRS-bw=%s, SRS-Nrcc=%d, SRS-hop=%s, SRS-Ncs=%s\n",
-                    current_cfg->srs_ul_cnfg_ded.srs_cnfg_idx,
-                    liblte_rrc_srs_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_bandwidth],
-                    current_cfg->srs_ul_cnfg_ded.freq_domain_pos,
-                    liblte_rrc_srs_hopping_bandwidth_text[current_cfg->srs_ul_cnfg_ded.srs_hopping_bandwidth],
-                    liblte_rrc_cyclic_shift_text[current_cfg->srs_ul_cnfg_ded.cyclic_shift]);
-    }
+  phy->set_config_dedicated(current_cfg);
 
-    phy->set_config_dedicated(current_cfg);
+  // Apply changes to PHY
+  phy->configure_ul_params();
 
-    // Apply changes to PHY
-    phy->configure_ul_params();
+}
 
-  }
+void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) {
+  // Set Default MAC main configuration (9.2.2)
+  LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
+  bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
+  default_cfg.ulsch_cnfg.max_harq_tx = LIBLTE_RRC_MAX_HARQ_TX_N5;
+  default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
+  default_cfg.ulsch_cnfg.retx_bsr_timer = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;
+  default_cfg.ulsch_cnfg.tti_bundling = false;
+  default_cfg.drx_cnfg.setup_present = false;
+  default_cfg.phr_cnfg.setup_present = false;
+  default_cfg.time_alignment_timer = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY;
 
-  void rrc::apply_mac_config_dedicated(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT *mac_cnfg, bool apply_defaults) {
-    // Set Default MAC main configuration (9.2.2)
-    LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT default_cfg;
-    bzero(&default_cfg, sizeof(LIBLTE_RRC_MAC_MAIN_CONFIG_STRUCT));
-    default_cfg.ulsch_cnfg.max_harq_tx = LIBLTE_RRC_MAX_HARQ_TX_N5;
-    default_cfg.ulsch_cnfg.periodic_bsr_timer = LIBLTE_RRC_PERIODIC_BSR_TIMER_INFINITY;
-    default_cfg.ulsch_cnfg.retx_bsr_timer = LIBLTE_RRC_RETRANSMISSION_BSR_TIMER_SF2560;
-    default_cfg.ulsch_cnfg.tti_bundling = false;
-    default_cfg.drx_cnfg.setup_present = false;
-    default_cfg.phr_cnfg.setup_present = false;
-    default_cfg.time_alignment_timer = LIBLTE_RRC_TIME_ALIGNMENT_TIMER_INFINITY;
-
-
-    if (!apply_defaults) {
-      if (mac_cnfg->ulsch_cnfg_present) {
-        if (mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
-          default_cfg.ulsch_cnfg.max_harq_tx = mac_cnfg->ulsch_cnfg.max_harq_tx;
-          default_cfg.ulsch_cnfg.max_harq_tx_present = true;
-        }
-        if (mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
-          default_cfg.ulsch_cnfg.periodic_bsr_timer = mac_cnfg->ulsch_cnfg.periodic_bsr_timer;
-          default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true;
-        }
-        default_cfg.ulsch_cnfg.retx_bsr_timer = mac_cnfg->ulsch_cnfg.retx_bsr_timer;
-        default_cfg.ulsch_cnfg.tti_bundling = mac_cnfg->ulsch_cnfg.tti_bundling;
-      }
-      if (mac_cnfg->drx_cnfg_present) {
-        memcpy(&default_cfg.drx_cnfg, &mac_cnfg->drx_cnfg, sizeof(LIBLTE_RRC_DRX_CONFIG_STRUCT));
-        default_cfg.drx_cnfg_present = true;
+
+  if (!apply_defaults) {
+    if (mac_cnfg->ulsch_cnfg_present) {
+      if (mac_cnfg->ulsch_cnfg.max_harq_tx_present) {
+        default_cfg.ulsch_cnfg.max_harq_tx = mac_cnfg->ulsch_cnfg.max_harq_tx;
+        default_cfg.ulsch_cnfg.max_harq_tx_present = true;
       }
-      if (mac_cnfg->phr_cnfg_present) {
-        memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT));
-        default_cfg.phr_cnfg_present = true;
+      if (mac_cnfg->ulsch_cnfg.periodic_bsr_timer_present) {
+        default_cfg.ulsch_cnfg.periodic_bsr_timer = mac_cnfg->ulsch_cnfg.periodic_bsr_timer;
+        default_cfg.ulsch_cnfg.periodic_bsr_timer_present = true;
       }
-      default_cfg.time_alignment_timer = mac_cnfg->time_alignment_timer;
+      default_cfg.ulsch_cnfg.retx_bsr_timer = mac_cnfg->ulsch_cnfg.retx_bsr_timer;
+      default_cfg.ulsch_cnfg.tti_bundling = mac_cnfg->ulsch_cnfg.tti_bundling;
     }
-
-    // Setup MAC configuration
-    mac->set_config_main(&default_cfg);
-
-    rrc_log->info("Set MAC main config: harq-MaxReTX=%d, bsr-TimerReTX=%d, bsr-TimerPeriodic=%d\n",
-                  liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx],
-                  liblte_rrc_retransmission_bsr_timer_num[default_cfg.ulsch_cnfg.retx_bsr_timer],
-                  liblte_rrc_periodic_bsr_timer_num[default_cfg.ulsch_cnfg.periodic_bsr_timer]);
-    if (default_cfg.phr_cnfg_present) {
-      rrc_log->info("Set MAC PHR config: periodicPHR-Timer=%d, prohibitPHR-Timer=%d, dl-PathlossChange=%d\n",
-                    liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
-                    liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
-                    liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
+    if (mac_cnfg->drx_cnfg_present) {
+      memcpy(&default_cfg.drx_cnfg, &mac_cnfg->drx_cnfg, sizeof(LIBLTE_RRC_DRX_CONFIG_STRUCT));
+      default_cfg.drx_cnfg_present = true;
     }
-  }
-
-  void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {
-    if (cnfg->phy_cnfg_ded_present) {
-      apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
-      // Apply SR configuration to MAC
-      if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
-        mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
-      }
+    if (mac_cnfg->phr_cnfg_present) {
+      memcpy(&default_cfg.phr_cnfg, &mac_cnfg->phr_cnfg, sizeof(LIBLTE_RRC_PHR_CONFIG_STRUCT));
+      default_cfg.phr_cnfg_present = true;
     }
+    default_cfg.time_alignment_timer = mac_cnfg->time_alignment_timer;
+  }
 
-    if (cnfg->mac_main_cnfg_present) {
-      apply_mac_config_dedicated(&cnfg->mac_main_cnfg.explicit_value, cnfg->mac_main_cnfg.default_value);
+  // Setup MAC configuration
+  mac->set_config_main(&default_cfg);
+
+  rrc_log->info("Set MAC main config: harq-MaxReTX=%d, bsr-TimerReTX=%d, bsr-TimerPeriodic=%d\n",
+                liblte_rrc_max_harq_tx_num[default_cfg.ulsch_cnfg.max_harq_tx],
+                liblte_rrc_retransmission_bsr_timer_num[default_cfg.ulsch_cnfg.retx_bsr_timer],
+                liblte_rrc_periodic_bsr_timer_num[default_cfg.ulsch_cnfg.periodic_bsr_timer]);
+  if (default_cfg.phr_cnfg_present) {
+    rrc_log->info("Set MAC PHR config: periodicPHR-Timer=%d, prohibitPHR-Timer=%d, dl-PathlossChange=%d\n",
+                  liblte_rrc_periodic_phr_timer_num[default_cfg.phr_cnfg.periodic_phr_timer],
+                  liblte_rrc_prohibit_phr_timer_num[default_cfg.phr_cnfg.prohibit_phr_timer],
+                  liblte_rrc_dl_pathloss_change_num[default_cfg.phr_cnfg.dl_pathloss_change]);
+  }
+}
+
+void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg) {
+  if (cnfg->phy_cnfg_ded_present) {
+    apply_phy_config_dedicated(&cnfg->phy_cnfg_ded, false);
+    // Apply SR configuration to MAC
+    if (cnfg->phy_cnfg_ded.sched_request_cnfg_present) {
+      mac->set_config_sr(&cnfg->phy_cnfg_ded.sched_request_cnfg);
     }
+  }
 
-    if (cnfg->sps_cnfg_present) {
-      //TODO
-    }
-    if (cnfg->rlf_timers_and_constants_present) {
-      //TODO
-    }
-    for (uint32_t i = 0; i < cnfg->srb_to_add_mod_list_size; i++) {
-      // TODO: handle SRB modification
-      add_srb(&cnfg->srb_to_add_mod_list[i]);
-    }
-    for (uint32_t i = 0; i < cnfg->drb_to_release_list_size; i++) {
-      release_drb(cnfg->drb_to_release_list[i]);
-    }
-    for (uint32_t i = 0; i < cnfg->drb_to_add_mod_list_size; i++) {
-      // TODO: handle DRB modification
-      add_drb(&cnfg->drb_to_add_mod_list[i]);
-    }
+  if (cnfg->mac_main_cnfg_present) {
+    apply_mac_config_dedicated(&cnfg->mac_main_cnfg.explicit_value, cnfg->mac_main_cnfg.default_value);
   }
 
-  void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
-    // Apply the Radio Resource configuration
-    apply_rr_config_dedicated(&setup->rr_cnfg);
+  if (cnfg->sps_cnfg_present) {
+    //TODO
+  }
+  if (cnfg->rlf_timers_and_constants_present) {
+    //TODO
   }
+  for (uint32_t i = 0; i < cnfg->srb_to_add_mod_list_size; i++) {
+    // TODO: handle SRB modification
+    add_srb(&cnfg->srb_to_add_mod_list[i]);
+  }
+  for (uint32_t i = 0; i < cnfg->drb_to_release_list_size; i++) {
+    release_drb(cnfg->drb_to_release_list[i]);
+  }
+  for (uint32_t i = 0; i < cnfg->drb_to_add_mod_list_size; i++) {
+    // TODO: handle DRB modification
+    add_drb(&cnfg->drb_to_add_mod_list[i]);
+  }
+}
+
+void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
+  // Apply the Radio Resource configuration
+  apply_rr_config_dedicated(&setup->rr_cnfg);
+}
 
 /* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
-  void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
-    mac_timers->get(t301)->stop();
+void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
+  mac_timers->get(t301)->stop();
 
-    // TODO: Restablish DRB1. Not done because never was suspended
+  // TODO: Restablish DRB1. Not done because never was suspended
 
-    // Apply the Radio Resource configuration
-    apply_rr_config_dedicated(&setup->rr_cnfg);
+  // Apply the Radio Resource configuration
+  apply_rr_config_dedicated(&setup->rr_cnfg);
 
-    // TODO: Some security stuff here... is it necessary?
+  // TODO: Some security stuff here... is it necessary?
 
-    send_con_restablish_complete();
-  }
+  send_con_restablish_complete();
+}
 
 
-  void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
-                                    byte_buffer_t *pdu) {
-    uint32_t i;
+void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
+                                  byte_buffer_t *pdu) {
+  uint32_t i;
 
-    if (reconfig->rr_cnfg_ded_present) {
-      apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
-    } else {
-      printf("received con reconfig no rr confg present\n");
-    }
-    if (reconfig->meas_cnfg_present) {
-      //TODO: handle meas_cnfg
-    }
-    if (reconfig->mob_ctrl_info_present) {
-      //TODO: handle mob_ctrl_info
-    }
+  if (reconfig->rr_cnfg_ded_present) {
+    apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
+  } else {
+    printf("received con reconfig no rr confg present\n");
+  }
+  if (reconfig->meas_cnfg_present) {
+    //TODO: handle meas_cnfg
+  }
+  if (reconfig->mob_ctrl_info_present) {
+    //TODO: handle mob_ctrl_info
+  }
 
-    send_rrc_con_reconfig_complete(lcid, pdu);
+  send_rrc_con_reconfig_complete(lcid, pdu);
 
-    byte_buffer_t *nas_sdu;
-    for (i = 0; i < reconfig->N_ded_info_nas; i++) {
-      nas_sdu = pool_allocate;;
-      memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
-      nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
-      nas->write_pdu(lcid, nas_sdu);
+  byte_buffer_t *nas_sdu;
+  for (i = 0; i < reconfig->N_ded_info_nas; i++) {
+    nas_sdu = pool_allocate;;
+    memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
+    nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
+    nas->write_pdu(lcid, nas_sdu);
+  }
+}
+
+void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg) {
+  // Setup PDCP
+  pdcp->add_bearer(srb_cnfg->srb_id);
+  if (RB_ID_SRB2 == srb_cnfg->srb_id)
+    pdcp->config_security(srb_cnfg->srb_id, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
+
+  // Setup RLC
+  if (srb_cnfg->rlc_cnfg_present) {
+    if (srb_cnfg->rlc_default_cnfg_present) {
+      rlc->add_bearer(srb_cnfg->srb_id);
+    } else {
+      rlc->add_bearer(srb_cnfg->srb_id, &srb_cnfg->rlc_explicit_cnfg);
     }
   }
 
-  void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg) {
-    // Setup PDCP
-    pdcp->add_bearer(srb_cnfg->srb_id);
-    if (RB_ID_SRB2 == srb_cnfg->srb_id)
-      pdcp->config_security(srb_cnfg->srb_id, k_rrc_enc, k_rrc_int, cipher_algo, integ_algo);
-
-    // Setup RLC
-    if (srb_cnfg->rlc_cnfg_present) {
-      if (srb_cnfg->rlc_default_cnfg_present) {
-        rlc->add_bearer(srb_cnfg->srb_id);
-      } else {
-        rlc->add_bearer(srb_cnfg->srb_id, &srb_cnfg->rlc_explicit_cnfg);
-      }
-    }
+  // Setup MAC
+  uint8_t log_chan_group = 0;
+  uint8_t priority = 1;
+  int prioritized_bit_rate = -1;
+  int bucket_size_duration = -1;
 
-    // Setup MAC
-    uint8_t log_chan_group = 0;
-    uint8_t priority = 1;
-    int prioritized_bit_rate = -1;
-    int bucket_size_duration = -1;
-
-    if (srb_cnfg->lc_cnfg_present) {
-      if (srb_cnfg->lc_default_cnfg_present) {
-        if (RB_ID_SRB2 == srb_cnfg->srb_id)
-          priority = 3;
-      } else {
-        if (srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present) {
-          //TODO
-        }
-        if (srb_cnfg->lc_explicit_cnfg.ul_specific_params_present) {
-          if (srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
-            log_chan_group = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
+  if (srb_cnfg->lc_cnfg_present) {
+    if (srb_cnfg->lc_default_cnfg_present) {
+      if (RB_ID_SRB2 == srb_cnfg->srb_id)
+        priority = 3;
+    } else {
+      if (srb_cnfg->lc_explicit_cnfg.log_chan_sr_mask_present) {
+        //TODO
+      }
+      if (srb_cnfg->lc_explicit_cnfg.ul_specific_params_present) {
+        if (srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group_present)
+          log_chan_group = srb_cnfg->lc_explicit_cnfg.ul_specific_params.log_chan_group;
 
-          priority = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
-          prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
-          bucket_size_duration = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
-        }
+        priority = srb_cnfg->lc_explicit_cnfg.ul_specific_params.priority;
+        prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.prioritized_bit_rate];
+        bucket_size_duration = liblte_rrc_bucket_size_duration_num[srb_cnfg->lc_explicit_cnfg.ul_specific_params.bucket_size_duration];
       }
-      mac->setup_lcid(srb_cnfg->srb_id, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
     }
-
-    srbs[srb_cnfg->srb_id] = *srb_cnfg;
-    rrc_log->info("Added radio bearer %s\n", rb_id_text[srb_cnfg->srb_id]);
+    mac->setup_lcid(srb_cnfg->srb_id, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
   }
 
-  void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg) {
+  srbs[srb_cnfg->srb_id] = *srb_cnfg;
+  rrc_log->info("Added radio bearer %s\n", rb_id_text[srb_cnfg->srb_id]);
+}
 
-    if (!drb_cnfg->pdcp_cnfg_present ||
-        !drb_cnfg->rlc_cnfg_present ||
-        !drb_cnfg->lc_cnfg_present) {
-      rrc_log->error("Cannot add DRB - incomplete configuration\n");
-      return;
-    }
-    uint32_t lcid = 0;
-    if (drb_cnfg->lc_id_present) {
-      lcid = drb_cnfg->lc_id;
-    } else {
-      lcid = RB_ID_SRB2 + drb_cnfg->drb_id;
-      rrc_log->warning("LCID not present, using %d\n", lcid);
-    }
+void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg) {
 
-    // Setup PDCP
-    pdcp->add_bearer(lcid, &drb_cnfg->pdcp_cnfg);
-    // TODO: setup PDCP security (using k_up_enc)
-
-    // Setup RLC
-    rlc->add_bearer(lcid, &drb_cnfg->rlc_cnfg);
-
-    // Setup MAC
-    uint8_t log_chan_group = 0;
-    uint8_t priority = 1;
-    int prioritized_bit_rate = -1;
-    int bucket_size_duration = -1;
-    if (drb_cnfg->lc_cnfg.ul_specific_params_present) {
-      if (drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
-        log_chan_group = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
-      } else {
-        rrc_log->warning("LCG not present, setting to 0\n");
-      }
-      priority = drb_cnfg->lc_cnfg.ul_specific_params.priority;
-      prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
+  if (!drb_cnfg->pdcp_cnfg_present ||
+      !drb_cnfg->rlc_cnfg_present ||
+      !drb_cnfg->lc_cnfg_present) {
+    rrc_log->error("Cannot add DRB - incomplete configuration\n");
+    return;
+  }
+  uint32_t lcid = 0;
+  if (drb_cnfg->lc_id_present) {
+    lcid = drb_cnfg->lc_id;
+  } else {
+    lcid = RB_ID_SRB2 + drb_cnfg->drb_id;
+    rrc_log->warning("LCID not present, using %d\n", lcid);
+  }
 
-      if (prioritized_bit_rate > 0) {
-        rrc_log->warning("PBR>0 currently not supported. Setting it to Inifinty\n");
-        prioritized_bit_rate = -1;
-      }
+  // Setup PDCP
+  pdcp->add_bearer(lcid, &drb_cnfg->pdcp_cnfg);
+  // TODO: setup PDCP security (using k_up_enc)
+
+  // Setup RLC
+  rlc->add_bearer(lcid, &drb_cnfg->rlc_cnfg);
+
+  // Setup MAC
+  uint8_t log_chan_group = 0;
+  uint8_t priority = 1;
+  int prioritized_bit_rate = -1;
+  int bucket_size_duration = -1;
+  if (drb_cnfg->lc_cnfg.ul_specific_params_present) {
+    if (drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group_present) {
+      log_chan_group = drb_cnfg->lc_cnfg.ul_specific_params.log_chan_group;
+    } else {
+      rrc_log->warning("LCG not present, setting to 0\n");
+    }
+    priority = drb_cnfg->lc_cnfg.ul_specific_params.priority;
+    prioritized_bit_rate = liblte_rrc_prioritized_bit_rate_num[drb_cnfg->lc_cnfg.ul_specific_params.prioritized_bit_rate];
 
-      bucket_size_duration = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
+    if (prioritized_bit_rate > 0) {
+      rrc_log->warning("PBR>0 currently not supported. Setting it to Inifinty\n");
+      prioritized_bit_rate = -1;
     }
-    mac->setup_lcid(lcid, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
 
-    drbs[lcid] = *drb_cnfg;
-    drb_up = true;
-    rrc_log->info("Added radio bearer %s\n", rb_id_text[lcid]);
+    bucket_size_duration = liblte_rrc_bucket_size_duration_num[drb_cnfg->lc_cnfg.ul_specific_params.bucket_size_duration];
   }
+  mac->setup_lcid(lcid, log_chan_group, priority, prioritized_bit_rate, bucket_size_duration);
 
-  void rrc::release_drb(uint8_t lcid) {
-    // TODO
-  }
+  drbs[lcid] = *drb_cnfg;
+  drb_up = true;
+  rrc_log->info("Added radio bearer %s\n", rb_id_text[lcid]);
+}
+
+void rrc::release_drb(uint8_t lcid) {
+  // TODO
+}
 
 /************************** 
- *  DEFAULT VALUES Section 9
+*  DEFAULT VALUES Section 9
 ****************************/
 
 // PHY CONFIG DEDICATED Defaults (3GPP 36.331 v10 9.2.4)
-  void rrc::set_phy_default_pucch_srs() {
-
-    phy_interface_rrc::phy_cfg_t current_cfg;
-    phy->get_config(&current_cfg);
-
-    // Set defaults to CQI, SRS and SR
-    current_cfg.dedicated.cqi_report_cnfg_present = false;
-    current_cfg.dedicated.srs_ul_cnfg_ded_present = false;
-    current_cfg.dedicated.sched_request_cnfg_present = false;
-
-    apply_phy_config_dedicated(&current_cfg.dedicated, true);
-
-    // Release SR configuration from MAC
-    LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg;
-    bzero(&cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-    mac->set_config_sr(&cfg);
-  }
-
-  void rrc::set_phy_default() {
-    LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults;
-    bzero(&defaults, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
-    apply_phy_config_dedicated(&defaults, true);
-  }
-
-  void rrc::set_mac_default() {
-    apply_mac_config_dedicated(NULL, true);
-    LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg;
-    bzero(&sr_cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
-    sr_cfg.setup_present = false;
-    mac->set_config_sr(&sr_cfg);
-  }
-
-  void rrc::set_rrc_default() {
-    N310 = 1;
-    N311 = 1;
-    t301 = mac_timers->get_unique_id();
-    t310 = mac_timers->get_unique_id();
-    t311 = mac_timers->get_unique_id();
-    safe_reset_timer = mac_timers->get_unique_id();
-    mac_timers->get(t310)->set(this, 1000);
-    mac_timers->get(t311)->set(this, 1000);
-    mac_timers->get(safe_reset_timer)->set(this, 10);
-  }
+void rrc::set_phy_default_pucch_srs() {
+
+  phy_interface_rrc::phy_cfg_t current_cfg;
+  phy->get_config(&current_cfg);
+
+  // Set defaults to CQI, SRS and SR
+  current_cfg.dedicated.cqi_report_cnfg_present = false;
+  current_cfg.dedicated.srs_ul_cnfg_ded_present = false;
+  current_cfg.dedicated.sched_request_cnfg_present = false;
+
+  apply_phy_config_dedicated(&current_cfg.dedicated, true);
+
+  // Release SR configuration from MAC
+  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT cfg;
+  bzero(&cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+  mac->set_config_sr(&cfg);
+}
+
+void rrc::set_phy_default() {
+  LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT defaults;
+  bzero(&defaults, sizeof(LIBLTE_RRC_PHYSICAL_CONFIG_DEDICATED_STRUCT));
+  apply_phy_config_dedicated(&defaults, true);
+}
+
+void rrc::set_mac_default() {
+  apply_mac_config_dedicated(NULL, true);
+  LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT sr_cfg;
+  bzero(&sr_cfg, sizeof(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT));
+  sr_cfg.setup_present = false;
+  mac->set_config_sr(&sr_cfg);
+}
+
+void rrc::set_rrc_default() {
+  N310 = 1;
+  N311 = 1;
+  t301 = mac_timers->get_unique_id();
+  t310 = mac_timers->get_unique_id();
+  t311 = mac_timers->get_unique_id();
+  safe_reset_timer = mac_timers->get_unique_id();
+  mac_timers->get(t310)->set(this, 1000);
+  mac_timers->get(t311)->set(this, 1000);
+  mac_timers->get(safe_reset_timer)->set(this, 10);
+}
 
 } // namespace srsue

From 02439f291d7b8ccfd6d9576e7fef2a192f65b39e Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Thu, 31 Aug 2017 19:25:22 +0100
Subject: [PATCH 117/269] Add n_prb description

---
 srsenb/enb.conf.example | 1 +
 1 file changed, 1 insertion(+)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 7446df122..1c54cfc45 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -12,6 +12,7 @@
 # mnc:            Mobile Network Code
 # mme_addr:       IP address of MME for S1 connnection
 # gtp_bind_addr:  Local IP address to bind for GTP connection
+# n_prb:          Number of Physical Resource Blocks (6,15,25,50,75,100)
 #
 #####################################################################
 [enb]

From 409d81f43c82edf80c5298bdde2e0eb47d9ad107 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 1 Sep 2017 12:19:38 +0200
Subject: [PATCH 118/269] Corrected comments and a few lines refactor in pdsch

---
 lib/src/phy/mimo/precoding.c |  4 ++--
 lib/src/phy/phch/pdsch.c     | 18 ++++++++----------
 2 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 02faa6e1c..f1aab3b5d 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -1406,9 +1406,9 @@ int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_P
 #endif /* LV_HAVE_AVX */
     }
   } else if (nof_ports == 4) {
-    ERROR("Error predecoding CCD: Only 2 ports supported");
+    ERROR("Error predecoding multiplex: not implemented for %d Tx ports", nof_ports);
   } else {
-    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
   }
   return SRSLTE_ERROR;
 }
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 018fb96e5..9f0e4ed12 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -611,21 +611,19 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       for (i = 0; i < cfg->nof_layers; i++) {
         x[i] = q->d[i];
       }
-
-      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
-                                    cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
-
     } else {
-      /* number of layers equals number of ports */
-      for (i = 0; i < cfg->nof_layers; i++) {
-        x[i] = q->x[i];
-      }
-      memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
+        /* number of layers equals number of ports */
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->x[i];
+        }
+        memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
+    }
 
       srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
                                     cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
-      srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+    if (cfg->nof_layers != cfg->grant.nof_tb) {
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 

From e5feec19342df5579e7025e8180d788deb662f58 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 1 Sep 2017 13:29:11 +0200
Subject: [PATCH 119/269] Temporal commit before merge. Changed sequence
 objects to avoid reallocating memory. new NAS/RRC working with initial
 attachment

---
 CMakeLists.txt                                |   2 +
 cmake/modules/FindMbedTLS.cmake               |   2 +-
 lib/examples/cell_measurement.c               |  28 +-
 lib/examples/cell_search.c                    |  30 +-
 lib/examples/pdsch_enodeb.c                   |  32 +-
 lib/examples/pdsch_ue.c                       |  29 +-
 lib/examples/usrp_capture_sync.c              |   8 +-
 lib/include/srslte/interfaces/ue_interfaces.h |  16 +-
 .../srslte/phy/ch_estimation/chest_dl.h       |   7 +-
 .../srslte/phy/ch_estimation/chest_ul.h       |   8 +-
 .../srslte/phy/ch_estimation/refsignal_dl.h   |   5 +-
 .../srslte/phy/ch_estimation/refsignal_ul.h   |  13 +-
 lib/include/srslte/phy/common/sequence.h      |   8 +-
 lib/include/srslte/phy/dft/dft.h              |  13 +-
 lib/include/srslte/phy/dft/dft_precoding.h    |  20 +-
 lib/include/srslte/phy/dft/ofdm.h             |  14 +-
 lib/include/srslte/phy/enb/enb_dl.h           |   7 +-
 lib/include/srslte/phy/enb/enb_ul.h           |  15 +-
 lib/include/srslte/phy/phch/pbch.h            |  10 +-
 lib/include/srslte/phy/phch/pcfich.h          |  10 +-
 lib/include/srslte/phy/phch/pdcch.h           |  14 +-
 lib/include/srslte/phy/phch/pdsch.h           |  30 +-
 lib/include/srslte/phy/phch/phich.h           |  12 +-
 lib/include/srslte/phy/phch/prach.h           |  25 +-
 lib/include/srslte/phy/phch/pucch.h           |  16 +-
 lib/include/srslte/phy/phch/pusch.h           |  15 +-
 lib/include/srslte/phy/resampling/interp.h    |  14 +-
 lib/include/srslte/phy/sync/cfo.h             |   5 +-
 lib/include/srslte/phy/sync/cp.h              |   4 +
 lib/include/srslte/phy/sync/pss.h             |   8 +
 lib/include/srslte/phy/sync/sss.h             |   3 +-
 lib/include/srslte/phy/sync/sync.h            |   7 +
 lib/include/srslte/phy/ue/ue_dl.h             |   9 +-
 lib/include/srslte/phy/ue/ue_mib.h            |  21 +-
 lib/include/srslte/phy/ue/ue_sync.h           |  26 +-
 lib/include/srslte/phy/ue/ue_ul.h             |   7 +-
 lib/include/srslte/phy/utils/convolution.h    |   5 +
 lib/include/srslte/upper/gw.h                 |   3 +-
 lib/src/phy/ch_estimation/chest_dl.c          |  71 ++-
 lib/src/phy/ch_estimation/chest_ul.c          |  63 ++-
 lib/src/phy/ch_estimation/refsignal_dl.c      | 126 ++---
 lib/src/phy/ch_estimation/refsignal_ul.c      | 101 ++--
 .../phy/ch_estimation/test/chest_test_dl.c    |  10 +-
 .../phy/ch_estimation/test/chest_test_ul.c    |  10 +-
 .../ch_estimation/test/refsignal_ul_test.c    |   7 +-
 lib/src/phy/common/phy_common.c               |   4 +-
 lib/src/phy/common/sequence.c                 |  88 +++-
 lib/src/phy/dft/dft_fftw.c                    |  47 ++
 lib/src/phy/dft/dft_precoding.c               |  52 +-
 lib/src/phy/dft/ofdm.c                        | 109 +++-
 lib/src/phy/enb/enb_dl.c                      | 101 +++-
 lib/src/phy/enb/enb_ul.c                      | 113 +++--
 lib/src/phy/phch/pbch.c                       |  75 +--
 lib/src/phy/phch/pcfich.c                     |  46 +-
 lib/src/phy/phch/pdcch.c                      |  62 ++-
 lib/src/phy/phch/pdsch.c                      | 170 ++++---
 lib/src/phy/phch/phich.c                      |  46 +-
 lib/src/phy/phch/prach.c                      | 172 ++++---
 lib/src/phy/phch/pucch.c                      |  66 +--
 lib/src/phy/phch/pusch.c                      | 163 +++---
 lib/src/phy/phch/sch.c                        |  10 +-
 lib/src/phy/phch/sequences.c                  |   7 -
 lib/src/phy/phch/test/pbch_file_test.c        |  12 +-
 lib/src/phy/phch/test/pbch_test.c             |   6 +-
 lib/src/phy/phch/test/pcfich_file_test.c      |  12 +-
 lib/src/phy/phch/test/pcfich_test.c           |   6 +-
 lib/src/phy/phch/test/pdcch_file_test.c       |  14 +-
 lib/src/phy/phch/test/pdcch_test.c            |   6 +-
 lib/src/phy/phch/test/pdsch_pdcch_file_test.c |   8 +-
 lib/src/phy/phch/test/pdsch_test.c            |  16 +-
 lib/src/phy/phch/test/phich_file_test.c       |  13 +-
 lib/src/phy/phch/test/phich_test.c            |   8 +-
 lib/src/phy/phch/test/prach_test.c            |  14 +-
 lib/src/phy/phch/test/prach_test_multi.c      |  14 +-
 lib/src/phy/phch/test/prach_test_usrp.c       |   3 +
 lib/src/phy/phch/test/pucch_test.c            |  14 +-
 lib/src/phy/phch/test/pusch_test.c            |  43 +-
 lib/src/phy/resampling/interp.c               |  39 +-
 lib/src/phy/rf/rf_utils.c                     |  10 +-
 lib/src/phy/scrambling/scrambling.c           |  14 +-
 lib/src/phy/scrambling/test/scrambling_test.c |  22 +-
 lib/src/phy/sync/cfo.c                        |  16 +-
 lib/src/phy/sync/cp.c                         |  16 +-
 lib/src/phy/sync/pss.c                        |  90 +++-
 lib/src/phy/sync/sss.c                        |  16 +-
 lib/src/phy/sync/sync.c                       |  84 ++-
 lib/src/phy/ue/ue_cell_search.c               |  29 +-
 lib/src/phy/ue/ue_dl.c                        | 126 +++--
 lib/src/phy/ue/ue_mib.c                       | 119 +++--
 lib/src/phy/ue/ue_sync.c                      | 187 ++++---
 lib/src/phy/ue/ue_ul.c                        |  85 +++-
 lib/src/phy/utils/convolution.c               |  36 +-
 lib/src/upper/gw.cc                           |   8 +-
 srsenb/src/phy/phch_worker.cc                 |  21 +-
 srsue/hdr/mac/mac.h                           |   1 -
 srsue/hdr/phy/phch_recv.h                     |  31 +-
 srsue/hdr/phy/phch_worker.h                   |  15 +-
 srsue/hdr/phy/phy.h                           |  16 +-
 srsue/hdr/phy/prach.h                         |  16 +-
 srsue/hdr/ue.h                                |   1 +
 srsue/hdr/upper/nas.h                         |   2 -
 srsue/hdr/upper/rrc.h                         |  10 +-
 srsue/src/mac/dl_harq.cc                      |   4 +-
 srsue/src/mac/mac.cc                          |  16 +-
 srsue/src/mac/proc_ra.cc                      |   6 +
 srsue/src/mac/ul_harq.cc                      |   2 +-
 srsue/src/phy/phch_common.cc                  |   4 +-
 srsue/src/phy/phch_recv.cc                    | 479 ++++++++++--------
 srsue/src/phy/phch_worker.cc                  |  99 ++--
 srsue/src/phy/phy.cc                          |  70 ++-
 srsue/src/phy/prach.cc                        | 140 ++---
 srsue/src/ue.cc                               |  25 +-
 srsue/src/upper/nas.cc                        |   8 -
 srsue/src/upper/rrc.cc                        | 179 ++++---
 114 files changed, 2949 insertions(+), 1502 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 17609e65f..2f020db8f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,6 +95,8 @@ else(POLARSSL_FOUND)
       set(SEC_LIBRARIES    "${MBEDTLS_LIBRARIES}")
     endif(BUILD_STATIC)
     add_definitions(-DHAVE_MBEDTLS)
+  else(MBEDTLS_FOUND)
+    message(FATAL_ERROR "Either PolarSSL or mbedTLS are required to build srsLTE")
   endif (MBEDTLS_FOUND)
 endif(POLARSSL_FOUND)
 
diff --git a/cmake/modules/FindMbedTLS.cmake b/cmake/modules/FindMbedTLS.cmake
index 11499bad4..2c9464e79 100644
--- a/cmake/modules/FindMbedTLS.cmake
+++ b/cmake/modules/FindMbedTLS.cmake
@@ -50,5 +50,5 @@ message(STATUS "MBEDTLS STATIC LIBRARIES: " ${MBEDTLS_STATIC_LIBRARIES})
 message(STATUS "MBEDTLS INCLUDE DIRS: " ${MBEDTLS_INCLUDE_DIRS})
 
 INCLUDE(FindPackageHandleStandardArgs)
-FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(MBEDTLS DEFAULT_MSG MBEDTLS_LIBRARIES MBEDTLS_INCLUDE_DIRS)
 MARK_AS_ADVANCED(MBEDTLS_LIBRARIES MBEDTLS_STATIC_LIBRARIES MBEDTLS_INCLUDE_DIRS)
diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index bc8262b29..96e98f055 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -237,19 +237,31 @@ int main(int argc, char **argv) {
   srslte_rf_stop_rx_stream(&rf);
   srslte_rf_flush_buffer(&rf);
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     return -1; 
   }
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, 1)) { 
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    fprintf(stderr, "Error initiating ue_sync\n");
+    return -1;
+  }
+  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, 1)) {
+    fprintf(stderr, "Error initiating UE downlink processing module\n");
+    return -1;
+  }
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     return -1;
   }
-  if (srslte_ue_mib_init(&ue_mib, cell)) {
+  if (srslte_ue_mib_init(&ue_mib, cell.nof_prb)) {
     fprintf(stderr, "Error initaiting UE MIB decoder\n");
     return -1;
   }
-  
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
+    fprintf(stderr, "Error initaiting UE MIB decoder\n");
+    return -1;
+  }
+
   /* Configure downlink receiver for the SI-RNTI since will be the only one we'll use */
   srslte_ue_dl_set_rnti(&ue_dl, SRSLTE_SIRNTI); 
 
@@ -260,11 +272,15 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error initiating FFT\n");
     return -1;
   }
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
     fprintf(stderr, "Error initiating channel estimator\n");
     return -1;
   }
-  
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
+    fprintf(stderr, "Error initiating channel estimator\n");
+    return -1;
+  }
+
   int sf_re = SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);
 
   cf_t *sf_symbols = srslte_vec_malloc(sf_re * sizeof(cf_t));
diff --git a/lib/examples/cell_search.c b/lib/examples/cell_search.c
index 8ab560e0c..281e3d102 100644
--- a/lib/examples/cell_search.c
+++ b/lib/examples/cell_search.c
@@ -188,6 +188,18 @@ int main(int argc, char **argv) {
   sigprocmask(SIG_UNBLOCK, &sigset, NULL);
   signal(SIGINT, sig_int_handler);
 
+  if (srslte_ue_cellsearch_init_multi(&cs, cell_detect_config.max_frames_pss, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+    fprintf(stderr, "Error initiating UE cell detect\n");
+    exit(-1);
+  }
+
+  if (cell_detect_config.max_frames_pss) {
+    srslte_ue_cellsearch_set_nof_valid_frames(&cs, cell_detect_config.nof_valid_pss_frames);
+  }
+  if (cell_detect_config.init_agc) {
+    srslte_ue_sync_start_agc(&cs.ue_sync, srslte_rf_set_rx_gain_wrapper, cell_detect_config.init_agc);
+  }
+
   for (freq=0;freq<nof_freqs && !go_exit;freq++) {
   
     /* set rf_freq */
@@ -203,18 +215,6 @@ int main(int argc, char **argv) {
     }
       
     bzero(found_cells, 3*sizeof(srslte_ue_cellsearch_result_t));
-      
-    if (srslte_ue_cellsearch_init_multi(&cs, cell_detect_config.max_frames_pss, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
-      fprintf(stderr, "Error initiating UE cell detect\n");
-      exit(-1);
-    }
-    
-    if (cell_detect_config.max_frames_pss) {
-      srslte_ue_cellsearch_set_nof_valid_frames(&cs, cell_detect_config.nof_valid_pss_frames);
-    }
-    if (cell_detect_config.init_agc) {
-      srslte_ue_sync_start_agc(&cs.ue_sync, srslte_rf_set_rx_gain_wrapper, cell_detect_config.init_agc);    
-    }
 
     INFO("Setting sampling frequency %.2f MHz for PSS search\n", SRSLTE_CS_SAMP_FREQ/1000000);
     srslte_rf_set_rx_srate(&rf, SRSLTE_CS_SAMP_FREQ);
@@ -222,7 +222,6 @@ int main(int argc, char **argv) {
     srslte_rf_start_rx_stream(&rf);
     
     n = srslte_ue_cellsearch_scan(&cs, found_cells, NULL); 
-    srslte_ue_cellsearch_free(&cs);
     if (n < 0) {
       fprintf(stderr, "Error searching cell\n");
       exit(-1);
@@ -254,7 +253,7 @@ int main(int argc, char **argv) {
       }
     }    
   }
-  
+
   printf("\n\nFound %d cells\n", n_found_cells);
   for (int i=0;i<n_found_cells;i++) {
     printf("Found CELL %.1f MHz, EARFCN=%d, PHYID=%d, %d PRB, %d ports, PSS power=%.1f dBm\n", 
@@ -268,7 +267,8 @@ int main(int argc, char **argv) {
   }
   
   printf("\nBye\n");
-    
+
+  srslte_ue_cellsearch_free(&cs);
   srslte_rf_close(&rf);
   exit(0);
 }
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 800e04de9..2067519a6 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -227,7 +227,11 @@ void base_init() {
     exit(-1);
   }
   srslte_ofdm_set_normalize(&ifft, true);
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
@@ -237,7 +241,11 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+  if (srslte_pcfich_init(&pcfich)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
@@ -247,16 +255,24 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
 
-  if (srslte_pdsch_init(&pdsch, cell)) {
+  if (srslte_pdsch_init_enb(&pdsch, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  
+  if (srslte_pdsch_set_cell(&pdsch, cell)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
   
   if (srslte_softbuffer_tx_init(&softbuffer, cell.nof_prb)) {
@@ -492,7 +508,11 @@ int main(int argc, char **argv) {
   srslte_sss_generate(sss_signal0, sss_signal5, cell.id);
   
   /* Generate CRS signals */
-  if (srslte_chest_dl_init(&est, cell)) {
+  if (srslte_chest_dl_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    exit(-1);
+  }
+  if (srslte_chest_dl_set_cell(&est, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     exit(-1);
   }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 72dc36731..3b58124cb 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -455,23 +455,42 @@ int main(int argc, char **argv) {
              //ue_sync.decimate = prog_args.decimate;
           }
       }
-    if (srslte_ue_sync_init_multi_decim(&ue_sync, cell, srslte_rf_recv_wrapper, prog_args.rf_nof_rx_ant, (void*) &rf,decimate)) {
+    if (srslte_ue_sync_init_multi_decim(&ue_sync,
+                                        cell.nof_prb,
+                                        cell.id==1000,
+                                        srslte_rf_recv_wrapper,
+                                        prog_args.rf_nof_rx_ant,
+                                        (void*) &rf,decimate))
+    {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
     }
+    if (srslte_ue_sync_set_cell(&ue_sync, cell))
+    {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      exit(-1);
+    }
 #endif
   }
 
-  if (srslte_ue_mib_init(&ue_mib, cell)) {
+  if (srslte_ue_mib_init(&ue_mib, cell.nof_prb)) {
+    fprintf(stderr, "Error initaiting UE MIB decoder\n");
+    exit(-1);
+  }
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
     fprintf(stderr, "Error initaiting UE MIB decoder\n");
     exit(-1);
-  }    
+  }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, prog_args.rf_nof_rx_ant)) {  // This is the User RNTI
+  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, prog_args.rf_nof_rx_ant)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     exit(-1);
   }
-  
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initiating UE downlink processing module\n");
+    exit(-1);
+  }
+
   for (int i=0;i<prog_args.rf_nof_rx_ant;i++) {
     sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
   }
diff --git a/lib/examples/usrp_capture_sync.c b/lib/examples/usrp_capture_sync.c
index 6d2ec18dc..bf7595c01 100644
--- a/lib/examples/usrp_capture_sync.c
+++ b/lib/examples/usrp_capture_sync.c
@@ -158,11 +158,15 @@ int main(int argc, char **argv) {
   cell.nof_prb = nof_prb; 
   cell.nof_ports = 1; 
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     exit(-1); 
   }
- 
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    fprintf(stderr, "Error initiating ue_sync\n");
+    exit(-1);
+  }
+
   uint32_t subframe_count = 0;
   bool start_capture = false; 
   bool stop_capture = false; 
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index cc84c992f..053479193 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -102,7 +102,6 @@ public:
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
   virtual void      plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) = 0;
-  virtual void      cell_selected() = 0;
 };
 
 // NAS interface for UE
@@ -140,19 +139,9 @@ public:
   virtual void enable_capabilities() = 0;
   virtual void plmn_search() = 0;
   virtual void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) = 0;
-  virtual void connect() = 0;
 
 };
 
-// RRC interface for GW
-class rrc_interface_gw
-{
-public:
-  virtual bool is_connected() = 0;
-  virtual void connect() = 0;
-  virtual bool have_drb() = 0;
-};
-
 // RRC interface for PDCP
 class rrc_interface_pdcp
 {
@@ -392,8 +381,8 @@ typedef struct {
   int worker_cpu_mask;
   int sync_cpu_affinity;
   
-  uint32_t nof_rx_ant;   
-  std::string equalizer_mode; 
+  uint32_t nof_rx_ant;
+  std::string equalizer_mode;
   int cqi_max; 
   int cqi_fixed; 
   float snr_ema_coeff; 
@@ -481,6 +470,7 @@ public:
   virtual void cell_search_start() = 0;
   virtual void cell_search_next() = 0;
   virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
+  virtual bool sync_stop() = 0;
 
   /* Is the PHY downlink synchronized? */
   virtual bool sync_status() = 0;
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index a454f939c..cecb099ce 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -92,9 +92,12 @@ typedef struct {
 
 
 SRSLTE_API int srslte_chest_dl_init(srslte_chest_dl_t *q, 
-                                    srslte_cell_t cell);
+                                    uint32_t max_prb);
 
-SRSLTE_API void srslte_chest_dl_free(srslte_chest_dl_t *q); 
+SRSLTE_API void srslte_chest_dl_free(srslte_chest_dl_t *q);
+
+SRSLTE_API int srslte_chest_dl_set_cell(srslte_chest_dl_t *q,
+                                        srslte_cell_t cell);
 
 SRSLTE_API void srslte_chest_dl_set_smooth_filter(srslte_chest_dl_t *q, 
                                                   float *filter, 
diff --git a/lib/include/srslte/phy/ch_estimation/chest_ul.h b/lib/include/srslte/phy/ch_estimation/chest_ul.h
index 1e505210c..0571fec8c 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_ul.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_ul.h
@@ -77,9 +77,13 @@ typedef struct {
 
 
 SRSLTE_API int srslte_chest_ul_init(srslte_chest_ul_t *q, 
-                                    srslte_cell_t cell);
+                                    uint32_t max_prb);
+
+SRSLTE_API void srslte_chest_ul_free(srslte_chest_ul_t *q);
+
+SRSLTE_API int srslte_chest_ul_set_cell(srslte_chest_ul_t *q,
+                                        srslte_cell_t cell);
 
-SRSLTE_API void srslte_chest_ul_free(srslte_chest_ul_t *q); 
 
 SRSLTE_API void srslte_chest_ul_set_cfg(srslte_chest_ul_t *q, 
                                         srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
diff --git a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
index 3cdf7901e..6b21c4fa2 100644
--- a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
@@ -53,7 +53,10 @@ typedef struct SRSLTE_API {
 
 
 SRSLTE_API int srslte_refsignal_cs_init(srslte_refsignal_cs_t *q, 
-                                        srslte_cell_t cell);
+                                        uint32_t max_prb);
+
+SRSLTE_API int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q,
+                                           srslte_cell_t cell);
 
 SRSLTE_API void srslte_refsignal_cs_free(srslte_refsignal_cs_t *q);
 
diff --git a/lib/include/srslte/phy/ch_estimation/refsignal_ul.h b/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
index 72bfb5d94..9e1da9e50 100644
--- a/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
+++ b/lib/include/srslte/phy/ch_estimation/refsignal_ul.h
@@ -94,7 +94,10 @@ typedef struct {
 } srslte_refsignal_srs_pregen_t;
 
 SRSLTE_API int srslte_refsignal_ul_init(srslte_refsignal_ul_t *q, 
-                                        srslte_cell_t cell);
+                                        uint32_t max_prb);
+
+SRSLTE_API int srslte_refsignal_ul_set_cell(srslte_refsignal_ul_t *q,
+                                            srslte_cell_t cell);
 
 SRSLTE_API void srslte_refsignal_ul_free(srslte_refsignal_ul_t *q);
 
@@ -115,9 +118,13 @@ SRSLTE_API uint32_t srslte_refsignal_dmrs_pucch_symbol(uint32_t m,
 
 SRSLTE_API bool srslte_refsignal_dmrs_pusch_cfg_isvalid(srslte_refsignal_ul_t *q, 
                                                         srslte_refsignal_dmrs_pusch_cfg_t *cfg, 
-                                                        uint32_t nof_prb); 
+                                                        uint32_t nof_prb);
+
+SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen_init(srslte_refsignal_ul_t *q,
+                                                       srslte_refsignal_ul_dmrs_pregen_t *pregen,
+                                                       uint32_t max_prb);
 
-SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q, 
+SRSLTE_API int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q,
                                                   srslte_refsignal_ul_dmrs_pregen_t *pregen);
 
 SRSLTE_API void srslte_refsignal_dmrs_pusch_pregen_free(srslte_refsignal_ul_t *q, 
diff --git a/lib/include/srslte/phy/common/sequence.h b/lib/include/srslte/phy/common/sequence.h
index dbeba5ff4..4c7c2c945 100644
--- a/lib/include/srslte/phy/common/sequence.h
+++ b/lib/include/srslte/phy/common/sequence.h
@@ -44,7 +44,8 @@ typedef struct SRSLTE_API {
   uint8_t *c_bytes;
   float *c_float;
   short *c_short;
-  uint32_t len;
+  uint32_t cur_len;
+  uint32_t max_len;
 } srslte_sequence_t;
 
 SRSLTE_API int srslte_sequence_init(srslte_sequence_t *q, uint32_t len);
@@ -55,8 +56,9 @@ SRSLTE_API int srslte_sequence_LTE_pr(srslte_sequence_t *q,
                                       uint32_t len, 
                                       uint32_t seed);
 
-SRSLTE_API void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, 
-                                           uint32_t seed); 
+SRSLTE_API int srslte_sequence_set_LTE_pr(srslte_sequence_t *q,
+                                           uint32_t len,
+                                           uint32_t seed);
 
 SRSLTE_API int srslte_sequence_pbch(srslte_sequence_t *seq, 
                                     srslte_cp_t cp, 
diff --git a/lib/include/srslte/phy/dft/dft.h b/lib/include/srslte/phy/dft/dft.h
index 12c06dd98..03a04fcd5 100644
--- a/lib/include/srslte/phy/dft/dft.h
+++ b/lib/include/srslte/phy/dft/dft.h
@@ -58,6 +58,7 @@ typedef enum {
 }srslte_dft_dir_t;
 
 typedef struct SRSLTE_API {
+  int init_size;      // DFT length used in the first initialization
   int size;           // DFT length
   void *in;           // Input buffer
   void *out;          // Output buffer
@@ -78,7 +79,7 @@ SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan,
                                srslte_dft_dir_t dir,                         
                                srslte_dft_mode_t type);
 
-SRSLTE_API int srslte_dft_plan_c(srslte_dft_plan_t *plan, 
+SRSLTE_API int srslte_dft_plan_c(srslte_dft_plan_t *plan,
                                  int dft_points, 
                                  srslte_dft_dir_t dir);
 
@@ -86,6 +87,16 @@ SRSLTE_API int srslte_dft_plan_r(srslte_dft_plan_t *plan,
                                  int dft_points, 
                                  srslte_dft_dir_t dir);
 
+SRSLTE_API int srslte_dft_replan(srslte_dft_plan_t *plan,
+                                 const int new_dft_points);
+
+SRSLTE_API int srslte_dft_replan_c(srslte_dft_plan_t *plan,
+                                   int new_dft_points);
+
+SRSLTE_API int srslte_dft_replan_r(srslte_dft_plan_t *plan,
+                                   int new_dft_points);
+
+
 SRSLTE_API void srslte_dft_plan_free(srslte_dft_plan_t *plan);
 
 /* Set options */
diff --git a/lib/include/srslte/phy/dft/dft_precoding.h b/lib/include/srslte/phy/dft/dft_precoding.h
index 6e5c5dc40..022593aa7 100644
--- a/lib/include/srslte/phy/dft/dft_precoding.h
+++ b/lib/include/srslte/phy/dft/dft_precoding.h
@@ -45,12 +45,18 @@ typedef struct SRSLTE_API {
 
   uint32_t max_prb;  
   srslte_dft_plan_t dft_plan[SRSLTE_MAX_PRB+1];
-  srslte_dft_plan_t idft_plan[SRSLTE_MAX_PRB+1];
-    
+
 }srslte_dft_precoding_t;
 
-SRSLTE_API int srslte_dft_precoding_init(srslte_dft_precoding_t *q, 
-                                         uint32_t max_prb);
+SRSLTE_API int srslte_dft_precoding_init(srslte_dft_precoding_t *q,
+                                         uint32_t max_prb,
+                                         bool is_tx);
+
+SRSLTE_API int srslte_dft_precoding_init_tx(srslte_dft_precoding_t *q,
+                                            uint32_t max_prb);
+
+SRSLTE_API int srslte_dft_precoding_init_rx(srslte_dft_precoding_t *q,
+                                            uint32_t max_prb);
 
 SRSLTE_API void srslte_dft_precoding_free(srslte_dft_precoding_t *q);
 
@@ -62,10 +68,4 @@ SRSLTE_API int srslte_dft_precoding(srslte_dft_precoding_t *q,
                                     uint32_t nof_prb, 
                                     uint32_t nof_symbols);
 
-SRSLTE_API int srslte_dft_predecoding(srslte_dft_precoding_t *q, 
-                                      cf_t *input, 
-                                      cf_t *output, 
-                                      uint32_t nof_prb, 
-                                      uint32_t nof_symbols);
-
 #endif
diff --git a/lib/include/srslte/phy/dft/ofdm.h b/lib/include/srslte/phy/dft/ofdm.h
index 990d7a8cc..175fe1ea9 100644
--- a/lib/include/srslte/phy/dft/ofdm.h
+++ b/lib/include/srslte/phy/dft/ofdm.h
@@ -47,6 +47,7 @@
 /* This is common for both directions */
 typedef struct SRSLTE_API{
   srslte_dft_plan_t fft_plan;
+  uint32_t max_prb;
   uint32_t nof_symbols;
   uint32_t symbol_sz;
   uint32_t nof_guards;
@@ -56,18 +57,27 @@ typedef struct SRSLTE_API{
   cf_t *tmp; // for removing zero padding
   
   bool freq_shift;
+  float freq_shift_f;
   cf_t *shift_buffer; 
 }srslte_ofdm_t;
 
 SRSLTE_API int srslte_ofdm_init_(srslte_ofdm_t *q, 
                                  srslte_cp_t cp, 
                                  int symbol_sz, 
-                                 int nof_prb, 
+                                 int max_prb,
                                  srslte_dft_dir_t dir); 
 
 SRSLTE_API int srslte_ofdm_rx_init(srslte_ofdm_t *q, 
                                srslte_cp_t cp_type, 
-                               uint32_t nof_prb);
+                               uint32_t max_prb);
+
+SRSLTE_API int srslte_ofdm_tx_set_prb(srslte_ofdm_t *q,
+                                      srslte_cp_t cp,
+                                      uint32_t nof_prb);
+
+SRSLTE_API int srslte_ofdm_rx_set_prb(srslte_ofdm_t *q,
+                                      srslte_cp_t cp,
+                                      uint32_t nof_prb);
 
 SRSLTE_API void srslte_ofdm_rx_free(srslte_ofdm_t *q);
 
diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index 08cb5da4f..e6b9ae9ba 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -108,11 +108,14 @@ typedef struct {
 
 /* This function shall be called just after the initial synchronization */
 SRSLTE_API int srslte_enb_dl_init(srslte_enb_dl_t *q, 
-                                  srslte_cell_t cell);
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_enb_dl_free(srslte_enb_dl_t *q);
 
-SRSLTE_API void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q, 
+SRSLTE_API int srslte_enb_dl_set_cell(srslte_enb_dl_t *q,
+                                      srslte_cell_t cell);
+
+SRSLTE_API void srslte_enb_dl_set_cfi(srslte_enb_dl_t *q,
                                       uint32_t cfi);
 
 SRSLTE_API void srslte_enb_dl_set_amp(srslte_enb_dl_t *q, 
diff --git a/lib/include/srslte/phy/enb/enb_ul.h b/lib/include/srslte/phy/enb/enb_ul.h
index 486566a1c..855b645ca 100644
--- a/lib/include/srslte/phy/enb/enb_ul.h
+++ b/lib/include/srslte/phy/enb/enb_ul.h
@@ -100,15 +100,18 @@ typedef struct {
 } srslte_enb_ul_pusch_t; 
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_enb_ul_init(srslte_enb_ul_t *q, 
-                                  srslte_cell_t cell, 
-                                  srslte_prach_cfg_t* prach_cfg, 
-                                  srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
-                                  srslte_pusch_hopping_cfg_t *hopping_cfg,
-                                  srslte_pucch_cfg_t *pucch_cfg);
+SRSLTE_API int srslte_enb_ul_init(srslte_enb_ul_t *q,
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_enb_ul_free(srslte_enb_ul_t *q);
 
+SRSLTE_API int srslte_enb_ul_set_cell(srslte_enb_ul_t *q,
+                                      srslte_cell_t cell,
+                                      srslte_prach_cfg_t* prach_cfg,
+                                      srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
+                                      srslte_pusch_hopping_cfg_t *hopping_cfg,
+                                      srslte_pucch_cfg_t *pucch_cfg);
+
 SRSLTE_API int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, 
                                       uint16_t rnti); 
 
diff --git a/lib/include/srslte/phy/phch/pbch.h b/lib/include/srslte/phy/phch/pbch.h
index 8489df05a..4dc7ec727 100644
--- a/lib/include/srslte/phy/phch/pbch.h
+++ b/lib/include/srslte/phy/phch/pbch.h
@@ -53,6 +53,8 @@
 #define SRSLTE_BCH_PAYLOADCRC_LEN  (SRSLTE_BCH_PAYLOAD_LEN+16)
 #define SRSLTE_BCH_ENCODED_LEN     3*(SRSLTE_BCH_PAYLOADCRC_LEN)
 
+#define SRSLTE_PBCH_MAX_RE 256 // make it avx2-aligned
+
 /* PBCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
@@ -83,12 +85,14 @@ typedef struct SRSLTE_API {
   
 } srslte_pbch_t;
 
-SRSLTE_API int srslte_pbch_init(srslte_pbch_t *q,
-                         srslte_cell_t cell);
+SRSLTE_API int srslte_pbch_init(srslte_pbch_t *q);
 
 SRSLTE_API void srslte_pbch_free(srslte_pbch_t *q);
 
-SRSLTE_API int srslte_pbch_decode(srslte_pbch_t *q, 
+SRSLTE_API int srslte_pbch_set_cell(srslte_pbch_t *q,
+                                    srslte_cell_t cell);
+
+SRSLTE_API int srslte_pbch_decode(srslte_pbch_t *q,
                            cf_t *slot1_symbols, 
                            cf_t *ce_slot1[SRSLTE_MAX_PORTS], 
                            float noise_estimate, 
diff --git a/lib/include/srslte/phy/phch/pcfich.h b/lib/include/srslte/phy/phch/pcfich.h
index 672caed2c..7c94d37b2 100644
--- a/lib/include/srslte/phy/phch/pcfich.h
+++ b/lib/include/srslte/phy/phch/pcfich.h
@@ -78,15 +78,15 @@ typedef struct SRSLTE_API {
   
 } srslte_pcfich_t;
 
-SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q, 
-                           srslte_regs_t *regs, 
-                           srslte_cell_t cell);
+SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q);
 
 SRSLTE_API int srslte_pcfich_init_multi(srslte_pcfich_t *q, 
-                                        srslte_regs_t *regs, 
-                                        srslte_cell_t cell, 
                                         uint32_t nof_rx_antennas);
 
+SRSLTE_API int srslte_pcfich_set_cell(srslte_pcfich_t *q,
+                                      srslte_regs_t *regs,
+                                      srslte_cell_t cell);
+
 SRSLTE_API void srslte_pcfich_free(srslte_pcfich_t *q);
 
 SRSLTE_API int srslte_pcfich_decode(srslte_pcfich_t *q, 
diff --git a/lib/include/srslte/phy/phch/pdcch.h b/lib/include/srslte/phy/phch/pdcch.h
index 8d4aba790..b179c21e7 100644
--- a/lib/include/srslte/phy/phch/pdcch.h
+++ b/lib/include/srslte/phy/phch/pdcch.h
@@ -84,15 +84,17 @@ typedef struct SRSLTE_API {
   
 } srslte_pdcch_t;
 
-SRSLTE_API int srslte_pdcch_init(srslte_pdcch_t *q, 
-                                 srslte_regs_t *regs, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pdcch_init(srslte_pdcch_t *q,
+                                 uint32_t max_prb);
 
-SRSLTE_API int srslte_pdcch_init_multi(srslte_pdcch_t *q, 
-                                       srslte_regs_t *regs, 
-                                       srslte_cell_t cell, 
+SRSLTE_API int srslte_pdcch_init_multi(srslte_pdcch_t *q,
+                                       uint32_t max_prb,
                                        uint32_t nof_rx_antennas);
 
+SRSLTE_API int srslte_pdcch_set_cell(srslte_pdcch_t *q,
+                                     srslte_regs_t *regs,
+                                     srslte_cell_t cell);
+
 SRSLTE_API void srslte_pdcch_free(srslte_pdcch_t *q);
 
 
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index ad01c4ef8..b92c197a1 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -59,7 +59,9 @@ typedef struct SRSLTE_API {
   uint32_t nof_rx_antennas;
   
   uint32_t max_re;
-  
+
+  bool is_ue;
+
   /* buffers */
   // void buffers are shared for tx and rx
   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
@@ -73,21 +75,33 @@ typedef struct SRSLTE_API {
   
   // This is to generate the scrambling seq for multiple CRNTIs
   srslte_pdsch_user_t **users;
-  
+
+  srslte_sequence_t tmp_seq;
+
   srslte_sch_t dl_sch;
   
 } srslte_pdsch_t;
 
-SRSLTE_API int srslte_pdsch_init(srslte_pdsch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pdsch_init_ue(srslte_pdsch_t *q,
+                                    uint32_t max_prb);
+
+SRSLTE_API int srslte_pdsch_init_enb(srslte_pdsch_t *q,
+                                     uint32_t max_prb);
 
-SRSLTE_API int srslte_pdsch_init_multi(srslte_pdsch_t *q, 
-                                       srslte_cell_t cell, 
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_pdsch_init_multi_ue(srslte_pdsch_t *q,
+                                          uint32_t max_prb,
+                                          uint32_t nof_rx_antennas);
+
+SRSLTE_API int srslte_pdsch_init_multi_enb(srslte_pdsch_t *q,
+                                           uint32_t max_prb,
+                                           uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_pdsch_free(srslte_pdsch_t *q);
 
-SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q, 
+SRSLTE_API int srslte_pdsch_set_cell(srslte_pdsch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API int srslte_pdsch_set_rnti(srslte_pdsch_t *q,
                                      uint16_t rnti);
 
 SRSLTE_API void srslte_pdsch_free_rnti(srslte_pdsch_t *q, 
diff --git a/lib/include/srslte/phy/phch/phich.h b/lib/include/srslte/phy/phch/phich.h
index 18454a2d5..8c519efd6 100644
--- a/lib/include/srslte/phy/phch/phich.h
+++ b/lib/include/srslte/phy/phch/phich.h
@@ -85,18 +85,18 @@ typedef struct SRSLTE_API {
   
 } srslte_phich_t;
 
-SRSLTE_API int srslte_phich_init(srslte_phich_t *q, 
-                                 srslte_regs_t *regs, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_phich_init(srslte_phich_t *q);
 
 SRSLTE_API int srslte_phich_init_multi(srslte_phich_t *q, 
-                                       srslte_regs_t *regs, 
-                                       srslte_cell_t cell, 
                                        uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_phich_free(srslte_phich_t *q);
 
-SRSLTE_API void srslte_phich_calc(srslte_phich_t *q, 
+SRSLTE_API int srslte_phich_set_cell(srslte_phich_t *q,
+                                     srslte_regs_t *regs,
+                                     srslte_cell_t cell);
+
+SRSLTE_API void srslte_phich_calc(srslte_phich_t *q,
                                   uint32_t n_prb_lowest, 
                                   uint32_t n_dmrs, 
                                   uint32_t *ngroup, 
diff --git a/lib/include/srslte/phy/phch/prach.h b/lib/include/srslte/phy/phch/prach.h
index a08256a57..3e3df01bb 100644
--- a/lib/include/srslte/phy/phch/prach.h
+++ b/lib/include/srslte/phy/phch/prach.h
@@ -44,6 +44,8 @@
 #include "srslte/phy/common/phy_common.h"
 
 
+#define SRSLTE_PRACH_MAX_LEN  (2*24576+21024) // Maximum Tcp + Tseq
+
 /** Generation and detection of RACH signals for uplink.
  *  Currently only supports preamble formats 0-3.
  *  Does not currently support high speed flag.
@@ -60,6 +62,8 @@ typedef struct SRSLTE_API {
   uint32_t N_ifft_ul;       // IFFT size for uplink
   uint32_t N_ifft_prach;    // IFFT size for PRACH generation
 
+  uint32_t max_N_ifft_ul;
+
   // Working parameters
   uint32_t N_zc;  // PRACH sequence length
   uint32_t N_cs;  // Cyclic shift size
@@ -82,12 +86,12 @@ typedef struct SRSLTE_API {
   float *corr;
 
   // PRACH IFFT
-  srslte_dft_plan_t *fft;
-  srslte_dft_plan_t *ifft;
+  srslte_dft_plan_t fft;
+  srslte_dft_plan_t ifft;
 
   // ZC-sequence FFT and IFFT
-  srslte_dft_plan_t *zc_fft;
-  srslte_dft_plan_t *zc_ifft;
+  srslte_dft_plan_t zc_fft;
+  srslte_dft_plan_t zc_ifft;
   
   cf_t *signal_fft; 
   float detect_factor; 
@@ -129,11 +133,14 @@ SRSLTE_API void srslte_prach_sf_config(uint32_t config_idx,
                                        srslte_prach_sf_config_t *sf_config);
 
 SRSLTE_API int srslte_prach_init(srslte_prach_t *p,
-                                 uint32_t N_ifft_ul,
-                                 uint32_t config_idx,
-                                 uint32_t root_seq_index,
-                                 bool high_speed_flag,
-                                 uint32_t zero_corr_zone_config);
+                                 uint32_t max_N_ifft_ul);
+
+SRSLTE_API int srslte_prach_set_cell(srslte_prach_t *p,
+                                     uint32_t N_ifft_ul,
+                                     uint32_t config_idx,
+                                     uint32_t root_seq_index,
+                                     bool high_speed_flag,
+                                     uint32_t zero_corr_zone_config);
 
 SRSLTE_API int srslte_prach_init_cfg(srslte_prach_t* p, 
                                      srslte_prach_cfg_t* cfg, 
diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index 56d512418..a1477cccf 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -45,7 +45,7 @@
 #define SRSLTE_PUCCH_N_SEQ       12 
 #define SRSLTE_PUCCH2_NOF_BITS   SRSLTE_UCI_CQI_CODED_PUCCH_B
 #define SRSLTE_PUCCH_MAX_BITS    SRSLTE_CQI_MAX_BITS
-#define SRSLTE_PUCCH_MAX_SYMBOLS 120
+#define SRSLTE_PUCCH_MAX_SYMBOLS 128
 
 typedef enum SRSLTE_API {
   SRSLTE_PUCCH_FORMAT_1 = 0, 
@@ -100,9 +100,9 @@ typedef struct SRSLTE_API {
   uint32_t f_gh[SRSLTE_NSLOTS_X_FRAME];
   float tmp_arg[SRSLTE_PUCCH_N_SEQ];
   
-  cf_t *z;
-  cf_t *z_tmp;
-  cf_t *ce;
+  cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS];
+  cf_t z_tmp[SRSLTE_PUCCH_MAX_SYMBOLS];
+  cf_t ce[SRSLTE_PUCCH_MAX_SYMBOLS];
   
   bool shortened; 
   bool group_hopping_en;
@@ -116,12 +116,14 @@ typedef struct SRSLTE_API {
 }srslte_pucch_t;
 
 
-SRSLTE_API int srslte_pucch_init(srslte_pucch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pucch_init(srslte_pucch_t *q);
 
 SRSLTE_API void srslte_pucch_free(srslte_pucch_t *q);
 
-SRSLTE_API bool srslte_pucch_set_cfg(srslte_pucch_t* q, 
+SRSLTE_API int srslte_pucch_set_cell(srslte_pucch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API bool srslte_pucch_set_cfg(srslte_pucch_t* q,
                                      srslte_pucch_cfg_t* cfg, 
                                      bool group_hopping_en); 
 
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index e5ee43995..4ef54aa6e 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -62,13 +62,14 @@ typedef struct {
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
-  bool sequences_generated;
+  bool sequence_generated;
 } srslte_pusch_user_t; 
 
 /* PUSCH object */
 typedef struct SRSLTE_API {
   srslte_cell_t cell;
 
+  bool is_ue;
   uint32_t max_re;
 
   srslte_dft_precoding_t dft_precoding;  
@@ -96,12 +97,18 @@ typedef struct SRSLTE_API {
 }srslte_pusch_t;
 
 
-SRSLTE_API int srslte_pusch_init(srslte_pusch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pusch_init_ue(srslte_pusch_t *q,
+                                    uint32_t max_prb);
+
+SRSLTE_API int srslte_pusch_init_enb(srslte_pusch_t *q,
+                                    uint32_t max_prb);
 
 SRSLTE_API void srslte_pusch_free(srslte_pusch_t *q);
 
-SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q, 
+SRSLTE_API int srslte_pusch_set_cell(srslte_pusch_t *q,
+                                     srslte_cell_t cell);
+
+SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q,
                                 srslte_pusch_cfg_t         *cfg, 
                                 srslte_ra_ul_grant_t       *grant, 
                                 srslte_uci_cfg_t           *uci_cfg, 
diff --git a/lib/include/srslte/phy/resampling/interp.h b/lib/include/srslte/phy/resampling/interp.h
index 0bc2a6e39..508b6e957 100644
--- a/lib/include/srslte/phy/resampling/interp.h
+++ b/lib/include/srslte/phy/resampling/interp.h
@@ -67,7 +67,8 @@ SRSLTE_API void srslte_interp_linear_f(float *input,
 
 typedef struct {
   cf_t *diff_vec; 
-  uint32_t vector_len; 
+  uint32_t vector_len;
+  uint32_t max_vector_len;
 } srslte_interp_linsrslte_vec_t;
 
 SRSLTE_API int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q, 
@@ -75,6 +76,9 @@ SRSLTE_API int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q
 
 SRSLTE_API void srslte_interp_linear_vector_free(srslte_interp_linsrslte_vec_t *q); 
 
+SRSLTE_API int srslte_interp_linear_vector_resize(srslte_interp_linsrslte_vec_t *q,
+                                                  uint32_t vector_len);
+
 SRSLTE_API void srslte_interp_linear_vector(srslte_interp_linsrslte_vec_t *q, 
                                             cf_t *in0, 
                                             cf_t *in1, 
@@ -107,7 +111,9 @@ typedef struct {
   cf_t *diff_vec2;
   float *ramp;
   uint32_t vector_len; 
-  uint32_t M; 
+  uint32_t M;
+  uint32_t max_vector_len;
+  uint32_t max_M;
 } srslte_interp_lin_t;
 
 SRSLTE_API int srslte_interp_linear_init(srslte_interp_lin_t *q, 
@@ -116,6 +122,10 @@ SRSLTE_API int srslte_interp_linear_init(srslte_interp_lin_t *q,
 
 SRSLTE_API void srslte_interp_linear_free(srslte_interp_lin_t *q);
 
+SRSLTE_API int srslte_interp_linear_resize(srslte_interp_lin_t *q,
+                                           uint32_t vector_len,
+                                           uint32_t M);
+
 SRSLTE_API void srslte_interp_linear_offset(srslte_interp_lin_t *q, 
                                             cf_t *input, 
                                             cf_t *output, 
diff --git a/lib/include/srslte/phy/sync/cfo.h b/lib/include/srslte/phy/sync/cfo.h
index 642e81fc4..fd2ab4d83 100644
--- a/lib/include/srslte/phy/sync/cfo.h
+++ b/lib/include/srslte/phy/sync/cfo.h
@@ -49,6 +49,7 @@ typedef struct SRSLTE_API {
   float last_freq;
   float tol;
   int nsamples;
+  int max_samples;
   srslte_cexptab_t tab;
   cf_t *cur_cexp;
 }srslte_cfo_t;
@@ -58,8 +59,8 @@ SRSLTE_API int srslte_cfo_init(srslte_cfo_t *h,
 
 SRSLTE_API void srslte_cfo_free(srslte_cfo_t *h);
 
-SRSLTE_API int srslte_cfo_realloc(srslte_cfo_t *h, 
-                                  uint32_t samples);
+SRSLTE_API int srslte_cfo_resize(srslte_cfo_t *h,
+                                 uint32_t samples);
 
 SRSLTE_API void srslte_cfo_set_tol(srslte_cfo_t *h, 
                                    float tol);
diff --git a/lib/include/srslte/phy/sync/cp.h b/lib/include/srslte/phy/sync/cp.h
index 211479030..9daf7d4f6 100644
--- a/lib/include/srslte/phy/sync/cp.h
+++ b/lib/include/srslte/phy/sync/cp.h
@@ -35,6 +35,7 @@
 typedef struct {
   cf_t *corr;
   uint32_t symbol_sz;
+  uint32_t max_symbol_sz;
 } srslte_cp_synch_t;
 
 SRSLTE_API int srslte_cp_synch_init(srslte_cp_synch_t *q, 
@@ -42,6 +43,9 @@ SRSLTE_API int srslte_cp_synch_init(srslte_cp_synch_t *q,
 
 SRSLTE_API void srslte_cp_synch_free(srslte_cp_synch_t *q);
 
+SRSLTE_API int srslte_cp_synch_resize(srslte_cp_synch_t *q,
+                                      uint32_t symbol_sz);
+
 SRSLTE_API uint32_t srslte_cp_synch(srslte_cp_synch_t *q, 
                                     cf_t *input, 
                                     uint32_t max_offset, 
diff --git a/lib/include/srslte/phy/sync/pss.h b/lib/include/srslte/phy/sync/pss.h
index f18c5b6af..9760a178c 100644
--- a/lib/include/srslte/phy/sync/pss.h
+++ b/lib/include/srslte/phy/sync/pss.h
@@ -79,6 +79,10 @@ typedef struct SRSLTE_API {
 
 #endif
   int decimate;
+
+  uint32_t max_frame_size;
+  uint32_t max_fft_size;
+
   uint32_t frame_size;
   uint32_t N_id_2;
   uint32_t fft_size;
@@ -113,6 +117,10 @@ SRSLTE_API int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
                                                 int cfo_i,
                                                 int decimate);
 
+SRSLTE_API int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size,
+                                       uint32_t fft_size,
+                                       int offset);
+
 SRSLTE_API int srslte_pss_synch_init(srslte_pss_synch_t *q, 
                                      uint32_t frame_size);
 
diff --git a/lib/include/srslte/phy/sync/sss.h b/lib/include/srslte/phy/sync/sss.h
index aca57e860..35eedac8f 100644
--- a/lib/include/srslte/phy/sync/sss.h
+++ b/lib/include/srslte/phy/sync/sss.h
@@ -70,6 +70,7 @@ typedef struct SRSLTE_API {
   srslte_dft_plan_t dftp_input;
   
   uint32_t fft_size;
+  uint32_t max_fft_size;
 
   float corr_peak_threshold;
   uint32_t symbol_sz;
@@ -89,7 +90,7 @@ typedef struct SRSLTE_API {
 SRSLTE_API int srslte_sss_synch_init(srslte_sss_synch_t *q,
                                      uint32_t fft_size);
 
-SRSLTE_API int srslte_sss_synch_realloc(srslte_sss_synch_t *q, 
+SRSLTE_API int srslte_sss_synch_resize(srslte_sss_synch_t *q,
                                         uint32_t fft_size); 
 
 SRSLTE_API void srslte_sss_synch_free(srslte_sss_synch_t *q);
diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index ab22d64fc..a11c43c59 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -97,6 +97,8 @@ typedef struct SRSLTE_API {
   float M_ext_avg; 
   cf_t  *temp;
 
+  uint32_t max_frame_size;
+
 }srslte_sync_t;
 
 typedef enum {
@@ -121,6 +123,11 @@ SRSLTE_API int srslte_sync_init_decim(srslte_sync_t *q,
 
 SRSLTE_API void srslte_sync_free(srslte_sync_t *q);
 
+SRSLTE_API int srslte_sync_resize(srslte_sync_t *q,
+                                  uint32_t frame_size,
+                                  uint32_t max_offset,
+                                  uint32_t fft_size);
+
 SRSLTE_API void srslte_sync_reset(srslte_sync_t *q); 
 
 /* Finds a correlation peak in the input signal around position find_offset */
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 7c3f83320..fb3ec585b 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -114,14 +114,17 @@ typedef struct SRSLTE_API {
 
 /* This function shall be called just after the initial synchronization */
 SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                                 srslte_cell_t cell);
+                                 uint32_t max_prb);
 
-SRSLTE_API int srslte_ue_dl_init_multi(srslte_ue_dl_t *q, 
-                                       srslte_cell_t cell, 
+SRSLTE_API int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
+                                       uint32_t max_prb,
                                        uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_ue_dl_free(srslte_ue_dl_t *q);
 
+SRSLTE_API int srslte_ue_dl_set_cell(srslte_ue_dl_t *q,
+                                          srslte_cell_t cell);
+
 SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, 
                                                 cf_t *input, 
                                                 uint32_t sf_idx, 
diff --git a/lib/include/srslte/phy/ue/ue_mib.h b/lib/include/srslte/phy/ue/ue_mib.h
index abefa014f..5d2ef14fa 100644
--- a/lib/include/srslte/phy/ue/ue_mib.h
+++ b/lib/include/srslte/phy/ue/ue_mib.h
@@ -79,10 +79,13 @@ typedef struct SRSLTE_API {
 } srslte_ue_mib_t;
 
 SRSLTE_API int srslte_ue_mib_init(srslte_ue_mib_t *q, 
-                                  srslte_cell_t cell);
+                                  uint32_t max_prb);
 
 SRSLTE_API void srslte_ue_mib_free(srslte_ue_mib_t *q);
 
+SRSLTE_API int srslte_ue_mib_set_cell(srslte_ue_mib_t * q,
+                                      srslte_cell_t cell);
+
 SRSLTE_API void srslte_ue_mib_reset(srslte_ue_mib_t * q); 
 
 SRSLTE_API int srslte_ue_mib_decode(srslte_ue_mib_t * q, 
@@ -104,21 +107,17 @@ typedef struct {
   uint32_t nof_rx_antennas;
 } srslte_ue_mib_sync_t;
 
-SRSLTE_API int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q, 
-                                       uint32_t cell_id, 
-                                       srslte_cp_t cp,
-                                       int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t *),   
-                                       void *stream_handler);
-
-SRSLTE_API int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
-                                             uint32_t cell_id, 
-                                             srslte_cp_t cp,
-                                             int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t *),   
+SRSLTE_API int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q,
+                                             int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t *),
                                              uint32_t nof_rx_antennas,
                                              void *stream_handler);
 
 SRSLTE_API void srslte_ue_mib_sync_free(srslte_ue_mib_sync_t *q);
 
+SRSLTE_API int srslte_ue_mib_sync_set_cell(srslte_ue_mib_sync_t *q,
+                                           uint32_t cell_id,
+                                           srslte_cp_t cp);
+
 SRSLTE_API void srslte_ue_mib_sync_reset(srslte_ue_mib_sync_t * q); 
 
 SRSLTE_API int srslte_ue_mib_sync_decode(srslte_ue_mib_sync_t * q, 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 94d4f9136..4bd8efb01 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -69,6 +69,8 @@ typedef enum SRSLTE_API { SF_FIND, SF_TRACK} srslte_ue_sync_state_t;
 typedef struct SRSLTE_API {
   srslte_sync_t sfind;
   srslte_sync_t strack;
+
+  uint32_t max_prb;
   
   srslte_agc_t agc; 
   bool do_agc; 
@@ -125,22 +127,25 @@ typedef struct SRSLTE_API {
 } srslte_ue_sync_t;
 
 SRSLTE_API int srslte_ue_sync_init(srslte_ue_sync_t *q, 
-                                   srslte_cell_t cell,
+                                   uint32_t max_prb,
+                                   bool search_cell,
                                    int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t*), 
                                    void *stream_handler);
 
-SRSLTE_API int srslte_ue_sync_init_multi(srslte_ue_sync_t *q, 
-                                         srslte_cell_t cell,
+SRSLTE_API int srslte_ue_sync_init_multi(srslte_ue_sync_t *q,
+                                         uint32_t max_prb,
+                                         bool search_cell,
                                          int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*), 
                                          uint32_t nof_rx_antennas,
                                          void *stream_handler);
 
-SRSLTE_API int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q, 
-                                         srslte_cell_t cell,
-                                         int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*), 
-                                         uint32_t nof_rx_antennas,
-                                         void *stream_handler,
-                                         int decimate);
+SRSLTE_API int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
+                                               uint32_t max_prb,
+                                               bool search_cell,
+                                               int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),
+                                               uint32_t nof_rx_antennas,
+                                               void *stream_handler,
+                                               int decimate);
 
 SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q, 
                                         uint32_t nof_prb,
@@ -150,6 +155,9 @@ SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q,
 
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
+SRSLTE_API int srslte_ue_sync_set_cell(srslte_ue_sync_t *q,
+                                       srslte_cell_t cell);
+
 SRSLTE_API int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, 
                                         double (set_gain_callback)(void*, double), 
                                         float init_gain_value); 
diff --git a/lib/include/srslte/phy/ue/ue_ul.h b/lib/include/srslte/phy/ue/ue_ul.h
index 2afa8068a..843948087 100644
--- a/lib/include/srslte/phy/ue/ue_ul.h
+++ b/lib/include/srslte/phy/ue/ue_ul.h
@@ -106,11 +106,14 @@ typedef struct SRSLTE_API {
 
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_ue_ul_init(srslte_ue_ul_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_ue_ul_init(srslte_ue_ul_t *q,
+                                 uint32_t max_prb);
 
 SRSLTE_API void srslte_ue_ul_free(srslte_ue_ul_t *q);
 
+SRSLTE_API int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
+                                     srslte_cell_t cell);
+
 SRSLTE_API void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, 
                                      float cur_cfo); 
 
diff --git a/lib/include/srslte/phy/utils/convolution.h b/lib/include/srslte/phy/utils/convolution.h
index 27ea7e590..fcb321ce5 100644
--- a/lib/include/srslte/phy/utils/convolution.h
+++ b/lib/include/srslte/phy/utils/convolution.h
@@ -46,6 +46,8 @@ typedef struct SRSLTE_API {
   uint32_t input_len;
   uint32_t filter_len;
   uint32_t output_len;
+  uint32_t max_input_len;
+  uint32_t max_filter_len;
   srslte_dft_plan_t input_plan;
   srslte_dft_plan_t filter_plan;
   srslte_dft_plan_t output_plan;
@@ -58,6 +60,9 @@ SRSLTE_API int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q,
                                        uint32_t input_len, 
                                        uint32_t filter_len);
 
+SRSLTE_API int srslte_conv_fft_cc_replan(srslte_conv_fft_cc_t *q,
+                                         uint32_t input_len,
+                                         uint32_t filter_len);
 
 SRSLTE_API void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q);
 
diff --git a/lib/include/srslte/upper/gw.h b/lib/include/srslte/upper/gw.h
index a01413829..fa6bc52d4 100644
--- a/lib/include/srslte/upper/gw.h
+++ b/lib/include/srslte/upper/gw.h
@@ -46,7 +46,7 @@ class gw
 {
 public:
   gw();
-  void init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_);
+  void init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_);
   void stop();
 
   void get_metrics(gw_metrics_t &m);
@@ -62,7 +62,6 @@ private:
   static const int GW_THREAD_PRIO = 7; 
   
   srsue::pdcp_interface_gw  *pdcp;
-  srsue::rrc_interface_gw   *rrc;
   srsue::ue_interface       *ue;
 
   byte_buffer_pool   *pool;
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 690721b3c..5adaa08c5 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -32,6 +32,8 @@
 #include <string.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/srslte.h>
 
 #include "srslte/config.h"
 
@@ -72,63 +74,56 @@ static void set_default_filter(srslte_chest_dl_t *q, int filter_len) {
  * 
  * This object depends on the srslte_refsignal_t object for creating the LTE CSR signal.  
 */
-
-int srslte_chest_dl_init(srslte_chest_dl_t *q, srslte_cell_t cell) 
+int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q                != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q                != NULL)
   {
     bzero(q, sizeof(srslte_chest_dl_t));
     
-    ret = srslte_refsignal_cs_init(&q->csr_signal, cell); 
+    ret = srslte_refsignal_cs_init(&q->csr_signal, max_prb);
     if (ret != SRSLTE_SUCCESS) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_estimates_average) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(cell.nof_prb));
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, SRSLTE_NRE*cell.nof_prb)) {
+    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, SRSLTE_NRE*max_prb)) {
       fprintf(stderr, "Error initializing vector interpolator\n");
       goto clean_exit; 
     }
 
-    if (srslte_interp_linear_init(&q->srslte_interp_lin, 2*cell.nof_prb, SRSLTE_NRE/2)) {
+    if (srslte_interp_linear_init(&q->srslte_interp_lin, 2*max_prb, SRSLTE_NRE/2)) {
       fprintf(stderr, "Error initializing interpolator\n");
       goto clean_exit; 
     }
     
-    if (srslte_pss_generate(q->pss_signal, cell.id%3)) {
-      fprintf(stderr, "Error initializing PSS signal for noise estimation\n");
-      goto clean_exit;
-    }
-    
-    q->noise_alg = SRSLTE_NOISE_ALG_REFS; 
+    q->noise_alg = SRSLTE_NOISE_ALG_REFS;
     
     q->smooth_filter_len = 3; 
     srslte_chest_dl_set_smooth_filter3_coeff(q, 0.1);
     
-    q->cell = cell; 
   }
   
   ret = SRSLTE_SUCCESS;
@@ -162,10 +157,45 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   bzero(q, sizeof(srslte_chest_dl_t));
 }
 
+int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q                != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      ret = srslte_refsignal_cs_set_cell(&q->csr_signal, cell);
+      if (ret != SRSLTE_SUCCESS) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pss_generate(q->pss_signal, cell.id%3)) {
+        fprintf(stderr, "Error initializing PSS signal for noise estimation\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, SRSLTE_NRE*q->cell.nof_prb)) {
+        fprintf(stderr, "Error initializing vector interpolator\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_interp_linear_resize(&q->srslte_interp_lin, 2*q->cell.nof_prb, SRSLTE_NRE/2)) {
+        fprintf(stderr, "Error initializing interpolator\n");
+        return SRSLTE_ERROR;
+      }
+
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+
+  return ret;
+}
+
 /* Uses the difference between the averaged and non-averaged pilot estimates */
 static float estimate_noise_pilots(srslte_chest_dl_t *q, uint32_t port_id) 
 {
   int nref=SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id);
+
   /* Substract noisy pilot estimates */
   srslte_vec_sub_ccc(q->pilot_estimates_average, q->pilot_estimates, q->tmp_noise, nref);  
   
@@ -318,7 +348,8 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
   
   /* Use the known CSR signal to compute Least-squares estimates */
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_signal.pilots[port_id/2][sf_idx], 
-              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
+              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id));
+
   if (ce != NULL) {
     
     /* Smooth estimates (if applicable) and interpolate */
@@ -370,7 +401,7 @@ int srslte_chest_dl_estimate_multi(srslte_chest_dl_t *q, cf_t *input[SRSLTE_MAX_
 int srslte_chest_dl_estimate(srslte_chest_dl_t *q, cf_t *input, cf_t *ce[SRSLTE_MAX_PORTS], uint32_t sf_idx) 
 {
   uint32_t port_id; 
-  
+
   for (port_id=0;port_id<q->cell.nof_ports;port_id++) {
     if (srslte_chest_dl_estimate_port(q, input, ce[port_id], sf_idx, port_id, 0)) {
       return SRSLTE_ERROR;
diff --git a/lib/src/phy/ch_estimation/chest_ul.c b/lib/src/phy/ch_estimation/chest_ul.c
index 9996fc9c3..f1d4f495a 100644
--- a/lib/src/phy/ch_estimation/chest_ul.c
+++ b/lib/src/phy/ch_estimation/chest_ul.c
@@ -32,6 +32,8 @@
 #include <string.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/config.h"
 
@@ -43,6 +45,8 @@
 #define NOF_REFS_SYM    (q->cell.nof_prb*SRSLTE_NRE)
 #define NOF_REFS_SF     (NOF_REFS_SYM*2) // 2 reference symbols per subframe
 
+#define MAX_REFS_SF     (max_prb*SRSLTE_NRE*2) // 2 reference symbols per subframe
+
 /** 3GPP LTE Downlink channel estimator and equalizer. 
  * Estimates the channel in the resource elements transmitting references and interpolates for the rest
  * of the resource grid. 
@@ -52,52 +56,49 @@
  * This object depends on the srslte_refsignal_t object for creating the LTE CSR signal.  
 */
 
-int srslte_chest_ul_init(srslte_chest_ul_t *q, srslte_cell_t cell) 
+int srslte_chest_ul_init(srslte_chest_ul_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q                != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q                != NULL)
   {
     bzero(q, sizeof(srslte_chest_ul_t));
 
-    q->cell = cell; 
-    
-    ret = srslte_refsignal_ul_init(&q->dmrs_signal, cell); 
+    ret = srslte_refsignal_ul_init(&q->dmrs_signal, max_prb);
     if (ret != SRSLTE_SUCCESS) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
     for (int i=0;i<4;i++) {
-      q->pilot_estimates_tmp[i] = srslte_vec_malloc(sizeof(cf_t) * NOF_REFS_SF);
+      q->pilot_estimates_tmp[i] = srslte_vec_malloc(sizeof(cf_t) * MAX_REFS_SF);
       if (!q->pilot_estimates_tmp[i]) {
         perror("malloc");
         goto clean_exit;
       }      
     }
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * (NOF_REFS_SF+1));
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * (MAX_REFS_SF+1));
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    q->pilot_known_signal = srslte_vec_malloc(sizeof(cf_t) * (NOF_REFS_SF+1));
+    q->pilot_known_signal = srslte_vec_malloc(sizeof(cf_t) * (MAX_REFS_SF+1));
     if (!q->pilot_known_signal) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, NOF_REFS_SYM)) {
+    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, MAX_REFS_SF)) {
       fprintf(stderr, "Error initializing vector interpolator\n");
       goto clean_exit; 
     }
@@ -105,12 +106,17 @@ int srslte_chest_ul_init(srslte_chest_ul_t *q, srslte_cell_t cell)
     q->smooth_filter_len = 3; 
     srslte_chest_ul_set_smooth_filter3_coeff(q, 0.3333);
   
-    q->dmrs_signal_configured = false; 
+    q->dmrs_signal_configured = false;
+
+    if (srslte_refsignal_dmrs_pusch_pregen_init(&q->dmrs_signal, &q->dmrs_pregen, max_prb)) {
+      fprintf(stderr, "Error allocating memory for pregenerated signals\n");
+      goto clean_exit;
+    }
   
   }
     
   ret = SRSLTE_SUCCESS;
-  
+
 clean_exit:
   if (ret != SRSLTE_SUCCESS) {
       srslte_chest_ul_free(q);
@@ -120,9 +126,8 @@ clean_exit:
 
 void srslte_chest_ul_free(srslte_chest_ul_t *q) 
 {
-  if (q->dmrs_signal_configured) {
-    srslte_refsignal_dmrs_pusch_pregen_free(&q->dmrs_signal, &q->dmrs_pregen);
-  }
+  srslte_refsignal_dmrs_pusch_pregen_free(&q->dmrs_signal, &q->dmrs_pregen);
+
   srslte_refsignal_ul_free(&q->dmrs_signal);
   if (q->tmp_noise) {
     free(q->tmp_noise);
@@ -146,6 +151,30 @@ void srslte_chest_ul_free(srslte_chest_ul_t *q)
   bzero(q, sizeof(srslte_chest_ul_t));
 }
 
+int srslte_chest_ul_set_cell(srslte_chest_ul_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q                != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      ret = srslte_refsignal_ul_set_cell(&q->dmrs_signal, cell);
+      if (ret != SRSLTE_SUCCESS) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, NOF_REFS_SF)) {
+        fprintf(stderr, "Error initializing vector interpolator\n");
+        return SRSLTE_ERROR;
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 void srslte_chest_ul_set_cfg(srslte_chest_ul_t *q, 
                              srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
                              srslte_pucch_cfg_t *pucch_cfg, 
diff --git a/lib/src/phy/ch_estimation/refsignal_dl.c b/lib/src/phy/ch_estimation/refsignal_dl.c
index ec0f0b4e0..fa8a27968 100644
--- a/lib/src/phy/ch_estimation/refsignal_dl.c
+++ b/lib/src/phy/ch_estimation/refsignal_dl.c
@@ -99,89 +99,99 @@ inline uint32_t srslte_refsignal_cs_nsymbol(uint32_t l, srslte_cp_t cp, uint32_t
 }
 
 
-/** Allocates and precomputes the Cell-Specific Reference (CSR) signal for 
- * the 20 slots in a subframe
+/** Allocates memory for the 20 slots in a subframe
  */
-int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, srslte_cell_t cell)
+int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, uint32_t max_prb)
 {
 
-  uint32_t c_init;
-  uint32_t i, ns, l, p;
-  uint32_t N_cp, mp;
-  srslte_sequence_t seq;
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR; 
-    
-    bzero(q, sizeof(srslte_refsignal_cs_t));
-    bzero(&seq, sizeof(srslte_sequence_t));
-    if (srslte_sequence_init(&seq, 2 * 2 * SRSLTE_MAX_PRB)) {
-      goto free_and_exit;
-    }
-    
-    if (SRSLTE_CP_ISNORM(cell.cp)) {
-      N_cp = 1;
-    } else {
-      N_cp = 0;
-    }
-    
-    q->cell = cell; 
-    
-    for (p=0;p<2;p++) {
-      for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
-        q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, 2*p));
+
+    for (int p=0;p<2;p++) {
+      for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
+        q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(max_prb, 2*p));
         if (!q->pilots[p][i]) {
           perror("malloc");
           goto free_and_exit;
         }
       }      
     }
-    
-    for (ns=0;ns<SRSLTE_NSLOTS_X_FRAME;ns++) {
-      for (p=0;p<2;p++) {
-        uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(2*p)/2;
-        for (l = 0; l < nsymbols; l++) {
-          /* Compute sequence init value */
-          uint32_t lp = srslte_refsignal_cs_nsymbol(l, cell.cp, 2*p);
-          c_init = 1024 * (7 * (ns + 1) + lp + 1) * (2 * cell.id + 1)
-            + 2 * cell.id + N_cp;
-          
-          /* generate sequence for this symbol and slot */
-          srslte_sequence_set_LTE_pr(&seq, c_init);
-          
-          /* Compute signal */
-          for (i = 0; i < 2*q->cell.nof_prb; i++) {
-            mp = i + SRSLTE_MAX_PRB - cell.nof_prb;
-            /* save signal */
-            q->pilots[p][ns/2][SRSLTE_REFSIGNAL_PILOT_IDX(i,(ns%2)*nsymbols+l,q->cell)] = 
-                                                  (1 - 2 * (float) seq.c[2 * mp]) / sqrt(2) +
-                                    _Complex_I * (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
-          }        
-        }
-        
-      }
-    }
-    srslte_sequence_free(&seq);
     ret = SRSLTE_SUCCESS;
   }
 free_and_exit:
   if (ret == SRSLTE_ERROR) {
-    srslte_sequence_free(&seq);
     srslte_refsignal_cs_free(q);
   }
   return ret;
 }
 
+/** Allocates and precomputes the Cell-Specific Reference (CSR) signal for
+ * the 20 slots in a subframe
+ */
+int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q, srslte_cell_t cell)
+{
+
+  uint32_t c_init;
+  uint32_t i, ns, l, p;
+  uint32_t N_cp, mp;
+  srslte_sequence_t seq;
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      bzero(&seq, sizeof(srslte_sequence_t));
+      if (srslte_sequence_init(&seq, 2*2*SRSLTE_MAX_PRB)) {
+        return SRSLTE_ERROR;
+      }
+
+      if (SRSLTE_CP_ISNORM(cell.cp)) {
+        N_cp = 1;
+      } else {
+        N_cp = 0;
+      }
+
+      for (ns=0;ns<SRSLTE_NSLOTS_X_FRAME;ns++) {
+        for (p=0;p<2;p++) {
+          uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(2*p)/2;
+          for (l = 0; l < nsymbols; l++) {
+            /* Compute sequence init value */
+            uint32_t lp = srslte_refsignal_cs_nsymbol(l, cell.cp, 2*p);
+            c_init = 1024 * (7 * (ns + 1) + lp + 1) * (2 * cell.id + 1)
+                     + 2 * cell.id + N_cp;
+
+            /* generate sequence for this symbol and slot */
+            srslte_sequence_set_LTE_pr(&seq, 2*2*SRSLTE_MAX_PRB, c_init);
+
+            /* Compute signal */
+            for (i = 0; i < 2*q->cell.nof_prb; i++) {
+              mp = i + SRSLTE_MAX_PRB - cell.nof_prb;
+              /* save signal */
+              q->pilots[p][ns/2][SRSLTE_REFSIGNAL_PILOT_IDX(i,(ns%2)*nsymbols+l,q->cell)] =
+                (1 - 2 * (float) seq.c[2 * mp]) / sqrt(2) +
+                _Complex_I * (1 - 2 * (float) seq.c[2 * mp + 1]) / sqrt(2);
+            }
+          }
+        }
+      }
+      srslte_sequence_free(&seq);
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 /** Deallocates a srslte_refsignal_cs_t object allocated with srslte_refsignal_cs_init */
 void srslte_refsignal_cs_free(srslte_refsignal_cs_t * q)
 {
-  int i, p; 
-  
-  for (p=0;p<2;p++) {
-    for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
+  for (int p=0;p<2;p++) {
+    for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
       if (q->pilots[p][i]) {
         free(q->pilots[p][i]);
       }
diff --git a/lib/src/phy/ch_estimation/refsignal_ul.c b/lib/src/phy/ch_estimation/refsignal_ul.c
index 609ef52d0..a287f8d48 100644
--- a/lib/src/phy/ch_estimation/refsignal_ul.c
+++ b/lib/src/phy/ch_estimation/refsignal_ul.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <stdlib.h>
 #include <complex.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
@@ -138,8 +139,8 @@ static int generate_n_prs(srslte_refsignal_ul_t * q) {
       }
       q->n_prs_pusch[delta_ss][ns] = n_prs;
     }
-    srslte_sequence_free(&seq);
   }
+  srslte_sequence_free(&seq);
 
   return SRSLTE_SUCCESS; 
 }
@@ -160,9 +161,9 @@ static int generate_srslte_sequence_hopping_v(srslte_refsignal_ul_t *q) {
         return SRSLTE_ERROR;
       }
       q->v_pusch[ns][delta_ss] = seq.c[ns];    
-      srslte_sequence_free(&seq);
     }
   }
+  srslte_sequence_free(&seq);
   return SRSLTE_SUCCESS;
 }
 
@@ -170,46 +171,24 @@ static int generate_srslte_sequence_hopping_v(srslte_refsignal_ul_t *q) {
 /** Initializes srslte_refsignal_ul_t object according to 3GPP 36.211 5.5
  *
  */
-int srslte_refsignal_ul_init(srslte_refsignal_ul_t * q, srslte_cell_t cell)
+int srslte_refsignal_ul_init(srslte_refsignal_ul_t * q, uint32_t max_prb)
 {
 
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL && srslte_cell_isvalid(&cell)) {
+  if (q != NULL) {
 
     ret = SRSLTE_ERROR; 
     
     bzero(q, sizeof(srslte_refsignal_ul_t));
-    q->cell = cell; 
-    
+
     // Allocate temporal buffer for computing signal argument
-    q->tmp_arg = srslte_vec_malloc(SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t)); 
+    q->tmp_arg = srslte_vec_malloc(SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->tmp_arg) {
       perror("malloc");
       goto free_and_exit;
     }
     
-    srslte_pucch_cfg_default(&q->pucch_cfg);
-    
-    // Precompute n_prs
-    if (generate_n_prs(q)) {
-      goto free_and_exit;
-    }
-    
-    // Precompute group hopping values u. 
-    if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
-      goto free_and_exit;
-    }
-    
-    // Precompute sequence hopping values v. Uses f_ss_pusch
-    if (generate_srslte_sequence_hopping_v(q)) {
-      goto free_and_exit;
-    }
-
-    if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
-      goto free_and_exit;
-    }
-
     ret = SRSLTE_SUCCESS;
   }
 free_and_exit:
@@ -226,6 +205,45 @@ void srslte_refsignal_ul_free(srslte_refsignal_ul_t * q) {
   bzero(q, sizeof(srslte_refsignal_ul_t));
 }
 
+/** Initializes srslte_refsignal_ul_t object according to 3GPP 36.211 5.5
+ *
+ */
+int srslte_refsignal_ul_set_cell(srslte_refsignal_ul_t * q, srslte_cell_t cell)
+{
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell)) {
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      srslte_pucch_cfg_default(&q->pucch_cfg);
+
+      // Precompute n_prs
+      if (generate_n_prs(q)) {
+        return SRSLTE_ERROR;
+      }
+
+      // Precompute group hopping values u.
+      if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+
+      // Precompute sequence hopping values v. Uses f_ss_pusch
+      if (generate_srslte_sequence_hopping_v(q)) {
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
+        return SRSLTE_ERROR;
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 void srslte_refsignal_ul_set_cfg(srslte_refsignal_ul_t *q, 
                                   srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
                                   srslte_pucch_cfg_t *pucch_cfg, 
@@ -363,15 +381,38 @@ void compute_r(srslte_refsignal_ul_t *q, uint32_t nof_prb, uint32_t ns, uint32_t
 
 }
 
+int srslte_refsignal_dmrs_pusch_pregen_init(srslte_refsignal_ul_t *q, srslte_refsignal_ul_dmrs_pregen_t *pregen,
+                                            uint32_t max_prb)
+{
+  for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
+    for (uint32_t cs=0;cs<SRSLTE_NOF_CSHIFT;cs++) {
+      pregen->r[cs][sf_idx] = (cf_t**) calloc(sizeof(cf_t*), max_prb + 1);
+      if (pregen->r[cs][sf_idx]) {
+        for (uint32_t n=0;n<=max_prb;n++) {
+          if (srslte_dft_precoding_valid_prb(n)) {
+            pregen->r[cs][sf_idx][n] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*n*2*SRSLTE_NRE);
+            if (pregen->r[cs][sf_idx][n]) {
+            } else {
+              return SRSLTE_ERROR;
+            }
+          }
+        }
+      } else {
+        return SRSLTE_ERROR;
+      }
+    }
+  }
+  return SRSLTE_SUCCESS;
+}
+
+
 int srslte_refsignal_dmrs_pusch_pregen(srslte_refsignal_ul_t *q, srslte_refsignal_ul_dmrs_pregen_t *pregen)
 {
   for (uint32_t sf_idx=0;sf_idx<SRSLTE_NSUBFRAMES_X_FRAME;sf_idx++) {
     for (uint32_t cs=0;cs<SRSLTE_NOF_CSHIFT;cs++) {
-      pregen->r[cs][sf_idx] = (cf_t**) calloc(sizeof(cf_t*), q->cell.nof_prb + 1);
       if (pregen->r[cs][sf_idx]) {
         for (uint32_t n=0;n<=q->cell.nof_prb;n++) {
           if (srslte_dft_precoding_valid_prb(n)) {
-            pregen->r[cs][sf_idx][n] = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*n*2*SRSLTE_NRE);
             if (pregen->r[cs][sf_idx][n]) {
               if (srslte_refsignal_dmrs_pusch_gen(q, n, sf_idx, cs, pregen->r[cs][sf_idx][n])) {
                 return SRSLTE_ERROR; 
diff --git a/lib/src/phy/ch_estimation/test/chest_test_dl.c b/lib/src/phy/ch_estimation/test/chest_test_dl.c
index c60e94d96..f91600872 100644
--- a/lib/src/phy/ch_estimation/test/chest_test_dl.c
+++ b/lib/src/phy/ch_estimation/test/chest_test_dl.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <unistd.h>
 #include <complex.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/srslte.h"
 
@@ -128,10 +129,13 @@ int main(int argc, char **argv) {
     cid = cell.id;
     max_cid = cell.id;
   }
-
+  if (srslte_chest_dl_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    goto do_exit;
+  }
   while(cid <= max_cid) {
     cell.id = cid; 
-    if (srslte_chest_dl_init(&est, cell)) {
+    if (srslte_chest_dl_set_cell(&est, cell)) {
       fprintf(stderr, "Error initializing equalizer\n");
       goto do_exit;
     }
@@ -214,10 +218,10 @@ int main(int argc, char **argv) {
         }
       }
     }
-    srslte_chest_dl_free(&est);
     cid+=10;
     INFO("cid=%d\n", cid);
   }
+  srslte_chest_dl_free(&est);
 
 
   ret = 0;
diff --git a/lib/src/phy/ch_estimation/test/chest_test_ul.c b/lib/src/phy/ch_estimation/test/chest_test_ul.c
index b3b2331da..7b9fbacc9 100644
--- a/lib/src/phy/ch_estimation/test/chest_test_ul.c
+++ b/lib/src/phy/ch_estimation/test/chest_test_ul.c
@@ -125,13 +125,17 @@ int main(int argc, char **argv) {
     max_cid = cell.id;
   }
   printf("max_cid=%d, cid=%d, cell.id=%d\n", max_cid, cid, cell.id);
+  if (srslte_chest_ul_init(&est, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    goto do_exit;
+  }
   while(cid <= max_cid) {
     cell.id = cid; 
-    if (srslte_chest_ul_init(&est, cell)) {
+    if (srslte_chest_ul_set_cell(&est, cell)) {
       fprintf(stderr, "Error initializing equalizer\n");
       goto do_exit;
     }
-    
+
     for (int n=6;n<=cell.nof_prb;n+=5) {
       if (srslte_dft_precoding_valid_prb(n)) {
         for (int delta_ss=29;delta_ss<SRSLTE_NOF_DELTA_SS;delta_ss++) {
@@ -213,9 +217,9 @@ int main(int argc, char **argv) {
     }
     cid+=10;
     printf("cid=%d\n", cid);
-    srslte_chest_ul_free(&est);
   }
 
+  srslte_chest_ul_free(&est);
 
   if (fmatlab) {
     fprintf(fmatlab, "input=");
diff --git a/lib/src/phy/ch_estimation/test/refsignal_ul_test.c b/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
index 40e940b49..c1323bf0d 100644
--- a/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
+++ b/lib/src/phy/ch_estimation/test/refsignal_ul_test.c
@@ -81,7 +81,12 @@ int main(int argc, char **argv) {
   
   parse_args(argc,argv);
 
-  if (srslte_refsignal_ul_init(&refs, cell)) {
+  if (srslte_refsignal_ul_init(&refs, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing UL reference signal\n");
+    goto do_exit;
+  }
+
+  if (srslte_refsignal_ul_set_cell(&refs, cell)) {
     fprintf(stderr, "Error initializing UL reference signal\n");
     goto do_exit;
   }
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index 8386ae18b..257d56e0f 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -212,7 +212,7 @@ int srslte_symbol_sz_power2(uint32_t nof_prb) {
     return 1024;
   } else if (nof_prb<=75) {
     return 1536;
-  } else if (nof_prb<=100) {
+  } else if (nof_prb<=110) {
     return 2048;
   } else {
     return -1;
@@ -234,7 +234,7 @@ int srslte_symbol_sz(uint32_t nof_prb) {
       return 768;
     } else if (nof_prb<=75) {
       return 1024;
-    } else if (nof_prb<=100) {
+    } else if (nof_prb<=110) {
       return 1536;
     } else {
       return SRSLTE_ERROR;
diff --git a/lib/src/phy/common/sequence.c b/lib/src/phy/common/sequence.c
index 2a863bcee..675dc9df3 100644
--- a/lib/src/phy/common/sequence.c
+++ b/lib/src/phy/common/sequence.c
@@ -27,7 +27,8 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <strings.h>
-#include <assert.h>
+#include <pthread.h>
+#include <srslte/phy/common/sequence.h>
 
 #include "srslte/phy/common/sequence.h"
 #include "srslte/phy/utils/vector.h"
@@ -35,26 +36,71 @@
 
 #define Nc 1600
 
+#define MAX_SEQ_LEN  (128*1024)
 
 /*
  * Pseudo Random Sequence generation.
  * It follows the 3GPP Release 8 (LTE) 36.211
  * Section 7.2
  */
-void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t seed) {
+#ifdef static
+static uint8_t x1[Nc+MAX_SEQ_LEN+31];
+static uint8_t x2[Nc+MAX_SEQ_LEN+31];
+
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+int srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
+  int n;
+
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d exceeds maximum len %d\n",
+            len, MAX_SEQ_LEN);
+    return -1;
+  }
+
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d is greater than allocated len %d\n",
+            len, q->max_len);
+    return -1;
+  }
+  pthread_mutex_lock(&mutex);
+
+  for (n = 0; n < 31; n++) {
+    x2[n] = (seed >> n) & 0x1;
+  }
+  x1[0] = 1;
+
+  for (n = 0; n < Nc + len; n++) {
+    x1[n + 31] = (x1[n + 3] + x1[n]) & 0x1;
+    x2[n + 31] = (x2[n + 3] + x2[n + 2] + x2[n+1] + x2[n]) & 0x1;
+  }
+
+  for (n = 0; n < len; n++) {
+    q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
+  }
+  pthread_mutex_unlock(&mutex);
+}
+
+#else
+int srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
   int n;
   uint32_t *x1, *x2;
 
-  x1 = calloc(Nc + q->len + 31, sizeof(uint32_t));
+  if (len > q->max_len) {
+    fprintf(stderr, "Error generating pseudo-random sequence: len %d is greater than allocated len %d\n",
+            len, q->max_len);
+    return -1;
+  }
+
+  x1 = calloc(Nc + len + 31, sizeof(uint32_t));
   if (!x1) {
     perror("calloc");
-    return;
+    return -1;
   }
-  x2 = calloc(Nc + q->len + 31, sizeof(uint32_t));
+  x2 = calloc(Nc + len + 31, sizeof(uint32_t));
   if (!x2) {
     free(x1);
     perror("calloc");
-    return;
+    return -1;
   }
 
   for (n = 0; n < 31; n++) {
@@ -62,45 +108,43 @@ void srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t seed) {
   }
   x1[0] = 1;
 
-  for (n = 0; n < Nc + q->len; n++) {
+  for (n = 0; n < Nc + len; n++) {
     x1[n + 31] = (x1[n + 3] + x1[n]) & 0x1;
     x2[n + 31] = (x2[n + 3] + x2[n + 2] + +x2[n+1] + x2[n]) & 0x1;
   }
 
-  for (n = 0; n < q->len; n++) {
+  for (n = 0; n < len; n++) {
     q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
   }
 
   free(x1);
   free(x2);
+
+  return 0;
 }
 
+#endif
+
 int srslte_sequence_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
   if (srslte_sequence_init(q, len)) {
     return SRSLTE_ERROR;
   }
-  q->len = len;
-  srslte_sequence_set_LTE_pr(q, seed);
+  q->cur_len = len;
+  srslte_sequence_set_LTE_pr(q, len, seed);
   srslte_bit_pack_vector(q->c, q->c_bytes, len);
   for (int i=0;i<len;i++) {
     q->c_float[i] = (1-2*q->c[i]);
     q->c_short[i] = (int16_t) q->c_float[i];
   }
+
+
+
   return SRSLTE_SUCCESS;
 }
 
 int srslte_sequence_init(srslte_sequence_t *q, uint32_t len) {
-  if (q->c && (q->len != len)) {
-    free(q->c);
-    if (q->c_bytes) {
-      free(q->c_bytes);
-    }
-    if (q->c_float) {
-      free(q->c_float);
-    }
-    if (q->c_short) {
-      free(q->c_short);
-    }
+  if (q->c && len > q->max_len) {
+    srslte_sequence_free(q);
   }
   if (!q->c) {
     q->c = srslte_vec_malloc(len * sizeof(uint8_t));
@@ -119,7 +163,7 @@ int srslte_sequence_init(srslte_sequence_t *q, uint32_t len) {
     if (!q->c_short) {
       return SRSLTE_ERROR;
     }
-    q->len = len;
+    q->max_len = len;
   }
   return SRSLTE_SUCCESS;
 }
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 347e04547..d841d7d33 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -29,6 +29,7 @@
 #include <complex.h>
 #include <fftw3.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/dft/dft.h"
 #include "srslte/phy/utils/vector.h"
@@ -46,11 +47,41 @@ int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_di
   return 0;
 }
 
+int srslte_dft_replan(srslte_dft_plan_t *plan, const int new_dft_points) {
+  if (new_dft_points <= plan->init_size) {
+    if(plan->mode == SRSLTE_DFT_COMPLEX){
+      return srslte_dft_replan_c(plan,new_dft_points);
+    } else {
+      return srslte_dft_replan_r(plan,new_dft_points);
+    }
+  } else {
+    fprintf(stderr, "DFT: Error calling replan: new_dft_points (%d) must be lower or equal "
+      "dft_size passed initially (%d)\n", new_dft_points, plan->init_size);
+    return -1;
+  }
+}
+
+
+
 static void allocate(srslte_dft_plan_t *plan, int size_in, int size_out, int len) {
   plan->in = fftwf_malloc(size_in*len);
   plan->out = fftwf_malloc(size_out*len);
 }
 
+int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
+  int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
+  if (plan->p) {
+    fftwf_destroy_plan(plan->p);
+    plan->p = NULL;
+  }
+  plan->p = fftwf_plan_dft_1d(new_dft_points, plan->in, plan->out, sign, 0U);
+  if (!plan->p) {
+    return -1;
+  }
+  plan->size = new_dft_points;
+  return 0;
+}
+
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
@@ -59,6 +90,7 @@ int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
     return -1;
   }
   plan->size = dft_points;
+  plan->init_size = plan->size;
   plan->mode = SRSLTE_DFT_COMPLEX;
   plan->dir = dir;
   plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
@@ -70,6 +102,20 @@ int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
   return 0;
 }
 
+int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
+  int sign = (plan->dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
+  if (plan->p) {
+    fftwf_destroy_plan(plan->p);
+    plan->p = NULL;
+  }
+  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, 0U);
+  if (!plan->p) {
+    return -1;
+  }
+  plan->size = new_dft_points;
+  return 0;
+}
+
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
@@ -78,6 +124,7 @@ int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
     return -1;
   }
   plan->size = dft_points;
+  plan->init_size = plan->size;
   plan->mode = SRSLTE_REAL;
   plan->dir = dir;
   plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
diff --git a/lib/src/phy/dft/dft_precoding.c b/lib/src/phy/dft/dft_precoding.c
index 3bec5b96e..a43d79406 100644
--- a/lib/src/phy/dft/dft_precoding.c
+++ b/lib/src/phy/dft/dft_precoding.c
@@ -40,37 +40,42 @@
 #include "srslte/phy/dft/dft_precoding.h"
 
 /* Create DFT plans for transform precoding */
-int srslte_dft_precoding_init(srslte_dft_precoding_t *q, uint32_t max_prb) 
+
+int srslte_dft_precoding_init(srslte_dft_precoding_t *q, uint32_t max_prb, bool is_tx)
 {
-  int ret = SRSLTE_ERROR_INVALID_INPUTS; 
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
   bzero(q, sizeof(srslte_dft_precoding_t));
-  
+
   if (max_prb <= SRSLTE_MAX_PRB) {
-    ret = SRSLTE_ERROR; 
+    ret = SRSLTE_ERROR;
     for (uint32_t i=1;i<=max_prb;i++) {
-      if(srslte_dft_precoding_valid_prb(i)) {        
+      if(srslte_dft_precoding_valid_prb(i)) {
         DEBUG("Initiating DFT precoding plan for %d PRBs\n", i);
-        if (srslte_dft_plan_c(&q->dft_plan[i], i*SRSLTE_NRE, SRSLTE_DFT_FORWARD)) {
+        if (srslte_dft_plan_c(&q->dft_plan[i], i*SRSLTE_NRE, is_tx?SRSLTE_DFT_FORWARD:SRSLTE_DFT_BACKWARD)) {
           fprintf(stderr, "Error: Creating DFT plan %d\n",i);
           goto clean_exit;
         }
         srslte_dft_plan_set_norm(&q->dft_plan[i], true);
-        if (srslte_dft_plan_c(&q->idft_plan[i], i*SRSLTE_NRE, SRSLTE_DFT_BACKWARD)) {
-          fprintf(stderr, "Error: Creating DFT plan %d\n",i);
-          goto clean_exit;
-        }
-        srslte_dft_plan_set_norm(&q->idft_plan[i], true);
       }
     }
     q->max_prb = max_prb;
     ret = SRSLTE_SUCCESS;
-  } 
+  }
 
-clean_exit:
+  clean_exit:
   if (ret == SRSLTE_ERROR) {
     srslte_dft_precoding_free(q);
   }
-  return ret; 
+  return ret;
+}
+
+int srslte_dft_precoding_init_rx(srslte_dft_precoding_t *q, uint32_t max_prb)
+{
+  return srslte_dft_precoding_init(q, max_prb, false);
+}
+
+int srslte_dft_precoding_init_tx(srslte_dft_precoding_t *q, uint32_t max_prb) {
+  return srslte_dft_precoding_init(q, max_prb, true);
 }
 
 /* Free DFT plans for transform precoding */
@@ -79,7 +84,6 @@ void srslte_dft_precoding_free(srslte_dft_precoding_t  *q)
   for (uint32_t i=1;i<=q->max_prb;i++) {
     if(srslte_dft_precoding_valid_prb(i)) {      
       srslte_dft_plan_free(&q->dft_plan[i]);
-      srslte_dft_plan_free(&q->idft_plan[i]);        
     }
   }  
   bzero(q, sizeof(srslte_dft_precoding_t));
@@ -98,7 +102,7 @@ bool srslte_dft_precoding_valid_prb(uint32_t nof_prb) {
 }
 
 int srslte_dft_precoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output, 
-                  uint32_t nof_prb, uint32_t nof_symbols) 
+                         uint32_t nof_prb, uint32_t nof_symbols)
 {
   
   if (!srslte_dft_precoding_valid_prb(nof_prb) && nof_prb <= q->max_prb) {
@@ -112,19 +116,3 @@ int srslte_dft_precoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output,
   
   return SRSLTE_SUCCESS;
 }
-
-int srslte_dft_predecoding(srslte_dft_precoding_t *q, cf_t *input, cf_t *output, 
-                    uint32_t nof_prb, uint32_t nof_symbols)
-{
-  if (!srslte_dft_precoding_valid_prb(nof_prb) && nof_prb <= q->max_prb) {
-    fprintf(stderr, "Error invalid number of PRB (%d)\n", nof_prb);
-    return SRSLTE_ERROR; 
-  }
-
-  for (uint32_t i=0;i<nof_symbols;i++) {
-    srslte_dft_run_c(&q->idft_plan[nof_prb], &input[i*SRSLTE_NRE*nof_prb], &output[i*SRSLTE_NRE*nof_prb]);
-  }
-  
-  return SRSLTE_SUCCESS;
-  
-}
diff --git a/lib/src/phy/dft/ofdm.c b/lib/src/phy/dft/ofdm.c
index 69b7e9161..825aff1e0 100644
--- a/lib/src/phy/dft/ofdm.c
+++ b/lib/src/phy/dft/ofdm.c
@@ -29,6 +29,7 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/dft/dft.h"
@@ -48,6 +49,12 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
     return -1;
   }
 
+  q->shift_buffer = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN(symbol_sz));
+  if (!q->shift_buffer) {
+    perror("malloc");
+    return -1;
+  }
+
   srslte_dft_plan_set_mirror(&q->fft_plan, true);
   srslte_dft_plan_set_dc(&q->fft_plan, true);
 
@@ -55,7 +62,6 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
   q->nof_symbols = SRSLTE_CP_NSYMB(cp);
   q->cp = cp;
   q->freq_shift = false;
-  q->shift_buffer = NULL; 
   q->nof_re = nof_prb * SRSLTE_NRE;
   q->nof_guards = ((symbol_sz - q->nof_re) / 2);
   q->slot_sz = SRSLTE_SLOT_LEN(symbol_sz);
@@ -67,6 +73,32 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
   return SRSLTE_SUCCESS;
 }
 
+
+int srslte_ofdm_replan_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_prb) {
+
+  if (srslte_dft_replan_c(&q->fft_plan, symbol_sz)) {
+    fprintf(stderr, "Error: Creating DFT plan\n");
+    return -1;
+  }
+
+  q->symbol_sz = (uint32_t) symbol_sz;
+  q->nof_symbols = SRSLTE_CP_NSYMB(cp);
+  q->cp = cp;
+  q->nof_re = nof_prb * SRSLTE_NRE;
+  q->nof_guards = ((symbol_sz - q->nof_re) / 2);
+  q->slot_sz = SRSLTE_SLOT_LEN(symbol_sz);
+
+  if (q->freq_shift) {
+    srslte_ofdm_set_freq_shift(q, q->freq_shift_f);
+  }
+
+  DEBUG("Replan symbol_sz=%d, nof_symbols=%d, cp=%s, nof_re=%d, nof_guards=%d\n",
+        q->symbol_sz, q->nof_symbols,
+        q->cp==SRSLTE_CP_NORM?"Normal":"Extended", q->nof_re, q->nof_guards);
+
+  return SRSLTE_SUCCESS;
+}
+
 void srslte_ofdm_free_(srslte_ofdm_t *q) {
   srslte_dft_plan_free(&q->fft_plan);
   if (q->tmp) {
@@ -78,30 +110,28 @@ void srslte_ofdm_free_(srslte_ofdm_t *q) {
   bzero(q, sizeof(srslte_ofdm_t));
 }
 
-int srslte_ofdm_rx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
-  int symbol_sz = srslte_symbol_sz(nof_prb);
+int srslte_ofdm_rx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
+  int symbol_sz = srslte_symbol_sz(max_prb);
   if (symbol_sz < 0) {
-    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", max_prb);
     return -1;
   }
-  return srslte_ofdm_init_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_FORWARD);
+  q->max_prb = max_prb;
+  return srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_FORWARD);
 }
 
-void srslte_ofdm_rx_free(srslte_ofdm_t *q) {
-  srslte_ofdm_free_(q);
-}
-
-int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
   uint32_t i;
   int ret;
   
-  int symbol_sz = srslte_symbol_sz(nof_prb);
+  int symbol_sz = srslte_symbol_sz(max_prb);
   if (symbol_sz < 0) {
-    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", max_prb);
     return -1;
   }
+  q->max_prb = max_prb;
 
-  ret = srslte_ofdm_init_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_BACKWARD); 
+  ret = srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_BACKWARD);
   
   if (ret == SRSLTE_SUCCESS) {
     srslte_dft_plan_set_norm(&q->fft_plan, false);
@@ -115,16 +145,57 @@ int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
   return ret;
 }
 
+int srslte_ofdm_rx_set_prb(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+  if (nof_prb <= q->max_prb) {
+    int symbol_sz = srslte_symbol_sz(nof_prb);
+    if (symbol_sz < 0) {
+      fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+      return -1;
+    }
+    return srslte_ofdm_replan_(q, cp, symbol_sz, nof_prb);
+  } else {
+    fprintf(stderr, "OFDM: Error calling set_prb: nof_prb must be equal or lower initialized max_prb\n");
+    return -1;
+  }
+}
+
+int srslte_ofdm_tx_set_prb(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
+  uint32_t i;
+  int ret;
+
+  if (nof_prb <= q->max_prb) {
+    int symbol_sz = srslte_symbol_sz(nof_prb);
+    if (symbol_sz < 0) {
+      fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+      return -1;
+    }
+
+    ret = srslte_ofdm_replan_(q, cp, symbol_sz, nof_prb);
+
+    if (ret == SRSLTE_SUCCESS) {
+      /* set now zeros at CP */
+      for (i=0;i<q->nof_symbols;i++) {
+        bzero(q->tmp, q->nof_guards * sizeof(cf_t));
+        bzero(&q->tmp[q->nof_re + q->nof_guards], q->nof_guards * sizeof(cf_t));
+      }
+    }
+    return ret;
+  } else {
+    fprintf(stderr, "OFDM: Error calling set_prb: nof_prb must be equal or lower initialized max_prb\n");
+    return -1;
+  }
+}
+
+
+void srslte_ofdm_rx_free(srslte_ofdm_t *q) {
+  srslte_ofdm_free_(q);
+}
+
 /* Shifts the signal after the iFFT or before the FFT. 
  * Freq_shift is relative to inter-carrier spacing.
  * Caution: This function shall not be called during run-time 
  */
 int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q, float freq_shift) {
-  q->shift_buffer = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN(q->symbol_sz));
-  if (!q->shift_buffer) {
-    perror("malloc");
-    return -1; 
-  }
   cf_t *ptr = q->shift_buffer;
   for (uint32_t n=0;n<2;n++) {
     for (uint32_t i=0;i<q->nof_symbols;i++) {
@@ -140,7 +211,7 @@ int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q, float freq_shift) {
   srslte_dft_plan_set_dc(&q->fft_plan, false);  
  
   q->freq_shift = true;
-  
+  q->freq_shift_f = freq_shift;
   return SRSLTE_SUCCESS;
 }
 
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index 4cc33d3b8..095312621 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -29,6 +29,7 @@
 #include <complex.h>
 #include <math.h>
 #include <string.h>
+#include <srslte/phy/common/phy_common.h>
 
 
 #define CURRENT_FFTSIZE   srslte_symbol_sz(q->cell.nof_prb)
@@ -39,61 +40,55 @@
 
 #define SRSLTE_ENB_RF_AMP 0.1
 
-int srslte_enb_dl_init(srslte_enb_dl_t *q, srslte_cell_t cell)
+int srslte_enb_dl_init(srslte_enb_dl_t *q, uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_enb_dl_t));
     
-    q->cell = cell;
-    q->cfi  = 3; 
+    q->cfi  = 3;
     q->tx_amp = SRSLTE_ENB_RF_AMP;
     
-    if (srslte_ofdm_tx_init(&q->ifft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_tx_init(&q->ifft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
 
     srslte_ofdm_set_normalize(&q->ifft, true);
 
-    if (srslte_regs_init(&q->regs, q->cell)) {
-      fprintf(stderr, "Error initiating REGs\n");
-      goto clean_exit;
-    }
-    if (srslte_pbch_init(&q->pbch, q->cell)) {
+    if (srslte_pbch_init(&q->pbch)) {
       fprintf(stderr, "Error creating PBCH object\n");
       goto clean_exit;
     }
-    if (srslte_pcfich_init(&q->pcfich, &q->regs, q->cell)) {
+    if (srslte_pcfich_init(&q->pcfich)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich, &q->regs, q->cell)) {
+    if (srslte_phich_init(&q->phich)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init(&q->pdcch, &q->regs, q->cell)) {
+    if (srslte_pdcch_init(&q->pdcch, max_prb)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init(&q->pdsch, q->cell)) {
+    if (srslte_pdsch_init_enb(&q->pdsch, max_prb)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
     
-    if (srslte_refsignal_cs_init(&q->csr_signal, q->cell)) {
+    if (srslte_refsignal_cs_init(&q->csr_signal, max_prb)) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    for (int i=0;i<q->cell.nof_ports;i++) {
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
       q->sf_symbols[i] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
       if (!q->sf_symbols[i]) {
         perror("malloc");
@@ -102,15 +97,10 @@ int srslte_enb_dl_init(srslte_enb_dl_t *q, srslte_cell_t cell)
       q->slot1_symbols[i] = &q->sf_symbols[i][CURRENT_SLOTLEN_RE];
     }
     
-    /* Generate PSS/SSS signals */
-    srslte_pss_generate(q->pss_signal, cell.id%3);
-    srslte_sss_generate(q->sss_signal0, q->sss_signal5, cell.id);
-    
     ret = SRSLTE_SUCCESS;
     
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -141,6 +131,71 @@ void srslte_enb_dl_free(srslte_enb_dl_t *q)
   }  
 }
 
+int srslte_enb_dl_set_cell(srslte_enb_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->cfi  = 3;
+    q->tx_amp = SRSLTE_ENB_RF_AMP;
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      if (q->cell.nof_prb) {
+        if (srslte_regs_init(&q->regs, q->cell)) {
+          fprintf(stderr, "Error initiating REGs\n");
+          return SRSLTE_ERROR;
+        }
+      }
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (srslte_ofdm_rx_set_prb(&q->ifft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error initiating FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pbch_set_cell(&q->pbch, q->cell)) {
+        fprintf(stderr, "Error creating PBCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pcfich_set_cell(&q->pcfich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PCFICH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_phich_set_cell(&q->phich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PHICH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdcch_set_cell(&q->pdcch, &q->regs, q->cell)) {
+        fprintf(stderr, "Error creating PDCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdsch_set_cell(&q->pdsch, q->cell)) {
+        fprintf(stderr, "Error creating PDSCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_refsignal_cs_set_cell(&q->csr_signal, q->cell)) {
+        fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
+        return SRSLTE_ERROR;
+      }
+      /* Generate PSS/SSS signals */
+      srslte_pss_generate(q->pss_signal, cell.id%3);
+      srslte_sss_generate(q->sss_signal0, q->sss_signal5, cell.id);
+    }
+    ret = SRSLTE_SUCCESS;
+
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+
+
 void srslte_enb_dl_set_amp(srslte_enb_dl_t *q, float amp)
 {
   q->tx_amp = amp; 
diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index 9c294ac43..653091935 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -39,78 +39,54 @@
 
 #define MAX_CANDIDATES  16
 
-int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell, 
-                       srslte_prach_cfg_t *prach_cfg, 
-                       srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg, 
-                       srslte_pusch_hopping_cfg_t *hopping_cfg, 
-                       srslte_pucch_cfg_t *pucch_cfg)
+int srslte_enb_ul_init(srslte_enb_ul_t *q,
+                       uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_enb_ul_t));
     
-    q->cell = cell;
-    
-    if (hopping_cfg) {
-      memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
-    } 
-    
     q->users = calloc(sizeof(srslte_enb_ul_user_t*), SRSLTE_SIRNTI);
     if (!q->users) {
       perror("malloc");
       goto clean_exit;
     }
     
-    if (srslte_ofdm_rx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
     srslte_ofdm_set_normalize(&q->fft, false);
     srslte_ofdm_set_freq_shift(&q->fft, -0.5);
 
-    if (srslte_pucch_init(&q->pucch, q->cell)) {
+    if (srslte_pucch_init(&q->pucch)) {
       fprintf(stderr, "Error creating PUCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pusch_init(&q->pusch, q->cell)) {
+    if (srslte_pusch_init_enb(&q->pusch, max_prb)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    
-    if (prach_cfg) {
-      if (srslte_prach_init_cfg(&q->prach, prach_cfg, q->cell.nof_prb)) {
-        fprintf(stderr, "Error initiating PRACH\n");
-        goto clean_exit; 
-      }
-      srslte_prach_set_detect_factor(&q->prach, 60);    
-    }
-    
+
     srslte_pucch_set_threshold(&q->pucch, 0.5, 0.5);
     
-    if (srslte_chest_ul_init(&q->chest, cell)) {
+    if (srslte_chest_ul_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit; 
     }
-    
-    // Configure common PUCCH configuration 
-    srslte_pucch_set_cfg(&q->pucch, pucch_cfg, pusch_cfg->group_hopping_en);
-    
-    // SRS is a dedicated configuration
-    srslte_chest_ul_set_cfg(&q->chest, pusch_cfg, pucch_cfg, NULL);
-        
-    q->sf_symbols = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit; 
     }
     
-    q->ce = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+    q->ce = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->ce) {
       perror("malloc");
       goto clean_exit; 
@@ -119,8 +95,7 @@ int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell,
     ret = SRSLTE_SUCCESS;
     
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -158,6 +133,70 @@ void srslte_enb_ul_free(srslte_enb_ul_t *q)
   }  
 }
 
+int srslte_enb_ul_set_cell(srslte_enb_ul_t *q, srslte_cell_t cell,
+                           srslte_prach_cfg_t *prach_cfg,
+                           srslte_refsignal_dmrs_pusch_cfg_t *pusch_cfg,
+                           srslte_pusch_hopping_cfg_t *hopping_cfg,
+                           srslte_pucch_cfg_t *pucch_cfg)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (hopping_cfg) {
+        memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
+      }
+
+      if (srslte_ofdm_rx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error initiating FFT\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_set_cell(&q->pucch, q->cell)) {
+        fprintf(stderr, "Error creating PUCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pusch_set_cell(&q->pusch, q->cell)) {
+        fprintf(stderr, "Error creating PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (prach_cfg) {
+        if (srslte_prach_init_cfg(&q->prach, prach_cfg, q->cell.nof_prb)) {
+          fprintf(stderr, "Error initiating PRACH\n");
+          return SRSLTE_ERROR;
+        }
+        srslte_prach_set_detect_factor(&q->prach, 60);
+      }
+
+      srslte_pucch_set_threshold(&q->pucch, 0.5, 0.5);
+
+      if (srslte_chest_ul_set_cell(&q->chest, cell)) {
+        fprintf(stderr, "Error initiating channel estimator\n");
+        return SRSLTE_ERROR;
+      }
+
+      // Configure common PUCCH configuration
+      srslte_pucch_set_cfg(&q->pucch, pucch_cfg, pusch_cfg->group_hopping_en);
+
+      // SRS is a dedicated configuration
+      srslte_chest_ul_set_cfg(&q->chest, pusch_cfg, pucch_cfg, NULL);
+
+      ret = SRSLTE_SUCCESS;
+    }
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+
 int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, uint16_t rnti)
 {
   if (!q->users[rnti]) {
diff --git a/lib/src/phy/phch/pbch.c b/lib/src/phy/phch/pbch.c
index 783ae1e04..dfd094dec 100644
--- a/lib/src/phy/phch/pbch.c
+++ b/lib/src/phy/phch/pbch.c
@@ -32,6 +32,8 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pbch.h"
@@ -43,7 +45,6 @@
 #define PBCH_RE_CP_NORM    240
 #define PBCH_RE_CP_EXT     216
 
-
 const uint8_t srslte_crc_mask[4][16] = {
     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, 
     { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, 
@@ -139,33 +140,18 @@ int srslte_pbch_get(cf_t *slot1_data, cf_t *pbch, srslte_cell_t cell) {
  * At the receiver, the field nof_ports in the cell structure indicates the 
  * maximum number of BS transmitter ports to look for.  
  */
-int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
+int srslte_pbch_init(srslte_pbch_t *q) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q                       != NULL &&
-      srslte_cell_isvalid(&cell))
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
 
     bzero(q, sizeof(srslte_pbch_t));
-    
-    if (cell.nof_ports == 0) {
-      q->search_all_ports = true; 
-      cell.nof_ports = SRSLTE_MAX_PORTS; 
-    } else {
-      q->search_all_ports = false; 
-    }
-    
-    q->cell = cell;
-    q->nof_symbols = (SRSLTE_CP_ISNORM(q->cell.cp)) ? PBCH_RE_CP_NORM : PBCH_RE_CP_EXT;
-    
+
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
     }
-    if (srslte_sequence_pbch(&q->seq, q->cell.cp, q->cell.id)) {
-      goto clean;
-    }
-
     int poly[3] = { 0x6D, 0x4F, 0x57 };
     if (srslte_viterbi_init(&q->decoder, SRSLTE_VITERBI_37, poly, 40, true)) {
       goto clean;
@@ -178,12 +164,14 @@ int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
     q->encoder.tail_biting = true;
     memcpy(q->encoder.poly, poly, 3 * sizeof(int));
 
+    q->nof_symbols = PBCH_RE_CP_NORM;
+
     q->d = srslte_vec_malloc(sizeof(cf_t) * q->nof_symbols);
     if (!q->d) {
       goto clean;
     }
     int i;
-    for (i = 0; i < q->cell.nof_ports; i++) {
+    for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->ce[i] = srslte_vec_malloc(sizeof(cf_t) * q->nof_symbols);
       if (!q->ce[i]) {
         goto clean;
@@ -209,6 +197,7 @@ int srslte_pbch_init(srslte_pbch_t *q, srslte_cell_t cell) {
     if (!q->rm_b) {
       goto clean;
     }
+
     ret = SRSLTE_SUCCESS;
   }
 clean: 
@@ -219,11 +208,11 @@ clean:
 }
 
 void srslte_pbch_free(srslte_pbch_t *q) {
-  if (q->d) {
-    free(q->d);
-  }
+  srslte_sequence_free(&q->seq);
+  srslte_modem_table_free(&q->mod);
+  srslte_viterbi_free(&q->decoder);
   int i;
-  for (i = 0; i < q->cell.nof_ports; i++) {
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->ce[i]) {
       free(q->ce[i]);
     }
@@ -243,12 +232,37 @@ void srslte_pbch_free(srslte_pbch_t *q) {
   if (q->rm_b) {
     free(q->rm_b);
   }
-  srslte_sequence_free(&q->seq);
-  srslte_modem_table_free(&q->mod);
-  srslte_viterbi_free(&q->decoder);
-
+  if (q->d) {
+    free(q->d);
+  }
   bzero(q, sizeof(srslte_pbch_t));
+}
 
+int srslte_pbch_set_cell(srslte_pbch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                       != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    if (cell.nof_ports == 0) {
+      q->search_all_ports = true;
+      cell.nof_ports = SRSLTE_MAX_PORTS;
+    } else {
+      q->search_all_ports = false;
+    }
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (srslte_sequence_pbch(&q->seq, q->cell.cp, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+    }
+    q->nof_symbols = (SRSLTE_CP_ISNORM(q->cell.cp)) ? PBCH_RE_CP_NORM : PBCH_RE_CP_EXT;
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
 }
 
 
@@ -476,6 +490,7 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
     } else {
       nant = q->cell.nof_ports; 
     }
+
     do {
       if (nant != 3) {
         DEBUG("Trying %d TX antennas with %d frames\n", nant, q->frame_idx);
@@ -486,7 +501,7 @@ int srslte_pbch_decode(srslte_pbch_t *q, cf_t *slot1_symbols, cf_t *ce_slot1[SRS
           srslte_predecoding_single(q->symbols[0], q->ce[0], q->d, q->nof_symbols, noise_estimate);
         } else {
           srslte_predecoding_diversity(q->symbols[0], q->ce, x, nant,
-              q->nof_symbols);
+                                       q->nof_symbols);
           srslte_layerdemap_diversity(x, q->d, nant, q->nof_symbols / nant);
         }
 
@@ -577,7 +592,7 @@ int srslte_pbch_encode(srslte_pbch_t *q, uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_
     if (q->cell.nof_ports > 1) {
       srslte_layermap_diversity(q->d, x, q->cell.nof_ports, q->nof_symbols);
       srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
-          q->nof_symbols / q->cell.nof_ports);
+                                 q->nof_symbols / q->cell.nof_ports);
     } else {
       memcpy(q->symbols[0], q->d, q->nof_symbols * sizeof(cf_t));
     }
diff --git a/lib/src/phy/phch/pcfich.c b/lib/src/phy/phch/pcfich.c
index ec1e13abc..b38c0266f 100644
--- a/lib/src/phy/phch/pcfich.c
+++ b/lib/src/phy/phch/pcfich.c
@@ -57,38 +57,28 @@ bool srslte_pcfich_exists(int nframe, int nslot) {
   return true;
 }
 
-int srslte_pcfich_init(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
-  return srslte_pcfich_init_multi(q, regs, cell, 1);  
+int srslte_pcfich_init(srslte_pcfich_t *q) {
+  return srslte_pcfich_init_multi(q, 1);
 }
 
 /** Initializes the pcfich channel receiver. 
  * On error, returns -1 and frees the structrure 
  */
-int srslte_pcfich_init_multi(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) {
+int srslte_pcfich_init_multi(srslte_pcfich_t *q, uint32_t nof_rx_antennas) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
-  if (q                         != NULL &&
-      regs                      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {   
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_pcfich_t));
-    q->cell = cell;
-    q->regs = regs;
+    q->nof_rx_antennas = nof_rx_antennas;
     q->nof_symbols = PCFICH_RE;
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
+
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
     }
 
-    for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
-      if (srslte_sequence_pcfich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
-        goto clean;
-      }
-    }
-    
     /* convert cfi bit tables to floats for demodulation */
     for (int i=0;i<3;i++) {
       for (int j=0;j<PCFICH_CFI_LEN;j++) {
@@ -115,6 +105,28 @@ void srslte_pcfich_free(srslte_pcfich_t *q) {
   bzero(q, sizeof(srslte_pcfich_t));
 }
 
+int srslte_pcfich_set_cell(srslte_pcfich_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                         != NULL &&
+      regs                      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->regs = regs;
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
+        if (srslte_sequence_pcfich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 /** Finds the CFI with minimum distance with the vector of received 32 bits.
  * Saves the CFI value in the cfi pointer and returns the distance.
  */
@@ -197,7 +209,7 @@ int srslte_pcfich_decode_multi(srslte_pcfich_t *q, cf_t *sf_symbols[SRSLTE_MAX_P
       }
 
       q_symbols[j] = q->symbols[j];
-      
+
       /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
         if (q->nof_symbols != srslte_regs_pcfich_get(q->regs, ce[i][j], q->ce[i][j])) {
diff --git a/lib/src/phy/phch/pdcch.c b/lib/src/phy/phch/pdcch.c
index cfdb19621..60ab136af 100644
--- a/lib/src/phy/phch/pdcch.c
+++ b/lib/src/phy/phch/pdcch.c
@@ -61,29 +61,24 @@ float srslte_pdcch_coderate(uint32_t nof_bits, uint32_t l) {
 }
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pdcch_init(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell) {
-  return srslte_pdcch_init_multi(q, regs, cell, 1);
+int srslte_pdcch_init(srslte_pdcch_t *q, uint32_t max_prb) {
+  return srslte_pdcch_init_multi(q, max_prb, 1);
 }
 
-int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+int srslte_pdcch_init_multi(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q                         != NULL &&
-      regs                      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {   
     ret = SRSLTE_ERROR;
     bzero(q, sizeof(srslte_pdcch_t));
-    q->cell = cell;
-    q->regs = regs;
-    q->nof_rx_antennas = nof_rx_antennas; 
+    q->nof_rx_antennas = nof_rx_antennas;
     
     /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
-    q->max_bits = (srslte_regs_pdcch_nregs(q->regs, 3) / 9) * 72;
+    q->max_bits = max_prb*3*12*2;
 
-    INFO("Init PDCCH: Max bits: %d, %d ports.\n", 
-         q->max_bits, q->cell.nof_ports);
+    INFO("Init PDCCH: Max bits: %d\n", q->max_bits);
 
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       goto clean;
@@ -92,14 +87,6 @@ int srslte_pdcch_init_multi(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_
       goto clean;
     }
 
-    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      // we need to pregenerate the sequence for the maximum number of bits, which is 8 times 
-      // the maximum number of REGs (for CFI=3)
-      if (srslte_sequence_pdcch(&q->seq[i], 2 * i, q->cell.id, 8*srslte_regs_pdcch_nregs(q->regs, 3))) {
-        goto clean;
-      }
-    }
-
     int poly[3] = { 0x6D, 0x4F, 0x57 };
     if (srslte_viterbi_init(&q->decoder, SRSLTE_VITERBI_37, poly, SRSLTE_DCI_MAX_BITS + 16, true)) {
       goto clean;
@@ -187,6 +174,39 @@ void srslte_pdcch_free(srslte_pdcch_t *q) {
 
 }
 
+int srslte_pdcch_set_cell(srslte_pdcch_t *q, srslte_regs_t *regs, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                         != NULL &&
+      regs                      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->regs = regs;
+
+    /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
+    q->max_bits = (srslte_regs_pdcch_nregs(q->regs, 3) / 9) * 72;
+
+    INFO("PDCCH: Cell config PCI=%d, %d ports.\n",
+         q->cell.id, q->cell.nof_ports);
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+        // we need to pregenerate the sequence for the maximum number of bits, which is 8 times
+        // the maximum number of REGs (for CFI=3)
+        if (srslte_sequence_pdcch(&q->seq[i], 2 * i, q->cell.id, 8*srslte_regs_pdcch_nregs(q->regs, 3))) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 uint32_t srslte_pdcch_ue_locations(srslte_pdcch_t *q, srslte_dci_location_t *c, uint32_t max_candidates,
                         uint32_t nsubframe, uint32_t cfi, uint16_t rnti) 
 {
@@ -472,7 +492,7 @@ int srslte_pdcch_extract_llr_multi(srslte_pdcch_t *q, cf_t *sf_symbols[SRSLTE_MA
 
     /* descramble */
     srslte_scrambling_f_offset(&q->seq[nsubframe], q->llr, 0, e_bits);
-    
+
     ret = SRSLTE_SUCCESS;
   } 
   return ret;  
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 63e7dbfa0..da0e4a6e4 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -33,6 +33,8 @@
 #include <assert.h>
 #include <math.h>
 #include <srslte/phy/phch/pdsch.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/phch/ra.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
@@ -203,31 +205,23 @@ int srslte_pdsch_get(srslte_pdsch_t *q, cf_t *sf_symbols, cf_t *symbols,
   return srslte_pdsch_cp(q, sf_symbols, symbols, grant, lstart, subframe, false);
 }
 
-int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell) 
-{
-  return srslte_pdsch_init_multi(q, cell, 1);
-}
-
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+int pdsch_init_multi(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas, bool is_ue)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   int i;
 
  if (q != NULL                  &&
-     srslte_cell_isvalid(&cell) && 
-     nof_rx_antennas <= SRSLTE_MAX_PORTS) 
+     nof_rx_antennas <= SRSLTE_MAX_PORTS)
   {   
     
     bzero(q, sizeof(srslte_pdsch_t));
     ret = SRSLTE_ERROR;
     
-    q->cell = cell;
-    q->max_re = q->cell.nof_prb * MAX_PDSCH_RE(q->cell.cp);
+    q->max_re = max_prb * MAX_PDSCH_RE(q->cell.cp);
     q->nof_rx_antennas = nof_rx_antennas; 
     
-    INFO("Init PDSCH: %d ports %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
-        q->cell.nof_prb, q->max_re);
+    INFO("Init PDSCH: %d PRBs, max_symbols: %d\n", max_prb, q->max_re);
 
     for (i = 0; i < 4; i++) {
       if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
@@ -249,15 +243,17 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       goto clean;
     }
 
-    for (i = 0; i < q->cell.nof_ports; i++) {
+    for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
       if (!q->x[i]) {
         goto clean;
       }
-      for (int j=0;j<q->nof_rx_antennas;j++) {
-        q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-        if (!q->ce[i][j]) {
-          goto clean;
+      if (is_ue) {
+        for (int j=0;j<q->nof_rx_antennas;j++) {
+          q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+          if (!q->ce[i][j]) {
+            goto clean;
+          }
         }
       }
     }
@@ -267,13 +263,18 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
         goto clean;
       }              
     }
-    
-    q->users = calloc(sizeof(srslte_pdsch_user_t*), 1+SRSLTE_SIRNTI);
+
+    q->is_ue = is_ue;
+    q->users = calloc(sizeof(srslte_pdsch_user_t*), is_ue?1:(1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean;
     }
-    
+
+    if (srslte_sequence_init(&q->tmp_seq, q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+      goto clean;
+    }
+
     ret = SRSLTE_SUCCESS;
   }
   clean: 
@@ -283,6 +284,27 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
   return ret;
 }
 
+int srslte_pdsch_init_ue(srslte_pdsch_t *q, uint32_t max_prb)
+{
+  return pdsch_init_multi(q, max_prb, 1, true);
+}
+
+int srslte_pdsch_init_enb(srslte_pdsch_t *q, uint32_t max_prb)
+{
+  return pdsch_init_multi(q, max_prb, 1, false);
+}
+
+int srslte_pdsch_init_multi_ue(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
+{
+  return pdsch_init_multi(q, max_prb, nof_rx_antennas, true);
+}
+
+int srslte_pdsch_init_multi_enb(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
+{
+  return pdsch_init_multi(q, max_prb, nof_rx_antennas, false);
+}
+
+
 void srslte_pdsch_free(srslte_pdsch_t *q) {
   int i;
 
@@ -292,7 +314,7 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
   if (q->d) {
     free(q->d);
   }
-  for (i = 0; i < q->cell.nof_ports; i++) {
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->x[i]) {
       free(q->x[i]);
     }
@@ -308,13 +330,20 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     }          
   }
   if (q->users) {
-    for (uint16_t u=0;u<SRSLTE_SIRNTI;u++) {
-      if (q->users[u]) {
-        srslte_pdsch_free_rnti(q, u);
+    if (q->is_ue) {
+      srslte_pdsch_free_rnti(q, 0);
+    } else {
+      for (uint16_t u=0;u<SRSLTE_SIRNTI;u++) {
+        if (q->users[u]) {
+          srslte_pdsch_free_rnti(q, u);
+        }
       }
-    }      
+    }
     free(q->users);
   }
+
+  srslte_sequence_free(&q->tmp_seq);
+
   for (i = 0; i < 4; i++) {
     srslte_modem_table_free(&q->mod[i]);
   }
@@ -325,6 +354,24 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
 
 }
 
+int srslte_pdsch_set_cell(srslte_pdsch_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL                  &&
+      srslte_cell_isvalid(&cell))
+  {
+    memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+    q->max_re = q->cell.nof_prb * MAX_PDSCH_RE(q->cell.cp);
+
+    INFO("PDSCH: Cell config PCI=%d, %d ports, %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
+         q->cell.id, q->cell.nof_prb, q->max_re);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 /* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg. 
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
@@ -353,17 +400,19 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  */
 int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
-  uint32_t i;  
-  if (!q->users[rnti]) {
-    q->users[rnti] = calloc(1, sizeof(srslte_pdsch_user_t));
-    if (q->users[rnti]) {
+  uint32_t i;
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  if (!q->users[rnti_idx]) {
+    q->users[rnti_idx] = calloc(1, sizeof(srslte_pdsch_user_t));
+    if (q->users[rnti_idx]) {
       for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pdsch(&q->users[rnti]->seq[i], rnti, 0, 2 * i, q->cell.id,
+        if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[i], rnti, 0, 2 * i, q->cell.id,
             q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
           return SRSLTE_ERROR; 
         }
       }
-      q->users[rnti]->sequence_generated = true;
+      q->users[rnti_idx]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
@@ -371,12 +420,13 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
 
 void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
 {
-  if (q->users[rnti]) {
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+  if (q->users[rnti_idx]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      srslte_sequence_free(&q->users[rnti]->seq[i]);
+      srslte_sequence_free(&q->users[rnti_idx]->seq[i]);
     }
-    free(q->users[rnti]);
-    q->users[rnti] = NULL; 
+    free(q->users[rnti_idx]);
+    q->users[rnti_idx] = NULL;
   }
 }
 
@@ -395,6 +445,17 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
   return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, data); 
 }
 
+static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
+{
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated) {
+    return &q->users[rnti_idx]->seq[sf_idx];
+  } else {
+    srslte_sequence_pdsch(&q->tmp_seq, rnti, 0, 2 * sf_idx, q->cell.id, len);
+    return &q->tmp_seq;
+  }
+}
+
 /** Decodes the PDSCH from the received symbols
  */
 int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
@@ -412,7 +473,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       data         != NULL && 
       cfg          != NULL)
   {
-    
+
     INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d, C_prb=%d\n",
         cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re, 
          cfg->nbits.nof_bits, cfg->rv, cfg->grant.nof_prb);
@@ -467,25 +528,19 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
     * thus we don't need tot set it in the LLRs normalization
     */
     srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
-    
-    /* descramble */
-    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_s_offset(&q->users[rnti]->seq[cfg->sf_idx], q->e, 0, cfg->nbits.nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_s_offset(&seq, q->e, 0, cfg->nbits.nof_bits);
-      srslte_sequence_free(&seq);
-    }
+
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+
+    // Run scrambling
+    srslte_scrambling_s_offset(seq, 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);      
+    return srslte_dlsch_decode(&q->dl_sch, cfg, softbuffer, q->e, data);
     
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
@@ -538,18 +593,11 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
       return SRSLTE_ERROR;
     }
 
-    /* scramble */
-    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits.nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
-      srslte_sequence_free(&seq);
-    }
-    
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+
+    srslte_scrambling_bytes(seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
+
     srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e, q->d, cfg->nbits.nof_bits);
     
     /* TODO: only diversity supported */
diff --git a/lib/src/phy/phch/phich.c b/lib/src/phy/phch/phich.c
index 7de2f5d37..7e7f0e8c8 100644
--- a/lib/src/phy/phch/phich.c
+++ b/lib/src/phy/phch/phich.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/phy/phch/regs.h"
 #include "srslte/phy/phch/phich.h"
@@ -67,37 +68,27 @@ void srslte_phich_reset(srslte_phich_t *q, cf_t *slot_symbols[SRSLTE_MAX_PORTS])
   }
 }
 
-int srslte_phich_init(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell) 
+int srslte_phich_init(srslte_phich_t *q)
 {
-  return srslte_phich_init_multi(q, regs, cell, 1);
+  return srslte_phich_init_multi(q, 1);
 }
 
 /** Initializes the phich channel receiver */
-int srslte_phich_init_multi(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell, uint32_t nof_rx_antennas) 
+int srslte_phich_init_multi(srslte_phich_t *q, uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
-  if (q         != NULL &&
-      regs      != NULL &&
-      srslte_cell_isvalid(&cell)) 
+  if (q != NULL)
   {
 
     bzero(q, sizeof(srslte_phich_t));
     ret = SRSLTE_ERROR;
     
-    q->cell = cell;
-    q->regs = regs;
-    q->nof_rx_antennas = nof_rx_antennas; 
+    q->nof_rx_antennas = nof_rx_antennas;
     
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_BPSK)) {
       goto clean;
     }
-
-    for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
-      if (srslte_sequence_phich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
-        goto clean;
-      }
-    }
     ret = SRSLTE_SUCCESS;
   }
   clean: 
@@ -114,9 +105,34 @@ void srslte_phich_free(srslte_phich_t *q) {
   srslte_modem_table_free(&q->mod);
 
   bzero(q, sizeof(srslte_phich_t));
+}
+
+int srslte_phich_set_cell(srslte_phich_t *q, srslte_regs_t *regs, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
+  if (q         != NULL &&
+      regs      != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+
+    q->regs = regs;
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      for (int nsf = 0; nsf < SRSLTE_NSUBFRAMES_X_FRAME; nsf++) {
+        if (srslte_sequence_phich(&q->seq[nsf], 2 * nsf, q->cell.id)) {
+          return SRSLTE_ERROR;
+        }
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
 }
 
+
+
 /* Computes n_group and n_seq according to Section 9.1.2 in 36.213 */
 void srslte_phich_calc(srslte_phich_t *q, uint32_t n_prb_lowest, uint32_t n_dmrs, 
                        uint32_t *ngroup, uint32_t *nseq) 
diff --git a/lib/src/phy/phch/prach.c b/lib/src/phy/phch/prach.c
index 5a2169fd9..f2ddd48db 100644
--- a/lib/src/phy/phch/prach.c
+++ b/lib/src/phy/phch/prach.c
@@ -26,6 +26,7 @@
 
 #include <math.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/phch/prach.h"
@@ -61,6 +62,8 @@ uint32_t prach_Tseq[5] = {24576, 24576, 2*24576, 2*24576, 4096};
 // Table 5.7.2-2 - N_cs values for unrestricted sets
 uint32_t prach_Ncs_unrestricted[16] = {0,13,15,18,22,26,32,38,46,59,76,93,119,167,279,419};
 
+#define MAX_N_zc 839
+
 // Table 5.7.2-2 - N_cs values for restricted sets
 uint32_t prach_Ncs_restricted[15] = {15,18,22,26,32,38,46,55,68,82,100,128,158,202,237};
 
@@ -328,7 +331,7 @@ int srslte_prach_gen_seqs(srslte_prach_t *p)
 
 int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t nof_prb)
 {
-  return srslte_prach_init(p, 
+  return srslte_prach_set_cell(p,
                            srslte_symbol_sz(nof_prb), 
                            cfg->config_idx, 
                            cfg->root_seq_idx,
@@ -336,28 +339,95 @@ int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t n
                            cfg->zero_corr_zone);
 }
 
-int srslte_prach_init(srslte_prach_t *p,
-               uint32_t N_ifft_ul,
-               uint32_t config_idx,
-               uint32_t root_seq_index,
-               bool high_speed_flag,
-               uint32_t zero_corr_zone_config)
+int srslte_prach_init(srslte_prach_t *p, uint32_t max_N_ifft_ul)
+{
+  int ret = SRSLTE_ERROR;
+  if(p                      != NULL      &&
+     max_N_ifft_ul          <  2049)
+  {
+    bzero(p, sizeof(srslte_prach_t));
+
+    p->max_N_ifft_ul = max_N_ifft_ul;
+
+    // Set up containers
+    p->prach_bins = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->corr_spec = srslte_vec_malloc(sizeof(cf_t)*MAX_N_zc);
+    p->corr = srslte_vec_malloc(sizeof(float)*MAX_N_zc);
+
+    // Set up ZC FFTS
+    if(srslte_dft_plan(&p->zc_fft, MAX_N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+      return SRSLTE_ERROR;
+    }
+    srslte_dft_plan_set_mirror(&p->zc_fft, false);
+    srslte_dft_plan_set_norm(&p->zc_fft, true);
+
+    if(srslte_dft_plan(&p->zc_ifft, MAX_N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)){
+      return SRSLTE_ERROR;
+    }
+    srslte_dft_plan_set_mirror(&p->zc_ifft, false);
+    srslte_dft_plan_set_norm(&p->zc_ifft, false);
+
+    uint32_t fft_size_alloc = max_N_ifft_ul * DELTA_F/DELTA_F_RA;
+
+    p->ifft_in = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    p->ifft_out = (cf_t*)srslte_vec_malloc(fft_size_alloc*sizeof(cf_t));
+    if(srslte_dft_plan(&p->ifft, fft_size_alloc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+      fprintf(stderr, "Error creating DFT plan\n");
+      return -1;
+    }
+    srslte_dft_plan_set_mirror(&p->ifft, true);
+    srslte_dft_plan_set_norm(&p->ifft, true);
+
+    if(srslte_dft_plan(&p->fft, fft_size_alloc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+      fprintf(stderr, "Error creating DFT plan\n");
+      return -1;
+    }
+    
+    p->signal_fft = srslte_vec_malloc(sizeof(cf_t)*fft_size_alloc);
+    if (!p->signal_fft) {
+      fprintf(stderr, "Error allocating memory\n");
+      return -1; 
+    }
+    
+    srslte_dft_plan_set_mirror(&p->fft, true);
+    srslte_dft_plan_set_norm(&p->fft, false);
+
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid parameters\n");
+  }
+
+  return ret;
+}
+
+
+int srslte_prach_set_cell(srslte_prach_t *p,
+                      uint32_t N_ifft_ul,
+                      uint32_t config_idx,
+                      uint32_t root_seq_index,
+                      bool high_speed_flag,
+                      uint32_t zero_corr_zone_config)
 {
   int ret = SRSLTE_ERROR;
   if(p                      != NULL      &&
      N_ifft_ul              <  2049      &&
-     config_idx             <  64        && 
+     config_idx             <  64        &&
      root_seq_index         <  MAX_ROOTS)
   {
+    if (N_ifft_ul > p->max_N_ifft_ul) {
+      fprintf(stderr, "PRACH: Error in set_cell(): N_ifft_ul must be lower or equal max_N_ifft_ul in init()\n");
+      return -1;
+    }
+
     uint32_t preamble_format = srslte_prach_get_preamble_format(config_idx);
-    p->config_idx = config_idx; 
+    p->config_idx = config_idx;
     p->f = preamble_format;
     p->rsi = root_seq_index;
     p->hs = high_speed_flag;
     p->zczc = zero_corr_zone_config;
-    p->detect_factor = PRACH_DETECT_FACTOR; 
-    
-    
+    p->detect_factor = PRACH_DETECT_FACTOR;
+
+
     // Determine N_zc and N_cs
     if(4 == preamble_format){
       if (p->zczc < 7) {
@@ -368,43 +438,33 @@ int srslte_prach_init(srslte_prach_t *p,
         return SRSLTE_ERROR;
       }
     }else{
-      p->N_zc = 839;
+      p->N_zc = MAX_N_zc;
       if(p->hs){
         if (p->zczc < 15) {
           p->N_cs = prach_Ncs_restricted[p->zczc];
         } else {
           fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d for restricted set\n", p->zczc);
           return SRSLTE_ERROR;
-        }   
+        }
       }else{
         if (p->zczc < 16) {
           p->N_cs = prach_Ncs_unrestricted[p->zczc];
         } else {
           fprintf(stderr, "Invalid zeroCorrelationZoneConfig=%d\n", p->zczc);
           return SRSLTE_ERROR;
-        }   
+        }
       }
     }
-    
-    // Set up containers
-    p->prach_bins = srslte_vec_malloc(sizeof(cf_t)*p->N_zc);
-    p->corr_spec = srslte_vec_malloc(sizeof(cf_t)*p->N_zc);
-    p->corr = srslte_vec_malloc(sizeof(float)*p->N_zc);
 
     // Set up ZC FFTS
-    p->zc_fft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->zc_fft, p->N_zc, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
-      return SRSLTE_ERROR;
-    }
-    srslte_dft_plan_set_mirror(p->zc_fft, false);
-    srslte_dft_plan_set_norm(p->zc_fft, true);
-
-    p->zc_ifft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->zc_ifft, p->N_zc, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)){
-      return SRSLTE_ERROR;
+    if (p->N_zc != MAX_N_zc) {
+      if(srslte_dft_replan(&p->zc_fft, p->N_zc)){
+        return SRSLTE_ERROR;
+      }
+      if(srslte_dft_replan(&p->zc_ifft, p->N_zc)){
+        return SRSLTE_ERROR;
+      }
     }
-    srslte_dft_plan_set_mirror(p->zc_ifft, false);
-    srslte_dft_plan_set_norm(p->zc_ifft, false);
 
     // Generate our 64 sequences
     p->N_roots = 0;
@@ -412,9 +472,9 @@ int srslte_prach_init(srslte_prach_t *p,
 
     // Generate sequence FFTs
     for(int i=0;i<N_SEQS;i++){
-      srslte_dft_run(p->zc_fft, p->seqs[i], p->dft_seqs[i]);
+      srslte_dft_run(&p->zc_fft, p->seqs[i], p->dft_seqs[i]);
     }
-    
+
     // Create our FFT objects and buffers
     p->N_ifft_ul = N_ifft_ul;
     if(4 == preamble_format){
@@ -422,47 +482,31 @@ int srslte_prach_init(srslte_prach_t *p,
     }else{
       p->N_ifft_prach = p->N_ifft_ul * DELTA_F/DELTA_F_RA;
     }
-    
+
     /* The deadzone specifies the number of samples at the end of the correlation window
      * that will be considered as belonging to the next preamble
      */
-    p->deadzone = 0; 
+    p->deadzone = 0;
     /*
     if(p->N_cs != 0) {
       float samp_rate=15000*p->N_ifft_ul;
       p->deadzone = (uint32_t) ceil((float) samp_rate/((float) p->N_zc*subcarrier_spacing));
     }*/
 
-    p->ifft_in = (cf_t*)srslte_vec_malloc(p->N_ifft_prach*sizeof(cf_t));
-    p->ifft_out = (cf_t*)srslte_vec_malloc(p->N_ifft_prach*sizeof(cf_t));
-    p->ifft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->ifft, p->N_ifft_prach, SRSLTE_DFT_BACKWARD, SRSLTE_DFT_COMPLEX)) {
+    if(srslte_dft_replan(&p->ifft, p->N_ifft_prach)) {
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    srslte_dft_plan_set_mirror(p->ifft, true);
-    srslte_dft_plan_set_norm(p->ifft, true);
-
-    p->fft = (srslte_dft_plan_t*)srslte_vec_malloc(sizeof(srslte_dft_plan_t));
-    if(srslte_dft_plan(p->fft, p->N_ifft_prach, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)){
+    if(srslte_dft_replan(&p->fft, p->N_ifft_prach)){
       fprintf(stderr, "Error creating DFT plan\n");
       return -1;
     }
-    
-    p->signal_fft = srslte_vec_malloc(sizeof(cf_t)*p->N_ifft_prach);
-    if (!p->signal_fft) {
-      fprintf(stderr, "Error allocating memory\n");
-      return -1; 
-    }
-    
-    srslte_dft_plan_set_mirror(p->fft, true);
-    srslte_dft_plan_set_norm(p->fft, false);
 
     p->N_seq = prach_Tseq[p->f]*p->N_ifft_ul/2048;
     p->N_cp  = prach_Tcp[p->f]*p->N_ifft_ul/2048;
     p->T_seq = prach_Tseq[p->f]*SRSLTE_LTE_TS;
     p->T_tot = (prach_Tseq[p->f]+prach_Tcp[p->f])*SRSLTE_LTE_TS;
-    
+
     ret = SRSLTE_SUCCESS;
   } else {
     fprintf(stderr, "Invalid parameters\n");
@@ -500,7 +544,7 @@ int srslte_prach_gen(srslte_prach_t *p,
     memcpy(&p->ifft_in[begin], p->dft_seqs[seq_index], p->N_zc * sizeof(cf_t));
     memset(&p->ifft_in[begin+p->N_zc], 0, (p->N_ifft_prach - begin - p->N_zc) * sizeof(cf_t));
 
-    srslte_dft_run(p->ifft, p->ifft_in, p->ifft_out);
+    srslte_dft_run(&p->ifft, p->ifft_in, p->ifft_out);
 
     // Copy CP into buffer
     memcpy(signal, &p->ifft_out[p->N_ifft_prach-p->N_cp], p->N_cp*sizeof(cf_t));
@@ -552,7 +596,7 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
     }
     
     // FFT incoming signal
-    srslte_dft_run(p->fft, signal, p->signal_fft);
+    srslte_dft_run(&p->fft, signal, p->signal_fft);
 
     *n_indices = 0;
 
@@ -569,7 +613,7 @@ int srslte_prach_detect_offset(srslte_prach_t *p,
      
       srslte_vec_prod_conj_ccc(p->prach_bins, root_spec, p->corr_spec, p->N_zc);
 
-      srslte_dft_run(p->zc_ifft, p->corr_spec, p->corr_spec);
+      srslte_dft_run(&p->zc_ifft, p->corr_spec, p->corr_spec);
       
       srslte_vec_abs_square_cf(p->corr_spec, p->corr, p->N_zc);
 
@@ -632,16 +676,12 @@ int srslte_prach_free(srslte_prach_t *p) {
   free(p->prach_bins);
   free(p->corr_spec);
   free(p->corr);
-  srslte_dft_plan_free(p->ifft);
-  free(p->ifft);
+  srslte_dft_plan_free(&p->ifft);
   free(p->ifft_in);
   free(p->ifft_out);
-  srslte_dft_plan_free(p->fft);
-  free(p->fft);
-  srslte_dft_plan_free(p->zc_fft);
-  free(p->zc_fft);
-  srslte_dft_plan_free(p->zc_ifft);
-  free(p->zc_ifft);
+  srslte_dft_plan_free(&p->fft);
+  srslte_dft_plan_free(&p->zc_fft);
+  srslte_dft_plan_free(&p->zc_ifft);
 
   if (p->signal_fft) {
     free(p->signal_fft); 
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 6a889b89c..4bc834ab9 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -417,43 +417,30 @@ void srslte_pucch_set_threshold(srslte_pucch_t *q, float format1, float format1a
 }
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pucch_init(srslte_pucch_t *q, srslte_cell_t cell) {
+int srslte_pucch_init(srslte_pucch_t *q) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  if (q != NULL && srslte_cell_isvalid(&cell)) {
+  if (q != NULL) {
     ret = SRSLTE_ERROR;
     bzero(q, sizeof(srslte_pucch_t));
     
-    q->cell = cell; 
-    
-    srslte_pucch_cfg_default(&q->pucch_cfg);
-    
     if (srslte_modem_table_lte(&q->mod, SRSLTE_MOD_QPSK)) {
       return SRSLTE_ERROR;
     }
-    
-    // Precompute group hopping values u. 
-    if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
-      return SRSLTE_ERROR;
-    }
-    
-    if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
-      return SRSLTE_ERROR;
-    }
-    
+
     q->users = calloc(sizeof(srslte_pucch_user_t*), 1+SRSLTE_SIRNTI);
     if (!q->users) {
       perror("malloc");
-      return SRSLTE_ERROR;
+      goto clean_exit;
     }
     
     srslte_uci_cqi_pucch_init(&q->cqi);
-    
-    q->z = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
-    q->z_tmp = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
-    q->ce = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
 
     ret = SRSLTE_SUCCESS;
   }
+clean_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_pucch_free(q);
+  }
   return ret;
 }
 
@@ -464,20 +451,36 @@ void srslte_pucch_free(srslte_pucch_t *q) {
     }
     free(q->users);
   }
-  if (q->z) {
-    free(q->z);
-  }
-  if (q->z_tmp) {
-    free(q->z_tmp);
-  }
-  if (q->ce) {
-    free(q->ce);
-  }
-  
+
   srslte_modem_table_free(&q->mod);
   bzero(q, sizeof(srslte_pucch_t));
 }
 
+int srslte_pucch_set_cell(srslte_pucch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q != NULL && srslte_cell_isvalid(&cell)) {
+
+    srslte_pucch_cfg_default(&q->pucch_cfg);
+
+    if (cell.id != q->cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      // Precompute group hopping values u.
+      if (srslte_group_hopping_f_gh(q->f_gh, q->cell.id)) {
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pucch_n_cs_cell(q->cell, q->n_cs_cell)) {
+        return SRSLTE_ERROR;
+      }
+    }
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
 void srslte_pucch_clear_rnti(srslte_pucch_t *q, uint16_t rnti) {
   if (q->users[rnti]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
@@ -496,6 +499,7 @@ int srslte_pucch_set_crnti(srslte_pucch_t *q, uint16_t rnti) {
         // Precompute scrambling sequence for pucch format 2    
         if (srslte_sequence_pucch(&q->users[rnti]->seq_f2[sf_idx], rnti, 2*sf_idx, q->cell.id)) {
           fprintf(stderr, "Error computing PUCCH Format 2 scrambling sequence\n");
+          srslte_pucch_clear_rnti(q, rnti);
           return SRSLTE_ERROR; 
         }        
       }
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 3fbb7627e..c3ce03edc 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -32,6 +32,8 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pusch.h>
 
 #include "srslte/phy/ch_estimation/refsignal_ul.h"
 #include "srslte/phy/phch/pusch.h"
@@ -185,22 +187,18 @@ int pusch_get(srslte_pusch_t *q, srslte_ra_ul_grant_t *grant, cf_t *input, cf_t
 
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
+int pusch_init(srslte_pusch_t *q, uint32_t max_prb, bool is_ue) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   int i;
 
- if (q                         != NULL                  &&
-     srslte_cell_isvalid(&cell)) 
+ if (q != NULL)
   {   
     
     bzero(q, sizeof(srslte_pusch_t));
     ret = SRSLTE_ERROR;
-    
-    q->cell = cell;
-    q->max_re = q->cell.nof_prb * MAX_PUSCH_RE(q->cell.cp);
+    q->max_re = max_prb * MAX_PUSCH_RE(SRSLTE_CP_NORM);
 
-    INFO("Init PUSCH: %d ports %d PRBs, max_symbols: %d\n", q->cell.nof_ports,
-        q->cell.nof_prb, q->max_re);
+    INFO("Init PUSCH: %d PRBs\n", max_prb);
 
     for (i = 0; i < 4; i++) {
       if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
@@ -208,26 +206,26 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
       }
       srslte_modem_table_bytes(&q->mod[i]);
     }
-    
-    q->users = calloc(sizeof(srslte_pusch_user_t*), 1+SRSLTE_SIRNTI);
+
+    q->is_ue = is_ue;
+
+    q->users = calloc(sizeof(srslte_pusch_user_t*), q->is_ue?1:(1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean;
     }
 
-    /* Precompute sequence for type2 frequency hopping */
-    if (srslte_sequence_LTE_pr(&q->seq_type2_fo, 210, q->cell.id)) {
-      fprintf(stderr, "Error initiating type2 frequency hopping sequence\n");
-      goto clean; 
+    if (srslte_sequence_init(&q->tmp_seq, q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+      goto clean;
     }
 
     srslte_sch_init(&q->ul_sch);
-    
-    if (srslte_dft_precoding_init(&q->dft_precoding, cell.nof_prb)) {
+
+    if (srslte_dft_precoding_init(&q->dft_precoding, max_prb, is_ue)) {
       fprintf(stderr, "Error initiating DFT transform precoding\n");
       goto clean; 
     }
-    
+
     // Allocate int16 for reception (LLRs). Buffer casted to uint8_t for transmission
     q->q = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
     if (!q->q) {
@@ -244,9 +242,11 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
       goto clean; 
     }
 
-    q->ce = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-    if (!q->ce) {
-      goto clean;
+    if (!q->is_ue) {
+      q->ce = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->ce) {
+        goto clean;
+      }
     }
     q->z = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
     if (!q->z) {
@@ -262,6 +262,14 @@ int srslte_pusch_init(srslte_pusch_t *q, srslte_cell_t cell) {
   return ret;
 }
 
+int srslte_pusch_init_ue(srslte_pusch_t *q, uint32_t max_prb) {
+  return pusch_init(q, max_prb, true);
+}
+
+int srslte_pusch_init_enb(srslte_pusch_t *q, uint32_t max_prb) {
+  return pusch_init(q, max_prb, false);
+}
+
 void srslte_pusch_free(srslte_pusch_t *q) {
   int i;
 
@@ -284,13 +292,20 @@ void srslte_pusch_free(srslte_pusch_t *q) {
   srslte_dft_precoding_free(&q->dft_precoding);
 
   if (q->users) {
-    for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {      
-      srslte_pusch_clear_rnti(q, rnti);
+    if (q->is_ue) {
+      srslte_pusch_clear_rnti(q, 0);
+    } else {
+      for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {
+        srslte_pusch_clear_rnti(q, rnti);
+      }
     }
     free(q->users);
   }
+
   srslte_sequence_free(&q->seq_type2_fo);
   
+  srslte_sequence_free(&q->tmp_seq);
+  
   for (i = 0; i < 4; i++) {
     srslte_modem_table_free(&q->mod[i]);
   }
@@ -300,6 +315,33 @@ void srslte_pusch_free(srslte_pusch_t *q) {
 
 }
 
+int srslte_pusch_set_cell(srslte_pusch_t *q, srslte_cell_t cell) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell))
+  {
+
+    q->max_re = cell.nof_prb * MAX_PUSCH_RE(q->cell.cp);
+
+    INFO("PUSCH: Cell config PCI=5d, %d ports %d PRBs, max_symbols: %d\n",
+         q->cell.id, q->cell.nof_ports, q->cell.nof_prb, q->max_re);
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      /* Precompute sequence for type2 frequency hopping */
+      if (srslte_sequence_LTE_pr(&q->seq_type2_fo, 210, q->cell.id)) {
+        fprintf(stderr, "Error initiating type2 frequency hopping sequence\n");
+        return SRSLTE_ERROR;
+      }
+
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+
+
 /* Configures the structure srslte_pusch_cfg_t from the UL DCI allocation dci_msg. 
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
@@ -391,29 +433,48 @@ int srslte_pusch_cfg(srslte_pusch_t             *q,
  * For the connection procedure, use srslte_pusch_encode() functions */
 int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
   uint32_t i;
+
+  uint32_t rnti_idx = q->is_ue?0:rnti; 
   
-  if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_pusch_user_t));
-    if (q->users[rnti]) {
+  if (!q->users[rnti_idx]) {
+    q->users[rnti_idx] = calloc(1, sizeof(srslte_pusch_user_t));
+    if (q->users[rnti_idx]) {
       for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pusch(&q->users[rnti]->seq[i], rnti, 2 * i, q->cell.id,
+        if (srslte_sequence_pusch(&q->users[rnti_idx]->seq[i], rnti, 2 * i, q->cell.id,
             q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          return SRSLTE_ERROR; 
+          fprintf(stderr, "Error initializing PUSCH scrambling sequence\n");
+          srslte_pusch_clear_rnti(q, rnti);
+          return SRSLTE_ERROR;
         }
       }
-      q->users[rnti]->sequences_generated = true;
+      q->users[rnti_idx]->sequence_generated = true;
     }
   }
   return SRSLTE_SUCCESS;
 }
 
 void srslte_pusch_clear_rnti(srslte_pusch_t *q, uint16_t rnti) {
-  if (q->users[rnti]) {
+
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+
+  if (q->users[rnti_idx]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      srslte_sequence_free(&q->users[rnti]->seq[i]);
+      srslte_sequence_free(&q->users[rnti_idx]->seq[i]);
     }    
-    free(q->users[rnti]);
-    q->users[rnti] = NULL; 
+    free(q->users[rnti_idx]);
+    q->users[rnti_idx] = NULL;
+  }
+}
+
+
+static srslte_sequence_t *get_user_sequence(srslte_pusch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
+{
+  uint32_t rnti_idx = q->is_ue?0:rnti;
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated) {
+    return &q->users[rnti_idx]->seq[sf_idx];
+  } else {
+    srslte_sequence_pusch(&q->tmp_seq, rnti, 2 * sf_idx, q->cell.id, len);
+    return &q->tmp_seq;
   }
 }
 
@@ -445,17 +506,12 @@ int srslte_pusch_encode(srslte_pusch_t *q, srslte_pusch_cfg_t *cfg, srslte_softb
       return SRSLTE_ERROR;
     }
 
-    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[cfg->sf_idx], (uint8_t*) q->q, cfg->nbits.nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pusch(&seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
-      srslte_sequence_free(&seq);
-    }
-    
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+
+    // Run scrambling
+    srslte_scrambling_bytes(seq, (uint8_t*) q->q, cfg->nbits.nof_bits);
+
     // Correct UCI placeholder/repetition bits    
     uint8_t *d = q->q; 
     for (int i = 0; i < q->ul_sch.nof_ri_ack_bits; i++) {     
@@ -528,23 +584,14 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     srslte_predecoding_single(q->d, q->ce, q->z, cfg->nbits.nof_re, noise_estimate);
     
     // DFT predecoding
-    srslte_dft_predecoding(&q->dft_precoding, q->z, q->d, cfg->grant.L_prb, cfg->nbits.nof_symb);
+    srslte_dft_precoding(&q->dft_precoding, q->z, q->d, cfg->grant.L_prb, cfg->nbits.nof_symb);
     
     // Soft demodulation
     srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->q, cfg->nbits.nof_re);
 
-    srslte_sequence_t *seq = NULL;
+    // Generate scrambling sequence if not pre-generated
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
 
-    // Create sequence if does not exist
-    if (q->users[rnti] && q->users[rnti]->sequences_generated) {
-      seq = &q->users[rnti]->seq[cfg->sf_idx];
-    } else {
-      seq = &q->tmp_seq;
-      if (srslte_sequence_pusch(seq, rnti, 2 * cfg->sf_idx, q->cell.id, cfg->nbits.nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-    }
-    
     // Decode RI/HARQ bits before descrambling 
     if (srslte_ulsch_uci_decode_ri_ack(&q->ul_sch, cfg, softbuffer, q->q, seq->c, uci_data)) {
       fprintf(stderr, "Error decoding RI/HARQ bits\n");
@@ -554,11 +601,7 @@ int srslte_pusch_decode(srslte_pusch_t *q,
     // Descrambling
     srslte_scrambling_s_offset(seq, q->q, 0, cfg->nbits.nof_bits);
         
-    if (!(q->users[rnti] && q->users[rnti]->sequences_generated)) {
-      srslte_sequence_free(seq);
-    }
-    
-    return srslte_ulsch_uci_decode(&q->ul_sch, cfg, softbuffer, q->q, q->g, data, uci_data);      
+    return srslte_ulsch_uci_decode(&q->ul_sch, cfg, softbuffer, q->q, q->g, data, uci_data);
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index dceab2506..b2c008e6c 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -455,7 +455,7 @@ static int decode_tb(srslte_sch_t *q,
       e_bits       != NULL &&
       cb_segm      != NULL)
   {
-    
+
     if (cb_segm->tbs == 0 || cb_segm->C == 0) {
       return SRSLTE_SUCCESS;
     }
@@ -476,8 +476,8 @@ static int decode_tb(srslte_sch_t *q,
     
     data[cb_segm->tbs/8+0] = 0; 
     data[cb_segm->tbs/8+1] = 0; 
-    data[cb_segm->tbs/8+2] = 0; 
-    
+    data[cb_segm->tbs/8+2] = 0;
+
     // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_NPAR
     for (uint32_t i=0;i<nof_cb_groups && crc_ok;i++) {
       crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i);            
@@ -519,7 +519,7 @@ int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
 {
   return decode_tb(q,                    
                    softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits, 
+                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits,
                    e_bits, data);
 }
 
@@ -538,7 +538,7 @@ int srslte_dlsch_encode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
 {
   return encode_tb(q, 
                    softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits, 
+                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits,
                    data, e_bits);
 }
 
diff --git a/lib/src/phy/phch/sequences.c b/lib/src/phy/phch/sequences.c
index 2816709b4..23ac410c4 100644
--- a/lib/src/phy/phch/sequences.c
+++ b/lib/src/phy/phch/sequences.c
@@ -33,7 +33,6 @@
  * 36.211 6.6.1
  */
 int srslte_sequence_pbch(srslte_sequence_t *seq, srslte_cp_t cp, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, SRSLTE_CP_ISNORM(cp)?1920:1728, cell_id);
 }
 
@@ -41,7 +40,6 @@ int srslte_sequence_pbch(srslte_sequence_t *seq, srslte_cp_t cp, uint32_t cell_i
  * 36.211 6.7.1
  */
 int srslte_sequence_pcfich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 32, (nslot/2+1) * (2*cell_id + 1) * 512 + cell_id);
 }
 
@@ -50,7 +48,6 @@ int srslte_sequence_pcfich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell
  * 36.211 6.9.1
  */
 int srslte_sequence_phich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 12, (nslot/2+1) * (2*cell_id + 1) * 512 + cell_id);
 }
 
@@ -58,7 +55,6 @@ int srslte_sequence_phich(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_
  * 36.211 6.8.2
  */
 int srslte_sequence_pdcch(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (nslot/2) * 512 + cell_id);
 }
 
@@ -66,7 +62,6 @@ int srslte_sequence_pdcch(srslte_sequence_t *seq, uint32_t nslot, uint32_t cell_
  * 36.211 6.3.1
  */
 int srslte_sequence_pdsch(srslte_sequence_t *seq, uint16_t rnti, int q, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (rnti<<14) + (q<<13) + ((nslot/2)<<9) + cell_id);
 }
 
@@ -74,7 +69,6 @@ int srslte_sequence_pdsch(srslte_sequence_t *seq, uint16_t rnti, int q, uint32_t
  * 36.211 5.3.1
  */
 int srslte_sequence_pusch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot, uint32_t cell_id, uint32_t len) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, len, (rnti<<14) + ((nslot/2)<<9) + cell_id);
 }
 
@@ -82,6 +76,5 @@ int srslte_sequence_pusch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot,
  * 36.211 5.4.2
  */
 int srslte_sequence_pucch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot, uint32_t cell_id) {
-  bzero(seq, sizeof(srslte_sequence_t));
   return srslte_sequence_LTE_pr(seq, 20, ((((nslot/2)+1)*(2*cell_id+1))<<16)+rnti);
 }
diff --git a/lib/src/phy/phch/test/pbch_file_test.c b/lib/src/phy/phch/test/pbch_file_test.c
index f3e4b9b08..734640d55 100644
--- a/lib/src/phy/phch/test/pbch_file_test.c
+++ b/lib/src/phy/phch/test/pbch_file_test.c
@@ -131,7 +131,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -141,7 +145,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error initiating PBCH\n");
+    return -1;
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error initiating PBCH\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/pbch_test.c b/lib/src/phy/phch/test/pbch_test.c
index c86e6f48a..a8c673714 100644
--- a/lib/src/phy/phch/test/pbch_test.c
+++ b/lib/src/phy/phch/test/pbch_test.c
@@ -104,7 +104,11 @@ int main(int argc, char **argv) {
     }
 
   }
-  if (srslte_pbch_init(&pbch, cell)) {
+  if (srslte_pbch_init(&pbch)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
+  if (srslte_pbch_set_cell(&pbch, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
diff --git a/lib/src/phy/phch/test/pcfich_file_test.c b/lib/src/phy/phch/test/pcfich_file_test.c
index 27b50baeb..bd1bc87c2 100644
--- a/lib/src/phy/phch/test/pcfich_file_test.c
+++ b/lib/src/phy/phch/test/pcfich_file_test.c
@@ -142,7 +142,11 @@ int base_init() {
     }
   }
   
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -157,7 +161,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+  if (srslte_pcfich_init(&pcfich)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    return -1;
+  }
+  if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/pcfich_test.c b/lib/src/phy/phch/test/pcfich_test.c
index 5d90b645a..d9217586f 100644
--- a/lib/src/phy/phch/test/pcfich_test.c
+++ b/lib/src/phy/phch/test/pcfich_test.c
@@ -124,7 +124,11 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 
-    if (srslte_pcfich_init(&pcfich, &regs, cell)) {
+    if (srslte_pcfich_init(&pcfich)) {
+      fprintf(stderr, "Error creating PBCH object\n");
+      exit(-1);
+    }
+    if (srslte_pcfich_set_cell(&pcfich, &regs, cell)) {
       fprintf(stderr, "Error creating PBCH object\n");
       exit(-1);
     }
diff --git a/lib/src/phy/phch/test/pdcch_file_test.c b/lib/src/phy/phch/test/pdcch_file_test.c
index b505e6b68..4d61e529a 100644
--- a/lib/src/phy/phch/test/pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdcch_file_test.c
@@ -148,7 +148,11 @@ int base_init() {
     }
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -167,11 +171,15 @@ int base_init() {
     fprintf(stderr, "Error setting CFI %d\n", cfi);
     return -1;
   }
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
-  
+  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+
   DEBUG("Memory init OK\n",0);
   return 0;
 }
diff --git a/lib/src/phy/phch/test/pdcch_test.c b/lib/src/phy/phch/test/pdcch_test.c
index 5d4fb0a7c..6b8ee1731 100644
--- a/lib/src/phy/phch/test/pdcch_test.c
+++ b/lib/src/phy/phch/test/pdcch_test.c
@@ -177,7 +177,11 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDCCH object\n");
+    exit(-1);
+  }
+  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
diff --git a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
index 0d19d689e..d6bd10018 100644
--- a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
@@ -137,11 +137,15 @@ int base_init() {
     exit(-1);
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, 1)) {
+  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, 1)) {
     fprintf(stderr, "Error initializing UE DL\n");
     return -1;
   }
-  
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initializing UE DL\n");
+    return -1;
+  }
+
   srslte_ue_dl_set_rnti(&ue_dl, rnti); 
 
   DEBUG("Memory init OK\n",0);
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index d4e5163d3..34150f4db 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -177,11 +177,15 @@ int main(int argc, char **argv) {
     goto quit;
   }
 
-  if (srslte_pdsch_init(&pdsch, cell)) {
+  if (srslte_pdsch_init_ue(&pdsch, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
-  
+  if (srslte_pdsch_set_cell(&pdsch, cell)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    goto quit;
+  }
+
   srslte_pdsch_set_rnti(&pdsch, rnti);
   
   if (srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb)) {
@@ -202,11 +206,15 @@ int main(int argc, char **argv) {
 #endif
     
     srslte_chest_dl_t chest; 
-    if (srslte_chest_dl_init(&chest, cell)) {
+    if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+      printf("Error initializing equalizer\n");
+      exit(-1);
+    }
+    if (srslte_chest_dl_set_cell(&chest, cell)) {
       printf("Error initializing equalizer\n");
       exit(-1);
     }
-    srslte_chest_dl_estimate(&chest, slot_symbols[0], ce, subframe);    
+    srslte_chest_dl_estimate(&chest, slot_symbols[0], ce, subframe);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
diff --git a/lib/src/phy/phch/test/phich_file_test.c b/lib/src/phy/phch/test/phich_file_test.c
index f037a55da..27e9bc14e 100644
--- a/lib/src/phy/phch/test/phich_file_test.c
+++ b/lib/src/phy/phch/test/phich_file_test.c
@@ -29,6 +29,7 @@
 #include <string.h>
 #include <strings.h>
 #include <unistd.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/srslte.h"
 
@@ -165,7 +166,11 @@ int base_init() {
     }
   }
 
-  if (srslte_chest_dl_init(&chest, cell)) {
+  if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
+    fprintf(stderr, "Error initializing equalizer\n");
+    return -1;
+  }
+  if (srslte_chest_dl_set_cell(&chest, cell)) {
     fprintf(stderr, "Error initializing equalizer\n");
     return -1;
   }
@@ -180,7 +185,11 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_phich_init(&phich, &regs, cell)) {
+  if (srslte_phich_init(&phich)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    return -1;
+  }
+  if (srslte_phich_set_cell(&phich, &regs, cell)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/phich_test.c b/lib/src/phy/phch/test/phich_test.c
index d1c6ef964..1269e02bd 100644
--- a/lib/src/phy/phch/test/phich_test.c
+++ b/lib/src/phy/phch/test/phich_test.c
@@ -142,6 +142,10 @@ int main(int argc, char **argv) {
     cid = cell.id;
     max_cid = cell.id;
   }
+  if (srslte_phich_init(&phich)) {
+    fprintf(stderr, "Error creating PBCH object\n");
+    exit(-1);
+  }
   while(cid <= max_cid) {
     cell.id = cid;
     
@@ -152,7 +156,7 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 
-    if (srslte_phich_init(&phich, &regs, cell)) {
+    if (srslte_phich_set_cell(&phich, &regs, cell)) {
       fprintf(stderr, "Error creating PBCH object\n");
       exit(-1);
     }
@@ -198,10 +202,10 @@ int main(int argc, char **argv) {
         }
       }
     }
-    srslte_phich_free(&phich);
     srslte_regs_free(&regs);
     cid++;
   }
+  srslte_phich_free(&phich);
 
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
     free(ce[i]);
diff --git a/lib/src/phy/phch/test/prach_test.c b/lib/src/phy/phch/test/prach_test.c
index ddd05eba0..9d09aaf1d 100644
--- a/lib/src/phy/phch/test/prach_test.c
+++ b/lib/src/phy/phch/test/prach_test.c
@@ -85,12 +85,14 @@ int main(int argc, char **argv) {
   cf_t preamble[MAX_LEN];
   memset(preamble, 0, sizeof(cf_t)*MAX_LEN);
 
-  srslte_prach_init(p,
-             N_ifft_ul,
-             config_idx,
-             root_seq_idx,
-             high_speed_flag,
-             zero_corr_zone);
+  srslte_prach_init(p, N_ifft_ul);
+
+  srslte_prach_set_cell( p,
+                         N_ifft_ul,
+                         config_idx,
+                         root_seq_idx,
+                         high_speed_flag,
+                         zero_corr_zone);
 
   uint32_t seq_index = 0;
   uint32_t frequency_offset = 0;
diff --git a/lib/src/phy/phch/test/prach_test_multi.c b/lib/src/phy/phch/test/prach_test_multi.c
index 9e3dc1c97..58f6d6af8 100644
--- a/lib/src/phy/phch/test/prach_test_multi.c
+++ b/lib/src/phy/phch/test/prach_test_multi.c
@@ -90,12 +90,14 @@ int main(int argc, char **argv) {
   cf_t preamble_sum[MAX_LEN];
   memset(preamble_sum, 0, sizeof(cf_t)*MAX_LEN);
 
-  srslte_prach_init(p,
-             N_ifft_ul,
-             preamble_format,
-             root_seq_idx,
-             high_speed_flag,
-             zero_corr_zone);
+  srslte_prach_init(p, N_ifft_ul);
+
+  srslte_prach_set_cell( p,
+                         N_ifft_ul,
+                         preamble_format,
+                         root_seq_idx,
+                         high_speed_flag,
+                         zero_corr_zone);
 
   uint32_t seq_index = 0;
   uint32_t frequency_offset = 0;
diff --git a/lib/src/phy/phch/test/prach_test_usrp.c b/lib/src/phy/phch/test/prach_test_usrp.c
index f5f1a9e5c..2defdec4f 100644
--- a/lib/src/phy/phch/test/prach_test_usrp.c
+++ b/lib/src/phy/phch/test/prach_test_usrp.c
@@ -133,6 +133,9 @@ int main(int argc, char **argv) {
   memset(preamble, 0, sizeof(cf_t)*MAX_LEN);
 
   srslte_prach_init(p,
+                    srslte_symbol_sz(nof_prb));
+
+  srslte_prach_set_cell(p,
                     srslte_symbol_sz(nof_prb),
                     preamble_format,
                     root_seq_idx,
diff --git a/lib/src/phy/phch/test/pucch_test.c b/lib/src/phy/phch/test/pucch_test.c
index 316a5efc4..43aaca953 100644
--- a/lib/src/phy/phch/test/pucch_test.c
+++ b/lib/src/phy/phch/test/pucch_test.c
@@ -87,15 +87,23 @@ int main(int argc, char **argv) {
   
   parse_args(argc,argv);
 
-  if (srslte_pucch_init(&pucch, cell)) {
+  if (srslte_pucch_init(&pucch)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  if (srslte_refsignal_ul_init(&dmrs, cell)) {
+  if (srslte_pucch_set_cell(&pucch, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
-  
+  if (srslte_refsignal_ul_init(&dmrs, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+  if (srslte_refsignal_ul_set_cell(&dmrs, cell)) {
+    fprintf(stderr, "Error creating PDSCH object\n");
+    exit(-1);
+  }
+
   bzero(&pucch_cfg, sizeof(srslte_pucch_cfg_t));
   
   for (int i=0;i<SRSLTE_PUCCH_MAX_BITS;i++) {
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index 0cb2847b7..991764bc0 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -110,7 +110,8 @@ void parse_args(int argc, char **argv) {
 }
 
 int main(int argc, char **argv) {
-  srslte_pusch_t pusch;
+  srslte_pusch_t pusch_tx;
+  srslte_pusch_t pusch_rx;
   uint8_t *data = NULL;
   cf_t *sf_symbols = NULL;
   cf_t *ce = NULL; 
@@ -145,11 +146,23 @@ int main(int argc, char **argv) {
   ul_hopping.hopping_offset = 0;
   ul_hopping.hop_mode = SRSLTE_PUSCH_HOP_MODE_INTER_SF;
   
-  if (srslte_pusch_init(&pusch, cell)) {
-    fprintf(stderr, "Error creating PDSCH object\n");
+  if (srslte_pusch_init_ue(&pusch_tx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
     goto quit;
   }
-  
+  if (srslte_pusch_set_cell(&pusch_tx, cell)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+  if (srslte_pusch_init_enb(&pusch_rx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+  if (srslte_pusch_set_cell(&pusch_rx, cell)) {
+    fprintf(stderr, "Error creating PUSCH object\n");
+    goto quit;
+  }
+
   /* Configure PUSCH */
     
   printf("Encoding rv_idx=%d\n",rv_idx);
@@ -159,14 +172,19 @@ int main(int argc, char **argv) {
   uci_cfg.I_offset_ri = 2; 
   uci_cfg.I_offset_ack = 4; 
 
-  if (srslte_pusch_cfg(&pusch, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
+  if (srslte_pusch_cfg(&pusch_tx, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     exit(-1);
   }
-  
+  if (srslte_pusch_cfg(&pusch_rx, &cfg, &grant, &uci_cfg, &ul_hopping, NULL, subframe, 0, 0)) {
+    fprintf(stderr, "Error configuring PDSCH\n");
+    exit(-1);
+  }
+
   uint16_t rnti = 1234; 
-  srslte_pusch_set_rnti(&pusch, rnti);
-  
+  srslte_pusch_set_rnti(&pusch_tx, rnti);
+  srslte_pusch_set_rnti(&pusch_rx, rnti);
+
   srslte_uci_data_t uci_data_tx; 
   srslte_uci_data_t uci_data_rx; 
   bzero(&uci_data_tx, sizeof(srslte_uci_data_t));
@@ -211,13 +229,13 @@ int main(int argc, char **argv) {
   
   uint32_t ntrials = 100; 
 
-  if (srslte_pusch_encode(&pusch, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
+  if (srslte_pusch_encode(&pusch_tx, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
     fprintf(stderr, "Error encoding TB\n");
     exit(-1);
   }
   if (rv_idx > 0) {
     cfg.rv = rv_idx; 
-    if (srslte_pusch_encode(&pusch, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
+    if (srslte_pusch_encode(&pusch_tx, &cfg, &softbuffer_tx, data, uci_data_tx, rnti, sf_symbols)) {
       fprintf(stderr, "Error encoding TB\n");
       exit(-1);
     }
@@ -233,7 +251,7 @@ int main(int argc, char **argv) {
   }
   
   gettimeofday(&t[1], NULL);
-  int r = srslte_pusch_decode(&pusch, &cfg, &softbuffer_rx, sf_symbols, ce, 0, rnti, data, &uci_data_rx);
+  int r = srslte_pusch_decode(&pusch_rx, &cfg, &softbuffer_rx, sf_symbols, ce, 0, rnti, data, &uci_data_rx);
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
   if (r) {
@@ -265,7 +283,8 @@ int main(int argc, char **argv) {
   }
 
 quit:
-  srslte_pusch_free(&pusch);
+  srslte_pusch_free(&pusch_tx);
+  srslte_pusch_free(&pusch_rx);
   srslte_softbuffer_tx_free(&softbuffer_tx);
   
   if (sf_symbols) {
diff --git a/lib/src/phy/resampling/interp.c b/lib/src/phy/resampling/interp.c
index 553caf4b7..180b3f9fc 100644
--- a/lib/src/phy/resampling/interp.c
+++ b/lib/src/phy/resampling/interp.c
@@ -27,7 +27,8 @@
 #include <complex.h>
 #include <math.h>
 #include <stdlib.h>
-#include <strings.h> 
+#include <strings.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/resampling/interp.h"
 #include "srslte/phy/utils/vector.h"
@@ -108,11 +109,23 @@ int srslte_interp_linear_vector_init(srslte_interp_linsrslte_vec_t *q, uint32_t
       perror("malloc");
       return SRSLTE_ERROR; 
     }    
-    q->vector_len = vector_len; 
+    q->vector_len     = vector_len;
+    q->max_vector_len = vector_len;
   }
   return ret; 
 }
 
+int srslte_interp_linear_vector_resize(srslte_interp_linsrslte_vec_t *q, uint32_t vector_len)
+{
+  if (vector_len <= q->max_vector_len) {
+    q->vector_len = vector_len;
+    return SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Error resizing interp_linear: vector_len must be lower or equal than initialized\n");
+    return SRSLTE_ERROR;
+  }
+}
+
 void srslte_interp_linear_vector_free(srslte_interp_linsrslte_vec_t *q) {
   if (q->diff_vec) {
     free(q->diff_vec);
@@ -189,7 +202,9 @@ int srslte_interp_linear_init(srslte_interp_lin_t *q, uint32_t vector_len, uint3
     }
 
     q->vector_len = vector_len; 
-    q->M = M; 
+    q->M = M;
+    q->max_vector_len = vector_len;
+    q->max_M = M;
   }
   return ret; 
 }
@@ -209,6 +224,24 @@ void srslte_interp_linear_free(srslte_interp_lin_t *q) {
 
 }
 
+
+int srslte_interp_linear_resize(srslte_interp_lin_t *q, uint32_t vector_len, uint32_t M)
+{
+  if (vector_len <= q->max_vector_len && M <= q->max_M) {
+
+    for (int i=0;i<M;i++) {
+      q->ramp[i] = (float) i;
+    }
+
+    q->vector_len = vector_len;
+    q->M = M;
+    return SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Error resizing interp_linear: vector_len and M must be lower or equal than initialized\n");
+    return SRSLTE_ERROR;
+  }
+}
+
 void srslte_interp_linear_offset(srslte_interp_lin_t *q, cf_t *input, cf_t *output, 
                           uint32_t off_st, uint32_t off_end) 
 {
diff --git a/lib/src/phy/rf/rf_utils.c b/lib/src/phy/rf/rf_utils.c
index f4a2c6790..ce288b5c5 100644
--- a/lib/src/phy/rf/rf_utils.c
+++ b/lib/src/phy/rf/rf_utils.c
@@ -103,11 +103,16 @@ int rf_mib_decoder(srslte_rf_t *rf, uint32_t nof_rx_antennas,cell_search_cfg_t *
   srslte_ue_mib_sync_t ue_mib; 
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
 
-  if (srslte_ue_mib_sync_init_multi(&ue_mib, cell->id, cell->cp, srslte_rf_recv_wrapper_cs, nof_rx_antennas, (void*) rf)) {
+  if (srslte_ue_mib_sync_init_multi(&ue_mib, srslte_rf_recv_wrapper_cs, nof_rx_antennas, (void*) rf)) {
     fprintf(stderr, "Error initiating srslte_ue_mib_sync\n");
     goto clean_exit; 
   }
-  
+
+  if (srslte_ue_mib_sync_set_cell(&ue_mib, cell->id, cell->cp)) {
+    fprintf(stderr, "Error initiating srslte_ue_mib_sync\n");
+    goto clean_exit;
+  }
+
   if (config->init_agc > 0) {
     srslte_ue_sync_start_agc(&ue_mib.ue_sync, srslte_rf_set_rx_gain_th_wrapper, config->init_agc);    
   }
@@ -168,7 +173,6 @@ int rf_cell_search(srslte_rf_t *rf, uint32_t nof_rx_antennas,
     fprintf(stderr, "Error initiating UE cell detect\n");
     return SRSLTE_ERROR; 
   }
-  
   if (config->nof_valid_pss_frames) {
     srslte_ue_cellsearch_set_nof_valid_frames(&cs, config->nof_valid_pss_frames);
   }
diff --git a/lib/src/phy/scrambling/scrambling.c b/lib/src/phy/scrambling/scrambling.c
index bb5637814..42f16d1e8 100644
--- a/lib/src/phy/scrambling/scrambling.c
+++ b/lib/src/phy/scrambling/scrambling.c
@@ -33,29 +33,29 @@
 #include "srslte/phy/scrambling/scrambling.h"
 
 void srslte_scrambling_f(srslte_sequence_t *s, float *data) {
-  srslte_scrambling_f_offset(s, data, 0, s->len);
+  srslte_scrambling_f_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_f_offset(srslte_sequence_t *s, float *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_fff(data, &s->c_float[offset], data, len);
 }
 
 void srslte_scrambling_s(srslte_sequence_t *s, short *data) {
-  srslte_scrambling_s_offset(s, data, 0, s->len);
+  srslte_scrambling_s_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_s_offset(srslte_sequence_t *s, short *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_sss(data, &s->c_short[offset], data, len);
 }
 
 void srslte_scrambling_c(srslte_sequence_t *s, cf_t *data) {
-  srslte_scrambling_c_offset(s, data, 0, s->len);
+  srslte_scrambling_c_offset(s, data, 0, s->cur_len);
 }
 
 void srslte_scrambling_c_offset(srslte_sequence_t *s, cf_t *data, int offset, int len) {
-  assert (len + offset <= s->len);
+  assert (len + offset <= s->cur_len);
   srslte_vec_prod_cfc(data, &s->c_float[offset], data, len);
 }
 
@@ -81,7 +81,7 @@ void scrambling_b_word(uint8_t *c, uint8_t *data, int len) {
 
 
 void srslte_scrambling_b(srslte_sequence_t *s, uint8_t *data) {
-  scrambling_b_word(s->c, data, s->len);  
+  scrambling_b_word(s->c, data, s->cur_len);  
 }
 
 void srslte_scrambling_b_offset(srslte_sequence_t *s, uint8_t *data, int offset, int len) {
diff --git a/lib/src/phy/scrambling/test/scrambling_test.c b/lib/src/phy/scrambling/test/scrambling_test.c
index 9ff47dc85..40722f09b 100644
--- a/lib/src/phy/scrambling/test/scrambling_test.c
+++ b/lib/src/phy/scrambling/test/scrambling_test.c
@@ -84,8 +84,10 @@ void parse_args(int argc, char **argv) {
 
 int init_sequence(srslte_sequence_t *seq, char *name) {
   if (!strcmp(name, "PBCH")) {
+    bzero(seq, sizeof(srslte_sequence_t));
     return srslte_sequence_pbch(seq, cp, cell_id);
   } else if (!strcmp(name, "PDSCH")) {
+    bzero(seq, sizeof(srslte_sequence_t));
     return srslte_sequence_pdsch(seq, 1234, 0, 0, cell_id, nof_bits);
   } else {
     fprintf(stderr, "Unsupported sequence name %s\n", name);
@@ -109,18 +111,18 @@ int main(int argc, char **argv) {
   }
 
   if (!do_floats) {
-    input_b = malloc(sizeof(uint8_t) * seq.len);
+    input_b = malloc(sizeof(uint8_t) * seq.cur_len);
     if (!input_b) {
       perror("malloc");
       exit(-1);
     }
-    scrambled_b = malloc(sizeof(uint8_t) * seq.len);
+    scrambled_b = malloc(sizeof(uint8_t) * seq.cur_len);
     if (!scrambled_b) {
       perror("malloc");
       exit(-1);
     }
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       input_b[i] = rand()%2;
       scrambled_b[i] = input_b[i];
     }
@@ -131,9 +133,9 @@ int main(int argc, char **argv) {
     srslte_scrambling_b(&seq, scrambled_b);
 
     get_time_interval(t);
-    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.len);
+    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.cur_len);
     
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       if (scrambled_b[i] != input_b[i]) {
         printf("Error in %d\n", i);
         exit(-1);
@@ -142,18 +144,18 @@ int main(int argc, char **argv) {
     free(input_b);
     free(scrambled_b);
   } else {
-    input_f = malloc(sizeof(float) * seq.len);
+    input_f = malloc(sizeof(float) * seq.cur_len);
     if (!input_f) {
       perror("malloc");
       exit(-1);
     }
-    scrambled_f = malloc(sizeof(float) * seq.len);
+    scrambled_f = malloc(sizeof(float) * seq.cur_len);
     if (!scrambled_f) {
       perror("malloc");
       exit(-1);
     }
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       input_f[i] = 100*(rand()/RAND_MAX);
       scrambled_f[i] = input_f[i];
     }
@@ -164,9 +166,9 @@ int main(int argc, char **argv) {
     srslte_scrambling_f(&seq, scrambled_f);
 
     get_time_interval(t);
-    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.len);
+    printf("Texec=%ld us for %d bits\n", t[0].tv_usec, seq.cur_len);
 
-    for (i=0;i<seq.len;i++) {
+    for (i=0;i<seq.cur_len;i++) {
       if (scrambled_f[i] != input_f[i]) {
         printf("Error in %d\n", i);
         exit(-1);
diff --git a/lib/src/phy/sync/cfo.c b/lib/src/phy/sync/cfo.c
index 456015164..34498ce58 100644
--- a/lib/src/phy/sync/cfo.c
+++ b/lib/src/phy/sync/cfo.c
@@ -27,6 +27,7 @@
 #include <strings.h>
 #include <stdlib.h>
 #include <math.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/utils/cexptab.h"
 #include "srslte/phy/sync/cfo.h"
@@ -47,6 +48,7 @@ int srslte_cfo_init(srslte_cfo_t *h, uint32_t nsamples) {
   h->tol = SRSLTE_CFO_TOLERANCE;
   h->last_freq = 0;
   h->nsamples = nsamples;
+  h->max_samples = nsamples;
   srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, h->nsamples);
 
   ret = SRSLTE_SUCCESS;
@@ -69,15 +71,15 @@ void srslte_cfo_set_tol(srslte_cfo_t *h, float tol) {
   h->tol = tol;
 }
 
-int srslte_cfo_realloc(srslte_cfo_t *h, uint32_t samples) {
-  h->cur_cexp = realloc(h->cur_cexp, sizeof(cf_t) * samples); 
-  if (!h->cur_cexp) {
-    perror("realloc");
+int srslte_cfo_resize(srslte_cfo_t *h, uint32_t samples) {
+  if (samples <= h->max_samples) {
+    srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, samples);
+    h->nsamples = samples;
+  } else {
+    fprintf(stderr, "Error in cfo_resize(): nof_samples must be lower than initialized\n");
     return SRSLTE_ERROR;
   }
-  srslte_cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, samples);
-  h->nsamples = samples;
-  
+
   return SRSLTE_SUCCESS;
 }
 
diff --git a/lib/src/phy/sync/cp.c b/lib/src/phy/sync/cp.c
index c745aca85..a282f6dfd 100644
--- a/lib/src/phy/sync/cp.c
+++ b/lib/src/phy/sync/cp.c
@@ -25,6 +25,7 @@
  */
 
 #include <stdlib.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/sync/cp.h"
 #include "srslte/phy/utils/vector.h"
@@ -33,7 +34,8 @@
 int srslte_cp_synch_init(srslte_cp_synch_t *q, uint32_t symbol_sz) 
 {
   q->symbol_sz = symbol_sz;
-  
+  q->max_symbol_sz = symbol_sz;
+
   q->corr = srslte_vec_malloc(sizeof(cf_t) * q->symbol_sz);
   if (!q->corr) {
     perror("malloc");
@@ -49,6 +51,18 @@ void srslte_cp_synch_free(srslte_cp_synch_t *q)
   }
 }
 
+int srslte_cp_synch_resize(srslte_cp_synch_t *q, uint32_t symbol_sz)
+{
+  if (symbol_sz > q->max_symbol_sz) {
+    fprintf(stderr, "Error in cp_synch_resize(): symbol_sz must be lower than initialized\n");
+    return SRSLTE_ERROR;
+  }
+  q->symbol_sz = symbol_sz;
+
+  return SRSLTE_SUCCESS;
+}
+
+
 uint32_t srslte_cp_synch(srslte_cp_synch_t *q, cf_t *input, uint32_t max_offset, uint32_t nof_symbols, uint32_t cp_len)
 {  
   if (max_offset > q->symbol_sz) {    
diff --git a/lib/src/phy/sync/pss.c b/lib/src/phy/sync/pss.c
index 2ad166bc0..3d221528c 100644
--- a/lib/src/phy/sync/pss.c
+++ b/lib/src/phy/sync/pss.c
@@ -93,7 +93,9 @@ int srslte_pss_synch_init_fft_offset(srslte_pss_synch_t *q, uint32_t frame_size,
  * It correlates a signal of frame_size samples with the PSS sequence in the frequency 
  * domain. The PSS sequence is transformed using fft_size samples. 
  */
-int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t fft_size, int offset, int decimate) {
+int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
+                                           uint32_t max_frame_size, uint32_t max_fft_size,
+                                           int offset, int decimate) {
 
     
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -107,18 +109,20 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame
     
     q->N_id_2 = 10;  
     q->ema_alpha = 0.2;
-    
+
+    q->max_fft_size  = max_fft_size;
+    q->max_frame_size = max_frame_size;
+
     q->decimate = decimate;
-    fft_size = fft_size/q->decimate;
-    frame_size = frame_size/q->decimate;
+    uint32_t fft_size = max_fft_size/q->decimate;
+    uint32_t frame_size = max_frame_size/q->decimate;
     
     q->fft_size = fft_size;
     q->frame_size = frame_size;
 
     buffer_size = fft_size + frame_size + 1;
     
-    if(q->decimate > 1)
-    {
+    if(q->decimate > 1) {
         int filter_order = 3;
         srslte_filt_decim_cc_init(&q->filter,q->decimate,filter_order);
         q->filter.filter_output = srslte_vec_malloc((buffer_size) * sizeof(cf_t));
@@ -175,7 +179,6 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q, uint32_t frame
         goto clean_and_exit;
       }      
       bzero(&q->pss_signal_time[N_id_2][q->fft_size], q->frame_size * sizeof(cf_t));
-
     }    
     #ifdef CONVOLUTION_FFT
 
@@ -208,6 +211,79 @@ clean_and_exit:
 }
 
 
+/* Initializes the PSS synchronization object.
+ *
+ * It correlates a signal of frame_size samples with the PSS sequence in the frequency
+ * domain. The PSS sequence is transformed using fft_size samples.
+ */
+int srslte_pss_synch_resize(srslte_pss_synch_t *q, uint32_t frame_size, uint32_t fft_size, int offset) {
+
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if (q != NULL) {
+
+    ret = SRSLTE_ERROR;
+
+    if (fft_size > q->max_fft_size || frame_size > q->max_frame_size) {
+      fprintf(stderr, "Error in pss_synch_config(): fft_size and frame_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+
+    uint32_t N_id_2;
+    uint32_t buffer_size;
+
+    q->N_id_2 = 10;
+    q->ema_alpha = 0.2;
+
+    fft_size = fft_size/q->decimate;
+    frame_size = frame_size/q->decimate;
+
+    q->fft_size = fft_size;
+    q->frame_size = frame_size;
+
+    buffer_size = fft_size + frame_size + 1;
+
+    if (srslte_dft_replan(&q->dftp_input, fft_size)) {
+      fprintf(stderr, "Error creating DFT plan \n");
+      return SRSLTE_ERROR;
+    }
+
+    bzero(&q->tmp_input[q->frame_size], q->fft_size * sizeof(cf_t));
+    bzero(q->conv_output, sizeof(cf_t) * buffer_size);
+    bzero(q->conv_output_avg, sizeof(float) * buffer_size);
+
+#ifdef SRSLTE_PSS_ACCUMULATE_ABS
+    bzero(q->conv_output_abs, sizeof(float) * buffer_size);
+#endif
+
+    // Generate PSS sequences for this FFT size
+    for (N_id_2=0;N_id_2<3;N_id_2++) {
+      if (srslte_pss_synch_init_N_id_2(q->pss_signal_freq[N_id_2], q->pss_signal_time[N_id_2], N_id_2, fft_size, offset)) {
+        fprintf(stderr, "Error initiating PSS detector for N_id_2=%d fft_size=%d\n", N_id_2, fft_size);
+        return SRSLTE_ERROR;
+      }
+      bzero(&q->pss_signal_time[N_id_2][q->fft_size], q->frame_size * sizeof(cf_t));
+    }
+#ifdef CONVOLUTION_FFT
+
+    if (srslte_conv_fft_cc_replan(&q->conv_fft, frame_size, fft_size)) {
+      fprintf(stderr, "Error initiating convolution FFT\n");
+      return SRSLTE_ERROR;
+    }
+    for(int i =0; i< 3; i++) {
+      srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
+    }
+
+#endif
+
+    srslte_pss_synch_reset(q);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+
+}
+
 void srslte_pss_synch_free(srslte_pss_synch_t *q) {
   uint32_t i;
 
diff --git a/lib/src/phy/sync/sss.c b/lib/src/phy/sync/sss.c
index 3586e04de..4ed7b1d6c 100644
--- a/lib/src/phy/sync/sss.c
+++ b/lib/src/phy/sync/sss.c
@@ -58,7 +58,8 @@ int srslte_sss_synch_init(srslte_sss_synch_t *q, uint32_t fft_size) {
     srslte_dft_plan_set_dc(&q->dftp_input, true);
 
     q->fft_size = fft_size; 
-    
+    q->max_fft_size = fft_size;
+
     generate_N_id_1_table(q->N_id_1_table);
     
     for (N_id_2=0;N_id_2<3;N_id_2++) {
@@ -71,19 +72,18 @@ int srslte_sss_synch_init(srslte_sss_synch_t *q, uint32_t fft_size) {
   return SRSLTE_ERROR_INVALID_INPUTS;
 }
 
-int srslte_sss_synch_realloc(srslte_sss_synch_t *q, uint32_t fft_size) {
+int srslte_sss_synch_resize(srslte_sss_synch_t *q, uint32_t fft_size) {
   if (q                 != NULL  &&
       fft_size          <= 2048)
   {
-    srslte_dft_plan_free(&q->dftp_input);
-    if (srslte_dft_plan(&q->dftp_input, fft_size, SRSLTE_DFT_FORWARD, SRSLTE_DFT_COMPLEX)) {
+    if (fft_size > q->max_fft_size) {
+      fprintf(stderr, "Error in sss_synch_resize(): fft_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_dft_replan(&q->dftp_input, fft_size)) {
       srslte_sss_synch_free(q);
       return SRSLTE_ERROR;
     }
-    srslte_dft_plan_set_mirror(&q->dftp_input, true);
-    srslte_dft_plan_set_norm(&q->dftp_input, true);
-    srslte_dft_plan_set_dc(&q->dftp_input, true);
-    
     q->fft_size = fft_size;
     return SRSLTE_SUCCESS;
   }
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index fb77e1b25..99d58a46f 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -28,6 +28,7 @@
 #include <strings.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/common/phy_common.h"
@@ -36,7 +37,7 @@
 #include "srslte/phy/sync/cfo.h"
 
 #define MEANPEAK_EMA_ALPHA      0.1
-#define CFO_EMA_ALPHA           0.1
+#define CFO_EMA_ALPHA           0.2
 #define CP_EMA_ALPHA            0.1
 
 static bool fft_size_isvalid(uint32_t fft_size) {
@@ -77,7 +78,8 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->frame_size = frame_size;
     q->max_offset = max_offset;
     q->sss_alg = SSS_FULL;
-        
+    q->max_frame_size = frame_size;
+
     q->enable_cfo_corr = true; 
     if (srslte_cfo_init(&q->cfocorr, q->frame_size)) {
       fprintf(stderr, "Error initiating CFO\n");
@@ -91,8 +93,6 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     
     // Set a CFO tolerance of approx 50 Hz
     srslte_cfo_set_tol(&q->cfocorr, 50.0/(15000.0*q->fft_size));
-
-    // Set a CFO tolerance of approx 50 Hz
     srslte_cfo_set_tol(&q->cfocorr2, 50.0/(15000.0*q->fft_size));
 
     for (int i=0;i<2;i++) {
@@ -113,8 +113,7 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->decimate = decimate;
     if(!decimate)
       decimate = 1;
-                 
-    
+
     if (srslte_pss_synch_init_fft_offset_decim(&q->pss, max_offset, fft_size,0,decimate)) {
       fprintf(stderr, "Error initializing PSS object\n");
       goto clean_exit;
@@ -162,6 +161,77 @@ void srslte_sync_free(srslte_sync_t *q) {
   }
 }
 
+int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offset, uint32_t fft_size) {
+
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                 != NULL         &&
+      frame_size        <= 307200       &&
+      fft_size_isvalid(fft_size))
+  {
+    ret = SRSLTE_ERROR;
+
+    if (frame_size > q->max_frame_size) {
+      fprintf(stderr, "Error in sync_resize(): frame_size must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+    q->detect_cp = true;
+    q->sss_en = true;
+    q->mean_cfo = 0;
+    q->mean_cfo2 = 0;
+    q->N_id_2 = 1000;
+    q->N_id_1 = 1000;
+    q->cfo_i = 0;
+    q->find_cfo_i = false;
+    q->find_cfo_i_initiated = false;
+    q->cfo_ema_alpha = CFO_EMA_ALPHA;
+    q->fft_size = fft_size;
+    q->frame_size = frame_size;
+    q->max_offset = max_offset;
+    q->sss_alg = SSS_FULL;
+
+    q->enable_cfo_corr = true;
+
+    if (srslte_pss_synch_resize(&q->pss, max_offset, fft_size, 0)) {
+      fprintf(stderr, "Error resizing PSS object\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_sss_synch_resize(&q->sss, fft_size)) {
+      fprintf(stderr, "Error resizing SSS object\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cp_synch_resize(&q->cp_synch, fft_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cfo_resize(&q->cfocorr, q->frame_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (srslte_cfo_resize(&q->cfocorr2, q->frame_size)) {
+      fprintf(stderr, "Error resizing CFO\n");
+      return SRSLTE_ERROR;
+    }
+
+    // Update CFO tolerance
+    srslte_cfo_set_tol(&q->cfocorr, 50.0/(15000.0*q->fft_size));
+    srslte_cfo_set_tol(&q->cfocorr2, 50.0/(15000.0*q->fft_size));
+
+
+    DEBUG("SYNC init with frame_size=%d, max_offset=%d and fft_size=%d\n", frame_size, max_offset, fft_size);
+
+    ret = SRSLTE_SUCCESS;
+  }  else {
+    fprintf(stderr, "Invalid parameters frame_size: %d, fft_size: %d\n", frame_size, fft_size);
+  }
+
+  return ret;
+}
+
+
 void srslte_sync_set_threshold(srslte_sync_t *q, float threshold) {
   q->threshold = threshold;
 }
@@ -451,7 +521,7 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
       q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
       
       /* Correct CFO with the averaged CFO estimation */
-      srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);      
+      srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);
       
       input_cfo = q->temp; 
     }
diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c
index f8c38a1bb..0e92e6dc1 100644
--- a/lib/src/phy/ue/ue_cell_search.c
+++ b/lib/src/phy/ue/ue_cell_search.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/ue/ue_cell_search.h"
 
@@ -51,11 +52,16 @@ int srslte_ue_cellsearch_init(srslte_ue_cellsearch_t * q, uint32_t max_frames,
     cell.id = SRSLTE_CELL_ID_UNKNOWN; 
     cell.nof_prb = SRSLTE_CS_NOF_PRB; 
 
-    if (srslte_ue_sync_init(&q->ue_sync, cell, recv_callback, stream_handler)) {
+    if (srslte_ue_sync_init(&q->ue_sync, cell.nof_prb, true, recv_callback, stream_handler)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       goto clean_exit; 
     }
-    
+
+    if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      goto clean_exit;
+    }
+
     q->sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
     q->nof_rx_antennas = 1; 
     
@@ -105,11 +111,15 @@ int srslte_ue_cellsearch_init_multi(srslte_ue_cellsearch_t * q, uint32_t max_fra
     cell.id = SRSLTE_CELL_ID_UNKNOWN; 
     cell.nof_prb = SRSLTE_CS_NOF_PRB; 
 
-    if (srslte_ue_sync_init_multi(&q->ue_sync, cell, recv_callback, nof_rx_antennas, stream_handler)) {
+    if (srslte_ue_sync_init_multi(&q->ue_sync, cell.nof_prb, true, recv_callback, nof_rx_antennas, stream_handler)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       goto clean_exit; 
     }
-    
+    if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
+      fprintf(stderr, "Error initiating ue_sync\n");
+      goto clean_exit;
+    }
+
     for (int i=0;i<nof_rx_antennas;i++) {
       q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
     }
@@ -242,7 +252,8 @@ int srslte_ue_cellsearch_scan(srslte_ue_cellsearch_t * q,
 {
   int ret = 0; 
   float max_peak_value = -1.0;
-  uint32_t nof_detected_cells = 0; 
+  uint32_t nof_detected_cells = 0;
+
   for (uint32_t N_id_2=0;N_id_2<3 && ret >= 0;N_id_2++) {
     ret = srslte_ue_cellsearch_scan_N_id_2(q, N_id_2, &found_cells[N_id_2]);
     if (ret < 0) {
@@ -273,8 +284,12 @@ int srslte_ue_cellsearch_scan_N_id_2(srslte_ue_cellsearch_t * q,
 
   if (q != NULL) 
   {
-    ret = SRSLTE_SUCCESS; 
-    
+    ret = SRSLTE_SUCCESS;
+
+    bzero(q->candidates, sizeof(srslte_ue_cellsearch_result_t)*q->max_frames);
+    bzero(q->mode_ntimes, sizeof(uint32_t)*q->max_frames);
+    bzero(q->mode_counted, sizeof(uint8_t)*q->max_frames);
+
     srslte_ue_sync_set_N_id_2(&q->ue_sync, N_id_2);
     srslte_ue_sync_reset(&q->ue_sync);
     do {
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 7201f96c2..ab3fa526e 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -37,6 +37,7 @@
 #define CURRENT_SLOTLEN_RE SRSLTE_SLOT_LEN_RE(q->cell.nof_prb, q->cell.cp)
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
+#define MAX_SFLEN_RE SRSLTE_SF_LEN_RE(max_prb, q->cell.cp)
 
 static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1}; // Only TM1 and TM2 are currently supported 
 const uint32_t nof_ue_formats = 2; 
@@ -45,80 +46,74 @@ static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FO
 const uint32_t nof_common_formats = 2; 
 
 int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                      srslte_cell_t cell) 
+                      uint32_t max_prb)
 {
-  return srslte_ue_dl_init_multi(q, cell, 1);
+  return srslte_ue_dl_init_multi(q, max_prb, 1);
 }
 
 int srslte_ue_dl_init_multi(srslte_ue_dl_t *q, 
-                            srslte_cell_t cell, 
+                            uint32_t max_prb,
                             uint32_t nof_rx_antennas) 
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
   if (q               != NULL             &&
-      nof_rx_antennas <= SRSLTE_MAX_PORTS &&
-      srslte_cell_isvalid(&cell))   
+      nof_rx_antennas <= SRSLTE_MAX_PORTS)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_ue_dl_t));
     
-    q->cell = cell; 
     q->pkt_errors = 0;
     q->pkts_total = 0;
     q->pending_ul_dci_rnti = 0; 
     q->sample_offset = 0; 
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
-    if (srslte_ofdm_rx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    q->nof_rx_antennas = nof_rx_antennas;
+
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
-    if (srslte_chest_dl_init(&q->chest, cell)) {
+    if (srslte_chest_dl_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit;
     }
-    if (srslte_regs_init(&q->regs, q->cell)) {
-      fprintf(stderr, "Error initiating REGs\n");
-      goto clean_exit;
-    }
-    if (srslte_pcfich_init_multi(&q->pcfich, &q->regs, q->cell, nof_rx_antennas)) {
+    if (srslte_pcfich_init_multi(&q->pcfich, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich, &q->regs, q->cell)) {
+    if (srslte_phich_init(&q->phich)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init_multi(&q->pdcch, &q->regs, q->cell, nof_rx_antennas)) {
+    if (srslte_pdcch_init_multi(&q->pdcch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_multi(&q->pdsch, q->cell, nof_rx_antennas)) {
+    if (srslte_pdsch_init_multi_ue(&q->pdsch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
-    if (srslte_softbuffer_rx_init(&q->softbuffer, q->cell.nof_prb)) {
+    if (srslte_softbuffer_rx_init(&q->softbuffer, max_prb)) {
       fprintf(stderr, "Error initiating soft buffer\n");
       goto clean_exit;
     }
-    if (srslte_cfo_init(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
+    if (srslte_cfo_init(&q->sfo_correct, max_prb*SRSLTE_NRE)) {
       fprintf(stderr, "Error initiating SFO correct\n");
       goto clean_exit;
     }
     srslte_cfo_set_tol(&q->sfo_correct, 1e-5/q->fft.symbol_sz);
     
     for (int j=0;j<nof_rx_antennas;j++) {
-      q->sf_symbols_m[j] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+      q->sf_symbols_m[j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
       if (!q->sf_symbols_m[j]) {
         perror("malloc");
         goto clean_exit; 
       }
-      for (uint32_t i=0;i<q->cell.nof_ports;i++) {
-        q->ce_m[i][j] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+      for (uint32_t i=0;i<SRSLTE_MAX_PORTS;i++) {
+        q->ce_m[i][j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
         if (!q->ce_m[i][j]) {
           perror("malloc");
           goto clean_exit; 
@@ -127,14 +122,13 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
     }
     
     q->sf_symbols = q->sf_symbols_m[0];
-    for (int i=0;i<q->cell.nof_ports;i++) {
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
       q->ce[i] = q->ce_m[i][0];
     }
     
     ret = SRSLTE_SUCCESS;
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parametres\n");
   }
 
 clean_exit: 
@@ -159,7 +153,7 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
       }
-      for (uint32_t i=0;i<q->cell.nof_ports;i++) {
+      for (uint32_t i=0;i<SRSLTE_MAX_PORTS;i++) {
         if (q->ce_m[i][j]) {
           free(q->ce_m[i][j]);
         }
@@ -169,6 +163,67 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
   }
 }
 
+int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q               != NULL             &&
+      srslte_cell_isvalid(&cell))
+  {
+    q->pkt_errors = 0;
+    q->pkts_total = 0;
+    q->pending_ul_dci_rnti = 0;
+    q->sample_offset = 0;
+
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      if (q->cell.nof_prb != 0) {
+        srslte_regs_free(&q->regs);
+      }
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+      if (srslte_regs_init(&q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing REGs\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_cfo_resize(&q->sfo_correct, q->cell.nof_prb*SRSLTE_NRE)) {
+        fprintf(stderr, "Error resizing SFO correct\n");
+        return SRSLTE_ERROR;
+      }
+      srslte_cfo_set_tol(&q->sfo_correct, 1e-5/q->fft.symbol_sz);
+      if (srslte_ofdm_rx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error resizing FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_chest_dl_set_cell(&q->chest, q->cell)) {
+        fprintf(stderr, "Error resizing channel estimator\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pcfich_set_cell(&q->pcfich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PCFICH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_phich_set_cell(&q->phich, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PHICH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdcch_set_cell(&q->pdcch, &q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing PDCCH object\n");
+        return SRSLTE_ERROR;
+      }
+
+      if (srslte_pdsch_set_cell(&q->pdsch, q->cell)) {
+        fprintf(stderr, "Error creating PDSCH object\n");
+        return SRSLTE_ERROR;
+      }
+    }
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
 /* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  * For the connection procedure, use srslte_pusch_encode_rnti() or srslte_pusch_decode_rnti() functions 
@@ -231,7 +286,7 @@ int srslte_ue_dl_decode_fft_estimate_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE
       /* Correct SFO multiplying by complex exponential in the time domain */
       if (q->sample_offset) {
         for (int i=0;i<2*SRSLTE_CP_NSYMB(q->cell.cp);i++) {
-          srslte_cfo_correct(&q->sfo_correct, 
+          srslte_cfo_correct(&q->sfo_correct,
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           q->sample_offset / q->fft.symbol_sz);
@@ -566,11 +621,11 @@ bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t n_pr
 }
 
 void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuffer, uint32_t tti, uint32_t rv_idx, uint16_t rnti, uint32_t cfi) {
-  srslte_vec_save_file("sf_symbols", q->sf_symbols_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+  srslte_vec_save_file("sf_symbols", q->sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   printf("%d samples\n", SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp));
-  srslte_vec_save_file("ce0", q->ce_m[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+  srslte_vec_save_file("ce0", q->ce[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   if (q->cell.nof_ports > 1) {
-    srslte_vec_save_file("ce1", q->ce_m[1], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+    srslte_vec_save_file("ce1", q->ce[1], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
   }
   srslte_vec_save_file("pcfich_ce0", q->pcfich.ce[0], q->pcfich.nof_symbols*sizeof(cf_t));
   srslte_vec_save_file("pcfich_ce1", q->pcfich.ce[1], q->pcfich.nof_symbols*sizeof(cf_t));
@@ -583,10 +638,11 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pdcch_symbols", q->pdcch.symbols[0], q->pdcch.nof_cce*36*sizeof(cf_t));
   srslte_vec_save_file("pdcch_eq_symbols", q->pdcch.d, q->pdcch.nof_cce*36*sizeof(cf_t));
   srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
-  
-  
-  srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits.nof_bits*sizeof(cf_t));
+
+
+  srslte_vec_save_file("pdsch_symbols", q->pdsch.symbols[0], q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
+  srslte_vec_save_file("pdsch_eq_symbols", q->pdsch.d, q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
+  srslte_vec_save_file("pdsch_llr", q->pdsch.e, q->pdsch_cfg.nbits.nof_bits*sizeof(cf_t));
   int cb_len = q->pdsch_cfg.cb_segm.K1; 
   for (int i=0;i<q->pdsch_cfg.cb_segm.C;i++) {
     char tmpstr[64]; 
diff --git a/lib/src/phy/ue/ue_mib.c b/lib/src/phy/ue/ue_mib.c
index 34c2bc2e3..7f68b9f23 100644
--- a/lib/src/phy/ue/ue_mib.c
+++ b/lib/src/phy/ue/ue_mib.c
@@ -36,45 +36,40 @@
 #include "srslte/phy/utils/vector.h"
 
 int srslte_ue_mib_init(srslte_ue_mib_t * q, 
-                       srslte_cell_t cell) 
+                       uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (q != NULL && 
-      cell.nof_ports <= SRSLTE_MAX_PORTS) 
+  if (q != NULL)
   {
 
     ret = SRSLTE_ERROR;    
     bzero(q, sizeof(srslte_ue_mib_t));
     
-    if (srslte_pbch_init(&q->pbch, cell)) {
+    if (srslte_pbch_init(&q->pbch)) {
       fprintf(stderr, "Error initiating PBCH\n");
       goto clean_exit;
     }
 
-    if (cell.nof_ports == 0) {
-      cell.nof_ports = SRSLTE_MAX_PORTS; 
-    }
-    
-    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit;
     }
     
-    for (int i=0;i<cell.nof_ports;i++) {
-      q->ce[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
+    for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
+      q->ce[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
       if (!q->ce[i]) {
         perror("malloc");
         goto clean_exit;
       }
     }
 
-    if (srslte_ofdm_rx_init(&q->fft, cell.cp, cell.nof_prb)) {
+    if (srslte_ofdm_rx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initializing FFT\n");
       goto clean_exit;
     }
-    if (srslte_chest_dl_init(&q->chest, cell)) {
+    if (srslte_chest_dl_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initializing reference signal\n");
       goto clean_exit;
     }
@@ -109,6 +104,38 @@ void srslte_ue_mib_free(srslte_ue_mib_t * q)
     
 }
 
+int srslte_ue_mib_set_cell(srslte_ue_mib_t * q,
+                           srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL &&
+      cell.nof_ports <= SRSLTE_MAX_PORTS)
+  {
+    if (srslte_pbch_set_cell(&q->pbch, cell)) {
+      fprintf(stderr, "Error initiating PBCH\n");
+      return SRSLTE_ERROR;
+    }
+    if (srslte_ofdm_rx_set_prb(&q->fft, cell.cp, cell.nof_prb)) {
+      fprintf(stderr, "Error initializing FFT\n");
+      return SRSLTE_ERROR;
+    }
+
+    if (cell.nof_ports == 0) {
+      cell.nof_ports = SRSLTE_MAX_PORTS;
+    }
+
+    if (srslte_chest_dl_set_cell(&q->chest, cell)) {
+      fprintf(stderr, "Error initializing reference signal\n");
+      return SRSLTE_ERROR;
+    }
+    srslte_ue_mib_reset(q);
+
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
 
 void srslte_ue_mib_reset(srslte_ue_mib_t * q)
 {
@@ -122,6 +149,8 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
   int ret = SRSLTE_SUCCESS;
   cf_t *ce_slot1[SRSLTE_MAX_PORTS]; 
 
+
+
   /* Run FFT for the slot symbols */
   srslte_ofdm_rx_sf(&q->fft, input, q->sf_symbols);
             
@@ -142,7 +171,7 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
 
   /* Decode PBCH */
   ret = srslte_pbch_decode(&q->pbch, &q->sf_symbols[SRSLTE_SLOT_LEN_RE(q->chest.cell.nof_prb, q->chest.cell.cp)], 
-                    ce_slot1, srslte_chest_dl_get_noise_estimate(&q->chest),
+                    ce_slot1, 0,
                     bch_payload, nof_tx_ports, sfn_offset);
   
 
@@ -161,27 +190,21 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
   return ret;
 }
 
-int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q, 
-                            uint32_t cell_id, 
-                            srslte_cp_t cp, 
-                            int (recv_callback)(void*, void*, uint32_t, srslte_timestamp_t*),                             
-                            void *stream_handler) 
+int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
+                                  int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),
+                                  uint32_t nof_rx_antennas,
+                                  void *stream_handler) 
 {
-  srslte_cell_t cell; 
-  // If the ports are set to 0, ue_mib goes through 1, 2 and 4 ports to blindly detect nof_ports
-  cell.nof_ports = 0;  
-  cell.id = cell_id; 
-  cell.cp = cp; 
-  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB; 
-  
-  q->sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-  q->nof_rx_antennas = 1;
+  for (int i=0;i<nof_rx_antennas;i++) {
+    q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(SRSLTE_UE_MIB_NOF_PRB));
+  }
+  q->nof_rx_antennas = nof_rx_antennas;
   
-  if (srslte_ue_mib_init(&q->ue_mib, cell)) {
+  if (srslte_ue_mib_init(&q->ue_mib, SRSLTE_UE_MIB_NOF_PRB)) {
     fprintf(stderr, "Error initiating ue_mib\n");
     return SRSLTE_ERROR;
   }
-  if (srslte_ue_sync_init(&q->ue_sync, cell, recv_callback, stream_handler)) {
+  if (srslte_ue_sync_init_multi(&q->ue_sync, SRSLTE_UE_MIB_NOF_PRB, false, recv_callback, nof_rx_antennas, stream_handler)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     srslte_ue_mib_free(&q->ue_mib);
     return SRSLTE_ERROR;
@@ -190,38 +213,30 @@ int srslte_ue_mib_sync_init(srslte_ue_mib_sync_t *q,
   return SRSLTE_SUCCESS;
 }
 
-int srslte_ue_mib_sync_init_multi(srslte_ue_mib_sync_t *q, 
-                                  uint32_t cell_id, 
-                                  srslte_cp_t cp, 
-                                  int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t, srslte_timestamp_t*),                             
-                                  uint32_t nof_rx_antennas,
-                                  void *stream_handler) 
+int srslte_ue_mib_sync_set_cell(srslte_ue_mib_sync_t *q,
+                                uint32_t cell_id,
+                                srslte_cp_t cp)
 {
-  srslte_cell_t cell; 
+  srslte_cell_t cell;
   // If the ports are set to 0, ue_mib goes through 1, 2 and 4 ports to blindly detect nof_ports
-  cell.nof_ports = 0;  
-  cell.id = cell_id; 
-  cell.cp = cp; 
-  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB; 
-  
-  for (int i=0;i<nof_rx_antennas;i++) {
-    q->sf_buffer[i] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-  }
-  q->nof_rx_antennas = nof_rx_antennas;
-  
-  if (srslte_ue_mib_init(&q->ue_mib, cell)) {
+  cell.nof_ports = 0;
+  cell.id = cell_id;
+  cell.cp = cp;
+  cell.nof_prb = SRSLTE_UE_MIB_NOF_PRB;
+
+  if (srslte_ue_mib_set_cell(&q->ue_mib, cell)) {
     fprintf(stderr, "Error initiating ue_mib\n");
     return SRSLTE_ERROR;
   }
-  if (srslte_ue_sync_init_multi(&q->ue_sync, cell, recv_callback, nof_rx_antennas, stream_handler)) {
+  if (srslte_ue_sync_set_cell(&q->ue_sync, cell)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     srslte_ue_mib_free(&q->ue_mib);
     return SRSLTE_ERROR;
   }
-  srslte_ue_sync_decode_sss_on_track(&q->ue_sync, true);
   return SRSLTE_SUCCESS;
 }
 
+
 void srslte_ue_mib_sync_free(srslte_ue_mib_sync_t *q) {
   for (int i=0;i<q->nof_rx_antennas;i++) {
     if (q->sf_buffer[i]) {
@@ -246,7 +261,9 @@ int srslte_ue_mib_sync_decode(srslte_ue_mib_sync_t * q,
   
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   uint32_t nof_frames = 0; 
-  int mib_ret = SRSLTE_UE_MIB_NOTFOUND; 
+  int mib_ret = SRSLTE_UE_MIB_NOTFOUND;
+
+  srslte_ue_mib_sync_reset(q);
 
   if (q != NULL) 
   {
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index b5b2be3d0..967b02d63 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -29,6 +29,7 @@
 #include <strings.h>
 #include <assert.h>
 #include <unistd.h>
+#include <srslte/srslte.h>
 
 
 #include "srslte/phy/ue/ue_sync.h"
@@ -122,39 +123,41 @@ int recv_callback_multi_to_single(void *h, cf_t *x[SRSLTE_MAX_PORTS], uint32_t n
 }
 
 int srslte_ue_sync_init(srslte_ue_sync_t *q, 
-                 srslte_cell_t cell,
-                 int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),                 
-                 void *stream_handler) 
+                        uint32_t max_prb,
+                        bool search_cell,
+                        int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),
+                        void *stream_handler)
 {
-  int ret = srslte_ue_sync_init_multi(q, cell, recv_callback_multi_to_single, 1, (void*) q);
+  int ret = srslte_ue_sync_init_multi(q, max_prb, search_cell, recv_callback_multi_to_single, 1, (void*) q);
   q->recv_callback_single = recv_callback;
   q->stream_single = stream_handler;
   return ret; 
 }
 
 int srslte_ue_sync_init_multi(srslte_ue_sync_t *q, 
-                              srslte_cell_t cell,
+                              uint32_t max_prb,
+                              bool search_cell,
                               int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
                               uint32_t nof_rx_antennas,
                               void *stream_handler) 
 
 {
     
-    return srslte_ue_sync_init_multi_decim(q,  cell,recv_callback ,nof_rx_antennas,stream_handler,1); 
+    return srslte_ue_sync_init_multi_decim(q, max_prb,search_cell, recv_callback ,nof_rx_antennas,stream_handler,1);
 }
 
 int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q, 
-                              srslte_cell_t cell,
-                              int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
-                              uint32_t nof_rx_antennas,
-                              void *stream_handler,
-                              int decimate) 
+                                    uint32_t max_prb,
+                                    bool search_cell,
+                                    int (recv_callback)(void*, cf_t*[SRSLTE_MAX_PORTS], uint32_t,srslte_timestamp_t*),
+                                    uint32_t nof_rx_antennas,
+                                    void *stream_handler,
+                                    int decimate)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q                                 != NULL && 
       stream_handler                    != NULL && 
-      srslte_nofprb_isvalid(cell.nof_prb)       &&
       nof_rx_antennas <= SRSLTE_MAX_PORTS       &&
       recv_callback                     != NULL)
   {
@@ -165,43 +168,39 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
     q->stream = stream_handler;
     q->recv_callback = recv_callback;
     q->nof_rx_antennas = nof_rx_antennas;
-    q->cell = cell;
-    q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
+    q->fft_size = srslte_symbol_sz(max_prb);
     q->sf_len = SRSLTE_SF_LEN(q->fft_size);
     q->file_mode = false; 
     q->correct_cfo = true; 
     q->agc_period = 0; 
     q->sample_offset_correct_period = DEFAULT_SAMPLE_OFFSET_CORRECT_PERIOD; 
     q->sfo_ema                      = DEFAULT_SFO_EMA_COEFF; 
-    
-    if (cell.id == 1000) {
+
+    q->max_prb = max_prb;
+
+    if (search_cell) {
       
       /* If the cell is unkown, we search PSS/SSS in 5 ms */
       q->nof_recv_sf = 5; 
 
-      q->decode_sss_on_track = true; 
-
     } else {
       
       /* If the cell is known, we work on a 1ms basis */
       q->nof_recv_sf = 1; 
 
-      q->decode_sss_on_track = true; 
     }
 
     q->frame_len = q->nof_recv_sf*q->sf_len;
     
-    if(q->fft_size < 700  && q->decimate)
-    {
+    if(q->fft_size < 700 && q->decimate) {
         q->decimate = 1;    
     }
-    
-  
+
     if(srslte_sync_init_decim(&q->sfind, q->frame_len, q->frame_len, q->fft_size,q->decimate)) {
       fprintf(stderr, "Error initiating sync find\n");
       goto clean_exit;
     }
-    if (cell.id == 1000) {
+    if (search_cell) {
       if(srslte_sync_init(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
         fprintf(stderr, "Error initiating sync track\n");
         goto clean_exit;
@@ -212,75 +211,139 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
         goto clean_exit;
       }
     }
-    
+
+    ret = SRSLTE_SUCCESS;
+  }
+  
+clean_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_ue_sync_free(q);
+  }
+  return ret; 
+}
+
+uint32_t srslte_ue_sync_sf_len(srslte_ue_sync_t *q) {
+  return q->frame_len;
+}
+
+void srslte_ue_sync_free(srslte_ue_sync_t *q) {
+  if (q->do_agc) {
+    srslte_agc_free(&q->agc);
+  }
+  if (!q->file_mode) {
+    srslte_sync_free(&q->sfind);
+    srslte_sync_free(&q->strack);    
+  } else {
+    srslte_filesource_free(&q->file_source);
+  }
+  bzero(q, sizeof(srslte_ue_sync_t));
+}
+
+
+int srslte_ue_sync_set_cell(srslte_ue_sync_t *q, srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q                                 != NULL &&
+      srslte_nofprb_isvalid(cell.nof_prb))
+  {
+    ret = SRSLTE_ERROR;
+
+    if (cell.nof_prb > q->max_prb) {
+      fprintf(stderr, "Error in ue_sync_set_cell(): cell.nof_prb must be lower than initialized\n");
+      return SRSLTE_ERROR;
+    }
+
+    memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+    q->fft_size = srslte_symbol_sz(q->cell.nof_prb);
+    q->sf_len = SRSLTE_SF_LEN(q->fft_size);
+    q->agc_period = 0;
+
     if (cell.id == 1000) {
-      /* If the cell id is unknown, enable CP detection on find */ 
+
+      /* If the cell is unkown, we search PSS/SSS in 5 ms */
+      q->nof_recv_sf = 5;
+
+      q->decode_sss_on_track = true;
+
+    } else {
+
+      /* If the cell is known, we work on a 1ms basis */
+      q->nof_recv_sf = 1;
+
+      q->decode_sss_on_track = true;
+    }
+
+    q->frame_len = q->nof_recv_sf*q->sf_len;
+
+    if(q->fft_size < 700 && q->decimate) {
+      q->decimate = 1;
+    }
+
+    if(srslte_sync_resize(&q->sfind, q->frame_len, q->frame_len, q->fft_size)) {
+      fprintf(stderr, "Error initiating sync find\n");
+      return SRSLTE_ERROR;
+    }
+    if (cell.id == 1000) {
+      if(srslte_sync_resize(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
+        fprintf(stderr, "Error initiating sync track\n");
+        return SRSLTE_ERROR;
+      }
+    } else {
+      if(srslte_sync_resize(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
+        fprintf(stderr, "Error initiating sync track\n");
+        return SRSLTE_ERROR;
+      }
+    }
+
+    if (cell.id == 1000) {
+      /* If the cell id is unknown, enable CP detection on find */
       // FIXME: CP detection not working very well. Not supporting Extended CP right now
-      srslte_sync_cp_en(&q->sfind, false);      
-      srslte_sync_cp_en(&q->strack, false); 
+      srslte_sync_cp_en(&q->sfind, false);
+      srslte_sync_cp_en(&q->strack, false);
 
       srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.8);
       srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
 
-      srslte_sync_cfo_i_detec_en(&q->sfind, false); 
+      srslte_sync_cfo_i_detec_en(&q->sfind, false);
 
-      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES; 
+      q->nof_avg_find_frames = FIND_NOF_AVG_FRAMES;
       srslte_sync_set_threshold(&q->sfind, 2.0);
       srslte_sync_set_threshold(&q->strack, 1.2);
-      
+
     } else {
       srslte_sync_set_N_id_2(&q->sfind, cell.id%3);
       srslte_sync_set_N_id_2(&q->strack, cell.id%3);
       q->sfind.cp = cell.cp;
       q->strack.cp = cell.cp;
-      srslte_sync_cp_en(&q->sfind, false);      
-      srslte_sync_cp_en(&q->strack, false);        
+      srslte_sync_cp_en(&q->sfind, false);
+      srslte_sync_cp_en(&q->strack, false);
+
+      srslte_sync_cfo_i_detec_en(&q->sfind, false);
 
-      srslte_sync_cfo_i_detec_en(&q->sfind, false); 
-      
       srslte_sync_set_cfo_ema_alpha(&q->sfind, 0.1);
       srslte_sync_set_cfo_ema_alpha(&q->strack, 0.1);
 
       /* In find phase and if the cell is known, do not average pss correlation
-       * because we only capture 1 subframe and do not know where the peak is. 
+       * because we only capture 1 subframe and do not know where the peak is.
        */
-      q->nof_avg_find_frames = 1; 
+      q->nof_avg_find_frames = 1;
       srslte_sync_set_em_alpha(&q->sfind, 1);
       srslte_sync_set_threshold(&q->sfind, 3.0);
-      
+
       srslte_sync_set_em_alpha(&q->strack, 0.2);
       srslte_sync_set_threshold(&q->strack, 1.2);
 
     }
-      
+
     srslte_ue_sync_reset(q);
-    
+
     ret = SRSLTE_SUCCESS;
   }
-  
-clean_exit:
-  if (ret == SRSLTE_ERROR) {
-    srslte_ue_sync_free(q);
-  }
-  return ret; 
-}
 
-uint32_t srslte_ue_sync_sf_len(srslte_ue_sync_t *q) {
-  return q->frame_len;
+  return ret;
 }
 
-void srslte_ue_sync_free(srslte_ue_sync_t *q) {
-  if (q->do_agc) {
-    srslte_agc_free(&q->agc);
-  }
-  if (!q->file_mode) {
-    srslte_sync_free(&q->sfind);
-    srslte_sync_free(&q->strack);    
-  } else {
-    srslte_filesource_free(&q->file_source);
-  }
-  bzero(q, sizeof(srslte_ue_sync_t));
-}
 
 void srslte_ue_sync_get_last_timestamp(srslte_ue_sync_t *q, srslte_timestamp_t *timestamp) {
   memcpy(timestamp, &q->last_timestamp, sizeof(srslte_timestamp_t));
@@ -642,7 +705,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
           } 
           if (q->correct_cfo) {
             for (int i=0;i<q->nof_rx_antennas;i++) {
-              srslte_cfo_correct(&q->sfind.cfocorr, 
+              srslte_cfo_correct(&q->strack.cfocorr,
                           input_buffer[i], 
                           input_buffer[i], 
                           -srslte_sync_get_cfo(&q->strack) / q->fft_size);                         
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 08edcafe0..2792cff09 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -36,22 +36,20 @@
 #define CURRENT_SLOTLEN_RE SRSLTE_SLOT_LEN_RE(q->cell.nof_prb, q->cell.cp)
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
+#define MAX_SFLEN     SRSLTE_SF_LEN(srslte_symbol_sz(max_prb))
 
-int srslte_ue_ul_init(srslte_ue_ul_t *q, 
-                      srslte_cell_t cell) 
+int srslte_ue_ul_init(srslte_ue_ul_t *q,
+                      uint32_t max_prb)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
-  if (q                 != NULL &&
-      srslte_cell_isvalid(&cell))   
+  if (q != NULL)
   {
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_ue_ul_t));
     
-    q->cell = cell; 
-    
-    if (srslte_ofdm_tx_init(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+    if (srslte_ofdm_tx_init(&q->fft, SRSLTE_CP_NORM, max_prb)) {
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
@@ -60,41 +58,41 @@ int srslte_ue_ul_init(srslte_ue_ul_t *q,
 
     q->normalize_en = false; 
 
-    if (srslte_cfo_init(&q->cfo, CURRENT_SFLEN)) {
+    if (srslte_cfo_init(&q->cfo, MAX_SFLEN)) {
       fprintf(stderr, "Error creating CFO object\n");
       goto clean_exit;
     }
     
     srslte_cfo_set_tol(&q->cfo, 0);
-    
-    if (srslte_pusch_init(&q->pusch, q->cell)) {
+
+    if (srslte_pusch_init_ue(&q->pusch, max_prb)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    if (srslte_pucch_init(&q->pucch, q->cell)) {
+    if (srslte_pucch_init(&q->pucch)) {
       fprintf(stderr, "Error creating PUSCH object\n");
       goto clean_exit;
     }
-    if (srslte_softbuffer_tx_init(&q->softbuffer, q->cell.nof_prb)) {
+    if (srslte_softbuffer_tx_init(&q->softbuffer, max_prb)) {
       fprintf(stderr, "Error initiating soft buffer\n");
       goto clean_exit;
     }
-    if (srslte_refsignal_ul_init(&q->signals, cell)) {
+    if (srslte_refsignal_ul_init(&q->signals, max_prb)) {
       fprintf(stderr, "Error initiating srslte_refsignal_ul\n");
       goto clean_exit;
     }
-    q->sf_symbols = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+    q->sf_symbols = srslte_vec_malloc(SRSLTE_SF_LEN_PRB(max_prb) * sizeof(cf_t));
     if (!q->sf_symbols) {
       perror("malloc");
       goto clean_exit; 
     }
-    q->refsignal = srslte_vec_malloc(2 * SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t));
+    q->refsignal = srslte_vec_malloc(2 * SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->refsignal) {
       perror("malloc");
       goto clean_exit; 
     }
     
-    q->srs_signal = srslte_vec_malloc(SRSLTE_NRE * q->cell.nof_prb * sizeof(cf_t));
+    q->srs_signal = srslte_vec_malloc(SRSLTE_NRE * max_prb * sizeof(cf_t));
     if (!q->srs_signal) {
       perror("malloc");
       goto clean_exit; 
@@ -102,8 +100,7 @@ int srslte_ue_ul_init(srslte_ue_ul_t *q,
     q->signals_pregenerated = false; 
     ret = SRSLTE_SUCCESS;
   } else {
-    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
-            cell.id, cell.nof_ports, cell.nof_prb);      
+    fprintf(stderr, "Invalid parameters\n");
   }
 
 clean_exit: 
@@ -115,7 +112,7 @@ clean_exit:
 
 void srslte_ue_ul_free(srslte_ue_ul_t *q) {
   if (q) {
-    srslte_ofdm_rx_free(&q->fft);
+    srslte_ofdm_tx_free(&q->fft);
     srslte_pusch_free(&q->pusch);
     srslte_pucch_free(&q->pucch);
     srslte_softbuffer_tx_free(&q->softbuffer);
@@ -140,6 +137,48 @@ void srslte_ue_ul_free(srslte_ue_ul_t *q) {
   }
 }
 
+int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
+                          srslte_cell_t cell)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL && srslte_cell_isvalid(&cell))
+  {
+    if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
+      memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
+
+      if (srslte_ofdm_tx_set_prb(&q->fft, q->cell.cp, q->cell.nof_prb)) {
+        fprintf(stderr, "Error resizing FFT\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_cfo_resize(&q->cfo, SRSLTE_SF_LEN_PRB(q->cell.nof_prb))) {
+        fprintf(stderr, "Error resizing CFO object\n");
+        return SRSLTE_ERROR;
+      }
+      srslte_cfo_set_tol(&q->cfo, 50.0/(15000.0*srslte_symbol_sz(q->cell.nof_prb)));
+      if (srslte_pusch_set_cell(&q->pusch, q->cell)) {
+        fprintf(stderr, "Error resizing PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_pucch_set_cell(&q->pucch, q->cell)) {
+        fprintf(stderr, "Error resizing PUSCH object\n");
+        return SRSLTE_ERROR;
+      }
+      if (srslte_refsignal_ul_set_cell(&q->signals, q->cell)) {
+        fprintf(stderr, "Error resizing srslte_refsignal_ul\n");
+        return SRSLTE_ERROR;
+      }
+      q->signals_pregenerated = false;
+    }
+    ret = SRSLTE_SUCCESS;
+  } else {
+    fprintf(stderr, "Invalid cell properties: Id=%d, Ports=%d, PRBs=%d\n",
+            cell.id, cell.nof_ports, cell.nof_prb);
+  }
+  return ret;
+}
+
+
 void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, float cur_cfo) {
   q->current_cfo = cur_cfo; 
 }
@@ -154,7 +193,7 @@ void srslte_ue_ul_set_normalization(srslte_ue_ul_t *q, bool enabled)
   q->normalize_en = enabled;
 }
 
-/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
+/* Precalculate the PUSCH scramble sequences for a given RNTI. This function takes a while
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  * For the connection procedure, use srslte_pusch_encode_rnti() or srslte_pusch_decode_rnti() functions 
  */
@@ -295,7 +334,7 @@ int srslte_ue_ul_pucch_encode(srslte_ue_ul_t *q, srslte_uci_data_t uci_data,
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
@@ -365,7 +404,7 @@ int srslte_ue_ul_srs_encode(srslte_ue_ul_t *q, uint32_t tti, cf_t *output_signal
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
@@ -434,7 +473,7 @@ int srslte_ue_ul_pusch_encode_rnti_softbuffer(srslte_ue_ul_t *q,
     srslte_ofdm_tx_sf(&q->fft, q->sf_symbols, output_signal);
     
     if (q->cfo_en) {
-      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));            
+      srslte_cfo_correct(&q->cfo, output_signal, output_signal, q->current_cfo / srslte_symbol_sz(q->cell.nof_prb));
     }
     
     if (q->normalize_en) {
diff --git a/lib/src/phy/utils/convolution.c b/lib/src/phy/utils/convolution.c
index d3b852b76..f96bdd4a2 100644
--- a/lib/src/phy/utils/convolution.c
+++ b/lib/src/phy/utils/convolution.c
@@ -27,6 +27,7 @@
 
 #include <stdlib.h>
 #include <string.h>
+#include <srslte/srslte.h>
 
 #include "srslte/phy/dft/dft.h"
 #include "srslte/phy/utils/vector.h"
@@ -34,14 +35,17 @@
 
 
 int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_t filter_len) {
+  bzero(q, sizeof(srslte_conv_fft_cc_t));
+
   q->input_len = input_len;
   q->filter_len = filter_len;
   q->output_len = input_len+filter_len;
+  q->max_filter_len = filter_len;
+  q->max_input_len  = input_len;
   q->input_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
   q->filter_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
   q->output_fft = srslte_vec_malloc(sizeof(cf_t)*q->output_len);
-  
-  
+
   if (!q->input_fft || !q->filter_fft || !q->output_fft) {
     return SRSLTE_ERROR;
   }
@@ -64,6 +68,34 @@ int srslte_conv_fft_cc_init(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_
   return SRSLTE_SUCCESS;
 }
 
+int srslte_conv_fft_cc_replan(srslte_conv_fft_cc_t *q, uint32_t input_len, uint32_t filter_len) {
+  if (input_len > q->max_input_len || filter_len > q->max_filter_len) {
+    fprintf(stderr, "Error in conv_fft_cc_replan(): input_len and filter_len must be lower than initialized\n");
+    return -1;
+  }
+
+  q->input_len = input_len;
+  q->filter_len = filter_len;
+  q->output_len = input_len+filter_len;
+
+  if (!q->input_fft || !q->filter_fft || !q->output_fft) {
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->input_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating input plan\n");
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->filter_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating filter plan\n");
+    return SRSLTE_ERROR;
+  }
+  if (srslte_dft_replan(&q->output_plan,q->output_len)) {
+    fprintf(stderr, "Error initiating output plan\n");
+    return SRSLTE_ERROR;
+  }
+  return SRSLTE_SUCCESS;
+}
+
 void srslte_conv_fft_cc_free(srslte_conv_fft_cc_t *q) {
   if (q->input_fft) {
     free(q->input_fft);
diff --git a/lib/src/upper/gw.cc b/lib/src/upper/gw.cc
index 4eba4b4d6..189c08912 100644
--- a/lib/src/upper/gw.cc
+++ b/lib/src/upper/gw.cc
@@ -44,11 +44,10 @@ gw::gw()
   :if_up(false)
 {}
 
-void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::rrc_interface_gw *rrc_, srsue::ue_interface *ue_, log *gw_log_)
+void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_)
 {
   pool    = byte_buffer_pool::get_instance();
   pdcp    = pdcp_;
-  rrc     = rrc_;
   ue      = ue_;
   gw_log  = gw_log_;
   run_enable = true;
@@ -249,11 +248,6 @@ void gw::run_thread()
           {
             gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
 
-            while(run_enable && (!rrc->is_connected() || !rrc->have_drb())) {
-              rrc->connect();
-              usleep(1000);
-            }
-            
             if (!run_enable) {
               break;
             }
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index eca637635..69748b11d 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -93,17 +93,22 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     fprintf(stderr, "Error allocating memory\n");
     return; 
   }
-  if (srslte_enb_dl_init(&enb_dl, phy->cell)) {
+  if (srslte_enb_dl_init(&enb_dl, phy->cell.nof_prb)) {
     fprintf(stderr, "Error initiating ENB DL\n");
     return;
   }
-  
-  if (srslte_enb_ul_init(&enb_ul, 
-                          phy->cell, 
-                          NULL, 
-                          &phy->pusch_cfg, 
-                          &phy->hopping_cfg,
-                          &phy->pucch_cfg)) 
+  if (srslte_enb_dl_set_cell(&enb_dl, phy->cell)) {
+    fprintf(stderr, "Error initiating ENB DL\n");
+    return;
+  }
+  srslte_enb_ul_init(&enb_ul, phy->cell.nof_prb);
+
+  if (srslte_enb_ul_set_cell(&enb_ul,
+                              phy->cell,
+                              NULL,
+                              &phy->pusch_cfg,
+                              &phy->hopping_cfg,
+                              &phy->pucch_cfg))
   {
     fprintf(stderr, "Error initiating ENB DL\n");
     return;
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index f6f22f334..394772703 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -163,7 +163,6 @@ private:
   
   // pointer to MAC PCAP object
   srslte::mac_pcap* pcap;
-  bool signals_pregenerated;
   bool is_first_ul_grant;
 
 
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 0bfae26c9..c50252ae2 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -45,6 +45,7 @@ class phch_recv : public thread
 {
 public:
   phch_recv();
+  ~phch_recv();
   void init(srslte::radio_multi* radio_handler, mac_interface_phy *mac,rrc_interface_phy *rrc,
             prach *prach_buffer, srslte::thread_pool *_workers_pool,
             phch_common *_worker_com, srslte::log* _log_h, uint32_t nof_rx_antennas, uint32_t prio, int sync_cpu_affinity = -1);
@@ -55,6 +56,7 @@ public:
 
   void    set_earfcn(std::vector<uint32_t> earfcn);
 
+  bool    stop_sync();
   void    cell_search_start();
   void    cell_search_next();
   bool    cell_select(uint32_t earfcn, srslte_cell_t cell);
@@ -75,6 +77,14 @@ private:
   void   set_ue_sync_opts(srslte_ue_sync_t *q); 
   void   run_thread();
 
+  void   set_sampling_rate();
+  bool   set_frequency();
+
+  void   cell_search_inc();
+
+  bool init_cell();
+  void free_cell();
+
   bool   running; 
   
   srslte::radio_multi  *radio_h;
@@ -84,10 +94,15 @@ private:
   srslte::thread_pool  *workers_pool;
   phch_common          *worker_com;
   prach                *prach_buffer;
-  
+
+  // Structures for Cell Camp
   srslte_ue_sync_t    ue_sync;
   srslte_ue_mib_t     ue_mib;
-  
+
+  // Structures for Cell Search
+  srslte_ue_cellsearch_t cs;
+  srslte_ue_mib_sync_t   ue_mib_sync;
+
   uint32_t      nof_rx_antennas;
 
   cf_t *sf_buffer[SRSLTE_MAX_PORTS];
@@ -99,6 +114,8 @@ private:
     IDLE, CELL_SEARCH, CELL_MEASURE, CELL_SELECT, CELL_CAMP
   } phy_state;
 
+  bool is_in_idle;
+
   enum {
     SRATE_NONE=0, SRATE_FIND, SRATE_CAMP
   } srate_mode;
@@ -108,8 +125,7 @@ private:
   bool          is_sfn_synched; 
   bool          started; 
   float         time_adv_sec;
-  bool          radio_is_streaming;
-  uint32_t      tti; 
+  uint32_t      tti;
   bool          do_agc;
   
   float         last_gain;
@@ -117,8 +133,10 @@ private:
   uint32_t      nof_tx_mutex;
   uint32_t      tx_mutex_cnt;
 
+  uint32_t      current_earfcn;
+
   uint32_t      sync_sfn_cnt;
-  const static uint32_t SYNC_SFN_TIMEOUT = 5000;
+  const static uint32_t SYNC_SFN_TIMEOUT = 100;
   float ul_dl_factor;
   int cur_earfcn_index;
   bool cell_search_in_progress;
@@ -132,8 +150,7 @@ private:
   int    cell_sync_sfn();
   int    cell_meas_rsrp();
   bool   cell_search(int force_N_id_2 = -1);
-  bool   init_cell();
-  void   free_cell();
+  bool   set_cell();
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 669422981..bb669408e 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -42,11 +42,13 @@ class phch_worker : public srslte::thread_pool::worker
 public:
   
   phch_worker();
+  ~phch_worker();
   void  reset(); 
   void  set_common(phch_common *phy);
-  bool  init_cell(srslte_cell_t cell);
-  void  free_cell();
-  
+  bool  init(uint32_t max_prb);
+
+  bool  set_cell(srslte_cell_t cell);
+
   /* Functions used by main PHY thread */
   cf_t* get_buffer(uint32_t antenna_idx);
   void  set_tti(uint32_t tti, uint32_t tx_tti); 
@@ -104,14 +106,15 @@ private:
   
   /* Common objects */  
   phch_common    *phy;
-  srslte_cell_t  cell; 
-  bool           cell_initiated; 
+  srslte_cell_t  cell;
+  bool           mem_initiated;
+  bool           cell_initiated;
   cf_t          *signal_buffer[SRSLTE_MAX_PORTS]; 
   uint32_t       tti; 
   uint32_t       tx_tti;
   bool           pregen_enabled;
   uint32_t       last_dl_pdcch_ncce;
-  bool           rnti_is_set; 
+  bool           rnti_is_set;
   
   /* Objects for DL */
   srslte_ue_dl_t ue_dl; 
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index a2a234535..c8d88ee44 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -46,6 +46,7 @@ typedef _Complex float cf_t;
 class phy
     : public phy_interface_mac
     , public phy_interface_rrc
+    , public thread
 {
 public:
   phy();
@@ -57,6 +58,9 @@ public:
   
   void stop();
 
+  void wait_initialize();
+  bool is_initiated();
+
   void set_agc_enable(bool enabled);
 
   void get_metrics(phy_metrics_t &m);
@@ -84,6 +88,7 @@ public:
   /********** MAC INTERFACE ********************/
   /* Functions to synchronize with a cell */
   bool    sync_status(); // this is also RRC interface
+  bool    sync_stop();
 
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
   void set_crnti(uint16_t rnti);
@@ -130,7 +135,10 @@ public:
   void    start_plot();
     
 private:
-    
+
+  void run_thread();
+
+  bool     initiated;
   uint32_t nof_workers; 
   
   const static int MAX_WORKERS         = 4;
@@ -139,8 +147,10 @@ private:
   const static int SF_RECV_THREAD_PRIO = 1;
   const static int WORKERS_THREAD_PRIO = 0; 
   
-  srslte::radio_multi   *radio_handler;
-  srslte::log           *log_h;
+  srslte::radio_multi      *radio_handler;
+  srslte::log              *log_h;
+  srsue::mac_interface_phy *mac;
+  srsue::rrc_interface_phy *rrc;
 
   srslte::thread_pool      workers_pool;
   std::vector<phch_worker> workers;
diff --git a/srsue/hdr/phy/prach.h b/srsue/hdr/phy/prach.h
index 694353926..67ccc0c94 100644
--- a/srsue/hdr/phy/prach.h
+++ b/srsue/hdr/phy/prach.h
@@ -45,14 +45,15 @@ namespace srsue {
       
       args              = NULL; 
       config            = NULL; 
-      initiated         = false; 
-      signal_buffer     = NULL; 
+      signal_buffer     = NULL;
       transmitted_tti   = 0; 
-      target_power_dbm  = 0; 
+      target_power_dbm  = 0;
+      mem_initiated     = false;
+      cell_initiated    = false;
     }
-    void           init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config, phy_args_t *args, srslte::log *log_h);
-    bool           init_cell(srslte_cell_t cell);
-    void           free_cell();
+    ~prach();
+    void           init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config, uint32_t max_prb, phy_args_t *args, srslte::log *log_h);
+    bool           set_cell(srslte_cell_t cell);
     bool           prepare_to_send(uint32_t preamble_idx, int allowed_subframe = -1, float target_power_dbm = -1);
     bool           is_ready_to_send(uint32_t current_tti);
     int            tx_tti();
@@ -70,7 +71,8 @@ namespace srsue {
     srslte::log   *log_h;
     int            preamble_idx;  
     int            allowed_subframe; 
-    bool           initiated;   
+    bool           mem_initiated;
+    bool           cell_initiated;
     uint32_t       len; 
     cf_t          *buffer[64]; 
     srslte_prach_t prach_obj; 
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 60aff8469..122b1e265 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -175,6 +175,7 @@ private:
   srsue::usim        usim;
 
   srslte::logger_file logger;
+  //srslte::logger_stdout logger;
   srslte::log_filter  rf_log;
   srslte::log_filter  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 1640c9d8a..c1a481461 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -89,8 +89,6 @@ public:
 
   void plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code);
 
-  void cell_selected();
-
   // UE interface
   void attach_request();
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 793c2ee4f..056a3cadd 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -69,7 +69,6 @@ class rrc
   : public rrc_interface_nas,
     public rrc_interface_phy,
     public rrc_interface_mac,
-    public rrc_interface_gw,
     public rrc_interface_pdcp,
     public rrc_interface_rlc,
     public srslte::timer_callback,
@@ -118,6 +117,14 @@ private:
   uint8_t transaction_id;
   bool drb_up;
 
+  uint32_t connecting_timeout;
+  static const uint32_t RRC_CONNECTING_TIMEOUT = 100;
+
+  uint32_t plmn_select_timeout;
+  static const uint32_t RRC_PLMN_SELECT_TIMEOUT = 1000;
+
+  uint32_t select_cell_timeout;
+  static const uint32_t RRC_SELECT_CELL_TIMEOUT = 500;
 
   uint8_t k_rrc_enc[32];
   uint8_t k_rrc_int[32];
@@ -173,7 +180,6 @@ private:
   void enable_capabilities();
   void plmn_search();
   void plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id);
-  void connect();
 
   // PHY interface
   void in_sync();
diff --git a/srsue/src/mac/dl_harq.cc b/srsue/src/mac/dl_harq.cc
index 685224786..7f0989982 100644
--- a/srsue/src/mac/dl_harq.cc
+++ b/srsue/src/mac/dl_harq.cc
@@ -177,8 +177,8 @@ void dl_harq_entity::dl_harq_process::reset() {
 }
 
 bool dl_harq_entity::dl_harq_process::init(uint32_t pid_, dl_harq_entity *parent) {
-  if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
-    Error("Error initiating soft buffer\n");
+  if (srslte_softbuffer_rx_init(&softbuffer, SRSLTE_MAX_PRB)) {
+    fprintf(stderr, "Error initiating soft buffer\n");
     return false; 
   } else {
     pid = pid_;
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 5a74492e6..41a0db323 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -47,7 +47,6 @@ mac::mac() : ttisync(10240),
 {
   started = false;  
   pcap    = NULL;   
-  signals_pregenerated = false;  
   bzero(&metrics, sizeof(mac_metrics_t));
 }
   
@@ -130,8 +129,7 @@ void mac::reset()
   phy_h->pdcch_dl_search_reset();
   phy_h->pdcch_ul_search_reset();
   
-  signals_pregenerated = false; 
-  is_first_ul_grant = true;   
+  is_first_ul_grant = true;
   
   bzero(&uernti, sizeof(ue_rnti_t));
 }
@@ -178,18 +176,6 @@ void mac::run_thread() {
         ra_procedure.start_mac_order();
       }
       ra_procedure.step(tti);
-      
-      if (ra_procedure.is_successful() && !signals_pregenerated) {
-
-        // Configure PHY to look for UL C-RNTI grants
-        phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, uernti.crnti);
-        phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, uernti.crnti);
-        
-        // Pregenerate UL signals and C-RNTI scrambling sequences
-        Debug("Pre-computing C-RNTI scrambling sequences for C-RNTI=0x%x\n", uernti.crnti);
-        phy_h->set_crnti(uernti.crnti);
-        signals_pregenerated = true; 
-      }      
     }
   }  
 }
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 495c9945f..1f03cda40 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -479,6 +479,12 @@ void ra_proc::step_completition() {
     mux_unit->msg3_flush();
     msg3_flushed = true; 
   }
+  // Configure PHY to look for UL C-RNTI grants
+  phy_h->pdcch_ul_search(SRSLTE_RNTI_USER, rntis->crnti);
+  phy_h->pdcch_dl_search(SRSLTE_RNTI_USER, rntis->crnti);
+
+  phy_h->set_crnti(rntis->crnti);
+
   msg3_transmitted = false;  
   state = COMPLETION_DONE;
 }
diff --git a/srsue/src/mac/ul_harq.cc b/srsue/src/mac/ul_harq.cc
index da28d62b3..970ac4276 100644
--- a/srsue/src/mac/ul_harq.cc
+++ b/srsue/src/mac/ul_harq.cc
@@ -206,7 +206,7 @@ void ul_harq_entity::ul_harq_process::set_harq_feedback(bool ack) {
 }
 
 bool ul_harq_entity::ul_harq_process::init(uint32_t pid_, ul_harq_entity* parent) {
-  if (srslte_softbuffer_tx_init(&softbuffer, 110)) {
+  if (srslte_softbuffer_tx_init(&softbuffer, SRSLTE_MAX_PRB)) {
     fprintf(stderr, "Error initiating soft buffer\n");
     return false; 
   } else {
diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index 3c3987863..c65ec7c77 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -186,7 +186,9 @@ void phch_common::set_dl_rnti(srslte_rnti_type_t type, uint16_t rnti_value, int
   dl_rnti_type  = type;
   dl_rnti_start = tti_start;
   dl_rnti_end   = tti_end;
-  Debug("Set DL rnti: start=%d, end=%d, value=0x%x\n", tti_start, tti_end, rnti_value);  
+  if (log_h) {
+    Debug("Set DL rnti: start=%d, end=%d, value=0x%x\n", tti_start, tti_end, rnti_value);
+  }
 }
 
 void phch_common::reset_pending_ack(uint32_t tti) {
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 3662f4d24..6142cfaf0 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -39,12 +39,33 @@
 
 namespace srsue {
 
+int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
+  srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
+  if (radio_h->rx_now(data, nsamples, rx_time)) {
+    int offset = nsamples - radio_h->get_tti_len();
+    if (abs(offset) < 10 && offset != 0) {
+      radio_h->tx_offset(offset);
+    } else if (nsamples < 10) {
+      radio_h->tx_offset(nsamples);
+    }
+    return nsamples;
+  } else {
+    return -1;
+  }
+}
+
+double callback_set_rx_gain(void *h, double gain) {
+  srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
+  return radio_handler->set_rx_gain_th(gain);
+}
+
 
 phch_recv::phch_recv() {
+  bzero(&cell, sizeof(srslte_cell_t));
   running = false;
 }
 
-void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
+void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_mac, rrc_interface_phy *_rrc,
                      prach *_prach_buffer, srslte::thread_pool *_workers_pool,
                      phch_common *_worker_com, srslte::log *_log_h, uint32_t nof_rx_antennas_, uint32_t prio,
                      int sync_cpu_affinity) {
@@ -65,11 +86,48 @@ void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_ma
   sync_sfn_cnt = 0;
   srate_mode = SRATE_NONE;
   cell_search_in_progress = false;
+  current_earfcn = 0;
 
   for (uint32_t i = 0; i < nof_rx_antennas; i++) {
     sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
   }
 
+
+  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
+                                      radio_h)) {
+    Error("Initiating UE cell search\n");
+    return;
+  }
+
+  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
+
+  // Set options defined in expert section
+  set_ue_sync_opts(&cs.ue_sync);
+
+  if (do_agc) {
+    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
+  }
+
+  if (srslte_ue_dl_init_multi(&ue_dl_measure, SRSLTE_MAX_PRB, nof_rx_antennas)) {
+    Error("Initiating ue_dl_measure\n");
+    return;
+  }
+
+  if (srslte_ue_mib_init(&ue_mib, SRSLTE_MAX_PRB)) {
+    Error("Initiating UE MIB decoder\n");
+    return;
+  }
+
+  if (srslte_ue_sync_init_multi(&ue_sync, SRSLTE_MAX_PRB, false, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+    Error("Initiating ue_sync\n");
+    return;
+  }
+
+  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+    Error("Initiating UE MIB synchronization\n");
+    return;
+  }
+
   nof_tx_mutex = MUTEX_X_WORKER * workers_pool->get_nof_workers();
   worker_com->set_nof_mutex(nof_tx_mutex);
   if (sync_cpu_affinity < 0) {
@@ -77,42 +135,28 @@ void phch_recv::init(srslte::radio_multi *_radio_handler, mac_interface_phy *_ma
   } else {
     start_cpu(prio, sync_cpu_affinity);
   }
-
-
 }
 
-void phch_recv::stop() {
-  running = false;
-  wait_thread_finish();
+phch_recv::~phch_recv() {
   for (uint32_t i = 0; i < nof_rx_antennas; i++) {
     if (sf_buffer[i]) {
       free(sf_buffer[i]);
     }
   }
+  srslte_ue_sync_free(&ue_sync);
+  srslte_ue_dl_free(&ue_dl_measure);
+  srslte_ue_mib_sync_free(&ue_mib_sync);
+  srslte_ue_cellsearch_free(&cs);
 }
 
-void phch_recv::set_agc_enable(bool enable) {
-  do_agc = enable;
-}
+void phch_recv::stop() {
 
-int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
-  srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
-  if (radio_h->rx_now(data, nsamples, rx_time)) {
-    int offset = nsamples - radio_h->get_tti_len();
-    if (abs(offset) < 10 && offset != 0) {
-      radio_h->tx_offset(offset);
-    } else if (nsamples < 10) {
-      radio_h->tx_offset(nsamples);
-    }
-    return nsamples;
-  } else {
-    return -1;
-  }
+  running = false;
+  wait_thread_finish();
 }
 
-double callback_set_rx_gain(void *h, double gain) {
-  srslte::radio_multi *radio_handler = (srslte::radio_multi *) h;
-  return radio_handler->set_rx_gain_th(gain);
+void phch_recv::set_agc_enable(bool enable) {
+  do_agc = enable;
 }
 
 void phch_recv::set_time_adv_sec(float _time_adv_sec) {
@@ -147,84 +191,48 @@ void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
   srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
 }
 
-bool phch_recv::init_cell() {
+bool phch_recv::set_cell() {
   cell_is_set = false;
-  if (!srslte_ue_mib_init(&ue_mib, cell)) {
-    if (!srslte_ue_sync_init_multi(&ue_sync, cell, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-
-      // Set options defined in expert section
-      set_ue_sync_opts(&ue_sync);
-
-      if (srslte_ue_dl_init_multi(&ue_dl_measure, cell, nof_rx_antennas)) {
-        Error("Setting cell: initiating ue_dl_measure\n");
-        return false;
-      }
-
-      for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
-        if (!((phch_worker *) workers_pool->get_worker(i))->init_cell(cell)) {
-          Error("Setting cell: initiating PHCH worker\n");
-          return false;
-        }
-      }
-      radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
-      if (do_agc) {
-        srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
-      }
-      srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
-      cell_is_set = true;
-    } else {
-      Error("Error setting cell: initiating ue_sync");
-    }
-  } else {
-    Error("Error setting cell: initiating ue_mib\n");
+  if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
+    Error("Setting cell: initiating ue_mib\n");
+    return false;
   }
-  return cell_is_set;
-}
-
-void phch_recv::free_cell() {
-  if (phy_state != IDLE) {
-    phy_state = IDLE;
-    usleep(2000);
+  if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
+    Error("Setting cell: initiating ue_sync");
+    return false;
   }
 
-  srslte_ue_sync_free(&ue_sync);
+  // Set options defined in expert section
+  set_ue_sync_opts(&ue_sync);
 
-  srslte_ue_dl_free(&ue_dl_measure);
+  if (srslte_ue_dl_set_cell(&ue_dl_measure, cell)) {
+    Error("Setting cell: initiating ue_dl_measure\n");
+    return false;
+  }
 
-  if (cell_is_set) {
-    for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
-      ((phch_worker *) workers_pool->get_worker(i))->free_cell();
+  for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
+    if (!((phch_worker *) workers_pool->get_worker(i))->set_cell(cell)) {
+      Error("Setting cell: initiating PHCH worker\n");
+      return false;
     }
-    prach_buffer->free_cell();
-    cell_is_set = false;
   }
-}
+  radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  if (do_agc) {
+    srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
+  }
+  cell_is_set = true;
 
+  return cell_is_set;
+}
 
 bool phch_recv::cell_search(int force_N_id_2) {
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
-  uint8_t bch_payload_bits[SRSLTE_BCH_PAYLOAD_LEN / 8];
 
   srslte_ue_cellsearch_result_t found_cells[3];
-  srslte_ue_cellsearch_t cs;
 
+  bzero(&cell, sizeof(srslte_cell_t));
   bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
 
-  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
-                                      radio_h)) {
-    Error("Initiating UE cell search\n");
-    return false;
-  }
-
-  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
-
-  // Set options defined in expert section
-  set_ue_sync_opts(&cs.ue_sync);
-
-  if (do_agc) {
-    srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
-  }
-
   if (srate_mode != SRATE_FIND) {
     srate_mode = SRATE_FIND;
     radio_h->set_rx_srate(1.92e6);
@@ -245,7 +253,6 @@ bool phch_recv::cell_search(int force_N_id_2) {
   last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
   radio_h->stop_rx();
-  srslte_ue_cellsearch_free(&cs);
 
   if (ret < 0) {
     Error("Error decoding MIB: Error searching PSS\n");
@@ -260,11 +267,8 @@ bool phch_recv::cell_search(int force_N_id_2) {
   cell.cp = found_cells[max_peak_cell].cp;
   cellsearch_cfo = found_cells[max_peak_cell].cfo;
 
-  srslte_ue_mib_sync_t ue_mib_sync;
-
-  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, cell.id, cell.cp, radio_recv_wrapper_cs, nof_rx_antennas,
-                                    radio_h)) {
-    Error("Initiating UE MIB synchronization\n");
+  if (srslte_ue_mib_sync_set_cell(&ue_mib_sync, cell.id, cell.cp)) {
+    Error("Setting UE MIB cell\n");
     return false;
   }
 
@@ -275,19 +279,23 @@ bool phch_recv::cell_search(int force_N_id_2) {
     srslte_ue_sync_start_agc(&ue_mib_sync.ue_sync, callback_set_rx_gain, last_gain);
   }
 
+  srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
   srslte_ue_sync_set_cfo(&ue_mib_sync.ue_sync, cellsearch_cfo);
 
   /* Find and decode MIB */
-  uint32_t sfn;
   int sfn_offset;
   radio_h->start_rx();
   ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
-                                  SRSLTE_DEFAULT_MAX_FRAMES_PBCH,
+                                  40,
                                   bch_payload, &cell.nof_ports, &sfn_offset);
   radio_h->stop_rx();
   last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
   cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
-  srslte_ue_mib_sync_free(&ue_mib_sync);
+
+  srslte_ue_sync_reset(&ue_sync);
+  srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
+
+  Info("Setting ue_sync cfo=%f KHz\n", cellsearch_cfo/1000);
 
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
@@ -300,87 +308,6 @@ bool phch_recv::cell_search(int force_N_id_2) {
 }
 
 
-void phch_recv::resync_sfn() {
-  sync_sfn_cnt = 0;
-  phy_state = CELL_SELECT;
-}
-
-void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
-  this->earfcn = earfcn;
-}
-
-void phch_recv::cell_search_next() {
-  cell_search_in_progress = true;
-  cur_earfcn_index++;
-  if (cur_earfcn_index >= 0) {
-    if (cur_earfcn_index >= (int) earfcn.size() - 1) {
-      cur_earfcn_index = 0;
-    }
-    // If PHY is running, stop and free resources
-    free_cell();
-
-    float dl_freq = 1e6*srslte_band_fd(earfcn[cur_earfcn_index]);
-    if (dl_freq >= 0) {
-      log_h->info("Cell Search: Set DL EARFCN=%d, frequency=%.1f MHz, channel_index=%d\n", earfcn[cur_earfcn_index],
-                  dl_freq / 1e6, cur_earfcn_index);
-      radio_h->set_rx_freq(dl_freq);
-
-      // Start PHY cell search (finds maximum cell in frequency)
-      phy_state = CELL_SEARCH;
-    } else {
-      log_h->error("Cell Search: Invalid EARFCN=%d, channel_index=%d\n", earfcn[cur_earfcn_index], cur_earfcn_index);
-    }
-  }
-}
-
-void phch_recv::cell_search_start() {
-  if (earfcn.size() > 0) {
-    cur_earfcn_index = -1;
-    log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
-    log_h->info("Cell Search: Starting procedure...\n");
-    cell_search_next();
-  } else {
-    log_h->info("Empty EARFCN list. Stopping cell search...\n");
-    log_h->console("Empty EARFCN list. Stopping cell search...\n");
-  }
-}
-
-bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
-  free_cell();
-
-  int cnt=0;
-  while(phy_state == CELL_SEARCH && cnt<100) {
-    usleep(10000);
-    log_h->info("PHY in CELL_SEARCH. Waiting...\n");
-  }
-  if (phy_state==CELL_SEARCH) {
-    log_h->warning("PHY still in CELL_SEARCH, forcing CELL_SELECT...\n");
-  }
-  float dl_freq = 1e6*srslte_band_fd(earfcn);
-  float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(earfcn));
-  if (dl_freq >= 0 || ul_freq <= 0) {
-    log_h->info("Cell Select: Set EARFCN=%d, frequency=%.1f MHz, UL frequency=%.1f MHz\n", earfcn, dl_freq / 1e6,
-                ul_freq / 1e6);
-    radio_h->set_rx_freq(dl_freq);
-    radio_h->set_tx_freq(ul_freq);
-
-    ul_dl_factor = ul_freq/dl_freq;
-
-    cell_search_in_progress = false;
-    this->cell = cell;
-    if (init_cell()) {
-      phy_state = CELL_SELECT;
-      return true;
-    } else {
-      log_h->error("Cell Select: Initializing cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
-    }
-  } else {
-    log_h->error("Cell Select: Invalid EARFCN=%d\n", earfcn);
-  }
-  return false;
-}
-
-
 int phch_recv::cell_sync_sfn(void) {
 
   int ret = SRSLTE_ERROR;
@@ -451,44 +378,186 @@ int phch_recv::cell_meas_rsrp() {
   return 0;
 }
 
+void phch_recv::resync_sfn() {
+  radio_h->stop_rx();
+  radio_h->start_rx();
+  srslte_ue_mib_reset(&ue_mib);
+  sync_sfn_cnt = 0;
+  phy_state = CELL_SELECT;
+}
+
+void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
+  this->earfcn = earfcn;
+}
+
+bool phch_recv::stop_sync() {
+  Info("SYNC:   Going to IDLE\n");
+  phy_state = IDLE;
+  int cnt=0;
+  while(!is_in_idle && cnt<100) {
+    usleep(10000);
+    cnt++;
+  }
+  return is_in_idle;
+}
+
+void phch_recv::cell_search_inc()
+{
+  cur_earfcn_index++;
+  Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
+  if (cur_earfcn_index >= 0) {
+    if (cur_earfcn_index >= (int) earfcn.size() - 1) {
+      cur_earfcn_index = 0;
+    }
+  }
+  if (current_earfcn != earfcn[cur_earfcn_index]) {
+    current_earfcn = earfcn[cur_earfcn_index];
+    set_frequency();
+  }
+}
+
+void phch_recv::cell_search_next() {
+  if (cell_search_in_progress) {
+    cell_search_in_progress = false;
+    if (!stop_sync()) {
+      log_h->warning("Couldn't stop sync\n");
+    }
+    cell_search_inc();
+    phy_state = CELL_SEARCH;
+    cell_search_in_progress = true;
+  }
+}
+
+void phch_recv::cell_search_start() {
+  if (earfcn.size() > 0) {
+    cell_search_in_progress = true;
+    cur_earfcn_index = -1;
+    cell_search_next();
+    log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+    log_h->info("Cell Search: Starting procedure...\n");
+  } else {
+    log_h->info("Empty EARFCN list. Stopping cell search...\n");
+    log_h->console("Empty EARFCN list. Stopping cell search...\n");
+  }
+}
+
+bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
+
+  // Check if we are already camping in this cell
+  if (earfcn == current_earfcn && this->cell.id == cell.id) {
+    log_h->info("Cell Select: Already in Cell EARFCN=%d\n", earfcn);
+      cell_search_in_progress = false;
+    if (srate_mode != SRATE_CAMP) {
+      set_sampling_rate();
+    }
+    if (phy_state != CELL_SELECT) {
+      resync_sfn();
+    }
+    return true;
+  } else {
+
+    cell_search_in_progress = false;
+
+    if (!stop_sync()) {
+      log_h->warning("Still not in idle\n");
+    }
+
+    current_earfcn = earfcn;
+
+    if (set_frequency()) {
+      this->cell = cell;
+      log_h->info("Cell Select: Configuring cell...\n");
+
+      if (set_cell()) {
+        log_h->info("Cell Select: Synchronizing on cell...\n");
+
+        resync_sfn();
+
+        usleep(500000); // Time offset we set start_rx to start receveing samples
+        return true;
+      } else {
+        log_h->error("Cell Select: Configuring cell in EARFCN=%d, PCI=%d\n", earfcn, cell.id);
+      }
+    }
+    return false;
+  }
+}
+
+bool phch_recv::set_frequency()
+{
+  float dl_freq = 1e6*srslte_band_fd(current_earfcn);
+  float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(current_earfcn));
+  if (dl_freq > 0 && ul_freq > 0) {
+    log_h->info("Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
+                current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
+
+    log_h->console("Tunning to EARFCN=%d, F_dl=%.1f MHz, F_ul=%.1f MHz\n",
+                current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
+
+    radio_h->set_rx_freq(dl_freq);
+    radio_h->set_tx_freq(ul_freq);
+    ul_dl_factor = ul_freq / dl_freq;
+
+    srslte_ue_sync_reset(&ue_sync);
+
+    return true;
+  } else {
+    log_h->error("Cell Search: Invalid EARFCN=%d\n", current_earfcn);
+    return false;
+  }
+}
+
+void phch_recv::set_sampling_rate()
+{
+  float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+
+  if (30720 % ((int) srate / 1000) == 0) {
+    radio_h->set_master_clock_rate(30.72e6);
+  } else {
+    radio_h->set_master_clock_rate(23.04e6);
+  }
+
+  srate_mode = SRATE_CAMP;
+  radio_h->set_rx_srate(srate);
+  radio_h->set_tx_srate(srate);
+}
+
 void phch_recv::run_thread() {
   int sync_res;
   phch_worker *worker = NULL;
   cf_t *buffer[SRSLTE_MAX_PORTS];
+  phy_state  = IDLE;
+  is_in_idle = true;
+
   while (running) {
+    if (phy_state != IDLE) {
+      is_in_idle = false;
+      Debug("SYNC state=%d\n", phy_state);
+    }
     switch (phy_state) {
       case CELL_SEARCH:
-        if (cell_search()) {
-          init_cell();
-          float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
-
-          if (30720 % ((int) srate / 1000) == 0) {
-            radio_h->set_master_clock_rate(30.72e6);
-          } else {
-            radio_h->set_master_clock_rate(23.04e6);
+        if (cell_search() && cell_search_in_progress) {
+          if (!srslte_cell_isvalid(&cell)) {
+            Error("Detected invalid cell\n");
+            phy_state = IDLE;
+            break;
+          }
+          if (set_cell()) {
+            set_sampling_rate();
+            resync_sfn();
           }
-
-          log_h->info("Setting Sampling frequency %.2f MHz\n", (float) srate / 1000000);
-          srate_mode = SRATE_CAMP;
-          radio_h->set_rx_srate(srate);
-          radio_h->set_tx_srate(srate);
 
           Info("SYNC:  Cell found. Synchronizing...\n");
-          phy_state = CELL_SELECT;
-          sync_sfn_cnt = 0;
-          srslte_ue_mib_reset(&ue_mib);
+        } else {
+          if (cell_search_in_progress) {
+            cell_search_inc();
+          }
         }
         break;
       case CELL_SELECT:
 
         srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
 
-        if (!radio_is_streaming) {
-          // Start streaming
-          radio_h->start_rx();
-          radio_is_streaming = true;
-        }
-
         switch (cell_sync_sfn()) {
           default:
             log_h->console("Going IDLE\n");
@@ -511,17 +580,17 @@ void phch_recv::run_thread() {
         sync_sfn_cnt++;
         if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
           sync_sfn_cnt = 0;
-          radio_h->stop_rx();
-          radio_is_streaming = false;
-          log_h->console("Timeout while synchronizing SFN\n");
+          phy_state = IDLE;
           log_h->warning("Timeout while synchronizing SFN\n");
         }
         break;
       case CELL_MEASURE:
         switch(cell_meas_rsrp()) {
           case 1:
-            rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
             phy_state = CELL_CAMP;
+            rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
+            log_h->info("Measured OK. Camping on cell PCI=%d...\n", cell.id);
+            break;
           case 0:
             break;
           default:
@@ -579,13 +648,11 @@ void phch_recv::run_thread() {
               log_h->debug("Sending in-sync to RRC\n");
             }
           } else {
-            log_h->console("Sync error.\n");
             log_h->error("Sync error. Sending out-of-sync to RRC\n");
             // Notify RRC of out-of-sync frame
             rrc->out_of_sync();
             worker->release();
             worker_com->reset_ul();
-            phy_state = CELL_SELECT;
           }
         } else {
           // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
@@ -593,6 +660,10 @@ void phch_recv::run_thread() {
         }
         break;
       case IDLE:
+        if (!is_in_idle) {
+          radio_h->stop_rx();
+        }
+        is_in_idle = true;
         usleep(1000);
         break;
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 14de9b51a..514b8eb0d 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -40,6 +40,10 @@
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/phch/ra.h>
+
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
 sem_t plot_sem; 
@@ -57,15 +61,31 @@ phch_worker::phch_worker() : tr_exec(10240)
 {
   phy = NULL; 
   bzero(signal_buffer, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  
+
+  mem_initiated   = false;
   cell_initiated  = false; 
   pregen_enabled  = false; 
-  trace_enabled   = false; 
-  
+  trace_enabled   = false;
+
   reset();  
 }
 
-void phch_worker::reset() 
+
+phch_worker::~phch_worker()
+{
+  if (mem_initiated) {
+    for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
+      if (signal_buffer[i]) {
+        free(signal_buffer[i]);
+      }
+    }
+    srslte_ue_dl_free(&ue_dl);
+    srslte_ue_ul_free(&ue_ul);
+    mem_initiated = false;
+  }
+}
+
+void phch_worker::reset()
 {
   bzero(&dl_metrics, sizeof(dl_metrics_t));
   bzero(&ul_metrics, sizeof(ul_metrics_t));
@@ -86,48 +106,56 @@ void phch_worker::set_common(phch_common* phy_)
 {
   phy = phy_;   
 }
-    
-bool phch_worker::init_cell(srslte_cell_t cell_)
+
+bool phch_worker::init(uint32_t max_prb)
 {
-  memcpy(&cell, &cell_, sizeof(srslte_cell_t));
-  
-  // ue_sync in phy.cc requires a buffer for 3 subframes 
+  // ue_sync in phy.cc requires a buffer for 3 subframes
   for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
-    signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+    signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(max_prb));
     if (!signal_buffer[i]) {
       Error("Allocating memory\n");
-      return false; 
+      return false;
     }
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, phy->args->nof_rx_ant)) {    
+  if (srslte_ue_dl_init_multi(&ue_dl, max_prb, phy->args->nof_rx_ant)) {
     Error("Initiating UE DL\n");
-    return false; 
+    return false;
   }
-  
-  if (srslte_ue_ul_init(&ue_ul, cell)) {  
+
+  if (srslte_ue_ul_init(&ue_ul, max_prb)) {
     Error("Initiating UE UL\n");
-    return false; 
+    return false;
   }
+
   srslte_ue_ul_set_normalization(&ue_ul, true);
   srslte_ue_ul_set_cfo_enable(&ue_ul, true);
-    
-  cell_initiated = true; 
-  
-  return true; 
+
+  mem_initiated = true;
+
+  return true;
 }
 
-void phch_worker::free_cell()
+bool phch_worker::set_cell(srslte_cell_t cell_)
 {
-  if (cell_initiated) {
-    for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
-      if (signal_buffer[i]) {
-        free(signal_buffer[i]);
-      }      
+  if (cell.id != cell_.id || !cell_initiated) {
+    memcpy(&cell, &cell_, sizeof(srslte_cell_t));
+
+    if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+      Error("Initiating UE DL\n");
+      return false;
     }
-    srslte_ue_dl_free(&ue_dl);
-    srslte_ue_ul_free(&ue_ul);
+
+    if (srslte_ue_ul_set_cell(&ue_ul, cell)) {
+      Error("Initiating UE UL\n");
+      return false;
+    }
+    srslte_ue_ul_set_normalization(&ue_ul, true);
+    srslte_ue_ul_set_cfo_enable(&ue_ul, true);
+
+    cell_initiated = true;
   }
+  return true;
 }
 
 cf_t* phch_worker::get_buffer(uint32_t antenna_idx)
@@ -375,7 +403,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
       Error("Converting DCI message to DL grant\n");
       return false;   
     }
-    
+
     /* Fill MAC grant structure */
     grant->ndi = dci_unpacked.ndi;
     grant->pid = dci_unpacked.harq_process;
@@ -426,21 +454,21 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
           srslte_sch_set_max_noi(&ue_dl.pdsch.dl_sch, phy->args->pdsch_max_its);
         }
 
-        
+
   #ifdef LOG_EXECTIME
         struct timeval t[3];
         gettimeofday(&t[1], NULL);
   #endif
-        
-        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffer, ue_dl.sf_symbols_m, 
+
+        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffer, ue_dl.sf_symbols_m,
                                       ue_dl.ce_m, noise_estimate, rnti, payload) == 0;
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
         get_time_interval(t);
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
-        
-        Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n", 
+
+        Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n",
               grant->nof_prb, harq_pid, 
               grant->mcs.tbs/8, grant->mcs.idx, rv, 
               ack?"OK":"KO", 
@@ -448,9 +476,6 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
               srslte_pdsch_last_noi(&ue_dl.pdsch),
               timestr);
 
-        //printf("tti=%d, cfo=%f\n", tti, cfo*15000);
-        //srslte_vec_save_file("pdsch", signal_buffer, sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        
         // Store metrics
         dl_metrics.mcs    = grant->mcs.idx;
         
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 9b03ab013..d96160cc7 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -94,44 +94,63 @@ bool phy::check_args(phy_args_t *args)
   return true; 
 }
 
-bool phy::init(srslte::radio_multi* radio_handler_, mac_interface_phy *mac, rrc_interface_phy *rrc, 
-               srslte::log *log_h_, phy_args_t *phy_args)
-{
+bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_interface_phy *rrc,
+               srslte::log *log_h, phy_args_t *phy_args) {
 
   mlockall(MCL_CURRENT | MCL_FUTURE);
-  
-  n_ta = 0; 
-  log_h = log_h_; 
-  radio_handler = radio_handler_;
-  
+
+  n_ta = 0;
+  this->log_h         = log_h;
+  this->radio_handler = radio_handler;
+  this->mac           = mac;
+  this->rrc           = rrc;
+
   if (!phy_args) {
-    args = &default_args; 
+    args = &default_args;
     set_default_args(args);
   } else {
     args = phy_args;
   }
-  
+
   if (!check_args(args)) {
-    return false; 
+    return false;
   }
-  
-  nof_workers = args->nof_phy_threads; 
-  
+
+  nof_workers = args->nof_phy_threads;
+
+  initiated = false;
+  start();
+  return true;
+}
+
+// Initializes PHY in a thread
+void phy::run_thread() {
+
+  prach_buffer.init(&config.common.prach_cnfg, SRSLTE_MAX_PRB, args, log_h);
+  workers_common.init(&config, args, log_h, radio_handler, mac);
+
   // Add workers to workers pool and start threads
   for (uint32_t i=0;i<nof_workers;i++) {
     workers[i].set_common(&workers_common);
-    workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);    
+    workers[i].init(SRSLTE_MAX_PRB);
+    workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);
   }
-  prach_buffer.init(&config.common.prach_cnfg, args, log_h);
-  workers_common.init(&config, args, log_h, radio_handler, mac);
-  
+
   // Warning this must be initialized after all workers have been added to the pool
   sf_recv.init(radio_handler, mac, rrc, &prach_buffer, &workers_pool, &workers_common, log_h, args->nof_rx_ant, SF_RECV_THREAD_PRIO, args->sync_cpu_affinity);
 
   // Disable UL signal pregeneration until the attachment 
   enable_pregen_signals(false);
 
-  return true; 
+  initiated = true;
+}
+
+void phy::wait_initialize() {
+  wait_thread_finish();
+}
+
+bool phy::is_initiated() {
+  return initiated;
 }
 
 void phy::set_agc_enable(bool enabled)
@@ -189,7 +208,7 @@ void phy::configure_prach_params()
     Debug("Configuring PRACH parameters\n");
     srslte_cell_t cell; 
     sf_recv.get_current_cell(&cell);
-    if (!prach_buffer.init_cell(cell)) {
+    if (!prach_buffer.set_cell(cell)) {
       Error("Configuring PRACH parameters\n");
     } 
   } else {
@@ -200,8 +219,10 @@ void phy::configure_prach_params()
 void phy::configure_ul_params(bool pregen_disabled)
 {
   Info("PHY:   Configuring UL parameters\n");
-  for (uint32_t i=0;i<nof_workers;i++) {
-    workers[i].set_ul_params(pregen_disabled);
+  if (is_initiated()) {
+    for (uint32_t i=0;i<nof_workers;i++) {
+      workers[i].set_ul_params(pregen_disabled);
+    }
   }
 }
 
@@ -309,6 +330,11 @@ bool phy::sync_status()
   return sf_recv.status_is_sync();
 }
 
+bool phy::sync_stop()
+{
+  return sf_recv.stop_sync();
+}
+
 void phy::set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN])
 {
   workers_common.set_rar_grant(tti, grant_payload);
diff --git a/srsue/src/phy/prach.cc b/srsue/src/phy/prach.cc
index 62980c623..7c693fe7a 100644
--- a/srsue/src/phy/prach.cc
+++ b/srsue/src/phy/prach.cc
@@ -41,14 +41,13 @@
 
 namespace srsue {
  
-  
-void prach::free_cell() 
-{
-  if (initiated) {
+
+prach::~prach() {
+  if (mem_initiated) {
     for (int i=0;i<64;i++) {
       if (buffer[i]) {
-        free(buffer[i]);    
-      }      
+        free(buffer[i]);
+      }
     }
     if (signal_buffer) {
       free(signal_buffer);
@@ -58,84 +57,103 @@ void prach::free_cell()
   }
 }
 
-void prach::init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config_, phy_args_t *args_, srslte::log* log_h_)
+void prach::init(LIBLTE_RRC_PRACH_CONFIG_SIB_STRUCT *config_, uint32_t max_prb, phy_args_t *args_, srslte::log* log_h_)
 {
-  log_h  = log_h_; 
-  config = config_; 
-  args   = args_; 
+  log_h  = log_h_;
+  config = config_;
+  args   = args_;
+
+  for (int i=0;i<64;i++) {
+    buffer[i] = (cf_t*) srslte_vec_malloc(SRSLTE_PRACH_MAX_LEN*sizeof(cf_t));
+    if(!buffer[i]) {
+      perror("malloc");
+      return;
+    }
+  }
+  if (srslte_cfo_init(&cfo_h, SRSLTE_PRACH_MAX_LEN)) {
+    fprintf(stderr, "PRACH: Error initiating CFO\n");
+    return;
+  }
+  srslte_cfo_set_tol(&cfo_h, 0);
+  signal_buffer = (cf_t*) srslte_vec_malloc(SRSLTE_PRACH_MAX_LEN*sizeof(cf_t));
+  if (!signal_buffer) {
+    perror("malloc");
+    return;
+  }
+  if (srslte_prach_init(&prach_obj, srslte_symbol_sz(max_prb))) {
+    Error("Initiating PRACH library\n");
+    return;
+  }
+  mem_initiated = true;
 }
 
-bool prach::init_cell(srslte_cell_t cell_)
+bool prach::set_cell(srslte_cell_t cell_)
 {
-  // TODO: Check if other PRACH parameters changed
-  if (cell_.id != cell.id || !initiated) {
-    if (initiated) {
-      free_cell();
-    }
-    cell = cell_; 
-    preamble_idx = -1; 
-
-    uint32_t configIdx      = config->prach_cnfg_info.prach_config_index;
-    uint32_t rootSeq        = config->root_sequence_index;
-    uint32_t zeroCorrConfig = config->prach_cnfg_info.zero_correlation_zone_config;
-    uint32_t freq_offset    = config->prach_cnfg_info.prach_freq_offset;
-    bool     highSpeed      = config->prach_cnfg_info.high_speed_flag; 
-    
-    if (6 + freq_offset > cell.nof_prb) {
-      log_h->console("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
-      log_h->error("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
-      return false; 
-    }
-    
-    if (srslte_prach_init(&prach_obj, srslte_symbol_sz(cell.nof_prb), 
-                          configIdx, rootSeq, highSpeed, zeroCorrConfig))
-    {
-      Error("Initiating PRACH library\n");
-      return false; 
-    }
-    
-    len = prach_obj.N_seq + prach_obj.N_cp;
-    for (int i=0;i<64;i++) {
-      buffer[i] = (cf_t*) srslte_vec_malloc(len*sizeof(cf_t));
-      if(!buffer[i]) {
-        return false; 
-      }    
-      if(srslte_prach_gen(&prach_obj, i, freq_offset, buffer[i])) {
-        Error("Generating PRACH preamble %d\n", i);
+  if (mem_initiated) {
+    // TODO: Check if other PRACH parameters changed
+    if (cell_.id != cell.id || !cell_initiated) {
+      memcpy(&cell, &cell_, sizeof(srslte_cell_t));
+      preamble_idx = -1;
+
+      uint32_t configIdx      = config->prach_cnfg_info.prach_config_index;
+      uint32_t rootSeq        = config->root_sequence_index;
+      uint32_t zeroCorrConfig = config->prach_cnfg_info.zero_correlation_zone_config;
+      uint32_t freq_offset    = config->prach_cnfg_info.prach_freq_offset;
+      bool     highSpeed      = config->prach_cnfg_info.high_speed_flag;
+
+      if (6 + freq_offset > cell.nof_prb) {
+        log_h->console("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
+        log_h->error("Error no space for PRACH: frequency offset=%d, N_rb_ul=%d\n", freq_offset, cell.nof_prb);
+        return false;
+      }
+
+      Info("PRACH: configIdx=%d, rootSequence=%d, zeroCorrelationConfig=%d, freqOffset=%d\n",
+            configIdx, rootSeq, zeroCorrConfig, freq_offset);
+
+      if (srslte_prach_set_cell(&prach_obj, srslte_symbol_sz(cell.nof_prb),
+                                 configIdx, rootSeq, highSpeed, zeroCorrConfig)) {
+        Error("Initiating PRACH library\n");
         return false;
       }
+      for (int i=0;i<64;i++) {
+        if(srslte_prach_gen(&prach_obj, i, freq_offset, buffer[i])) {
+          Error("Generating PRACH preamble %d\n", i);
+          return false;
+        }
+      }
+
+      len = prach_obj.N_seq + prach_obj.N_cp;
+      transmitted_tti = -1;
+      cell_initiated = true;
     }
-    srslte_cfo_init(&cfo_h, len);
-    srslte_cfo_set_tol(&cfo_h, 0);
-    signal_buffer = (cf_t*) srslte_vec_malloc(len*sizeof(cf_t)); 
-    initiated = signal_buffer?true:false; 
-    transmitted_tti = -1; 
-    Debug("PRACH Initiated %s\n", initiated?"OK":"KO");
+    return true;
+  } else {
+    fprintf(stderr, "PRACH: Error must call init() first\n");
+    return false;
   }
-  return initiated;  
 }
 
 bool prach::prepare_to_send(uint32_t preamble_idx_, int allowed_subframe_, float target_power_dbm_)
 {
-  if (initiated && preamble_idx_ < 64) {
+  if (cell_initiated && preamble_idx_ < 64) {
     preamble_idx = preamble_idx_;
     target_power_dbm = target_power_dbm_;
     allowed_subframe = allowed_subframe_; 
     transmitted_tti = -1; 
-    Debug("PRACH prepare to send preamble %d\n", preamble_idx);
+    Debug("PRACH: prepare to send preamble %d\n", preamble_idx);
     return true; 
   } else {
-    if (!initiated) {
-      Error("PRACH not initiated\n");
+    if (!cell_initiated) {
+      Error("PRACH: Cell not configured\n");
     } else if (preamble_idx_ >= 64) {
-      Error("Invalid preamble %d\n", preamble_idx_);
+      Error("PRACH: Invalid preamble %d\n", preamble_idx_);
     }
     return false; 
   }
 }
 
 bool prach::is_ready_to_send(uint32_t current_tti_) {
-  if (initiated && preamble_idx >= 0 && preamble_idx < 64) {
+  if (cell_initiated && preamble_idx >= 0 && preamble_idx < 64) {
     // consider the number of subframes the transmission must be anticipated 
     uint32_t current_tti = (current_tti_ + tx_advance_sf)%10240;
     if (srslte_prach_tti_opportunity(&prach_obj, current_tti, allowed_subframe)) {
@@ -195,9 +213,9 @@ void prach::send(srslte::radio *radio_handler, float cfo, float pathloss, srslte
   
   Info("PRACH: Transmitted preamble=%d, CFO=%.2f KHz, tx_time=%f\n", 
        preamble_idx, cfo*15, tx_time.frac_secs);
-  preamble_idx = -1; 
+  preamble_idx = -1;
 
-  radio_handler->set_tx_gain(old_gain);    
+  radio_handler->set_tx_gain(old_gain);
   Debug("Restoring TX gain to %.0f dB\n", old_gain);  
 }
   
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 2fd38c3c8..b6e541932 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -121,7 +121,11 @@ bool ue::init(all_args_t *args_)
   }
   
   // Init layers
-  
+
+  // PHY initis in background, start before radio
+  args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
+  phy.init(&radio, &mac, &rrc, &phy_log, &args->expert.phy);
+
   /* Start Radio */
   char *dev_name = NULL;
   if (args->rf.device_name.compare("auto")) {
@@ -152,15 +156,13 @@ bool ue::init(all_args_t *args_)
   radio.set_manual_calibration(&args->rf_cal);
 
   // Set PHY options
-  args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
 
   if (args->rf.tx_gain > 0) {
     args->expert.phy.ul_pwr_ctrl_en = false; 
   } else {
     args->expert.phy.ul_pwr_ctrl_en = true; 
   }
-  phy.init(&radio, &mac, &rrc, &phy_log, &args->expert.phy);
-  
+
   if (args->rf.rx_gain < 0) {
     radio.start_agc(false);    
     radio.set_tx_rx_gain_offset(10);
@@ -170,7 +172,6 @@ bool ue::init(all_args_t *args_)
   }
   if (args->rf.tx_gain > 0) {
     radio.set_tx_gain(args->rf.tx_gain);    
-    printf("set tx gain %f\n", args->rf.tx_gain);
   } else {
     radio.set_tx_gain(args->rf.rx_gain);
     std::cout << std::endl << 
@@ -183,19 +184,23 @@ bool ue::init(all_args_t *args_)
   mac.init(&phy, &rlc, &rrc, &mac_log);
   rlc.init(&pdcp, &rrc, this, &rlc_log, &mac);
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, SECURITY_DIRECTION_UPLINK);
-  rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
-  
-  rrc.set_ue_category(args->expert.ue_cateogry);
-  
+
   nas.init(&usim, &rrc, &gw, &nas_log);
-  gw.init(&pdcp, &rrc, this, &gw_log);
+  gw.init(&pdcp, this, &gw_log);
   usim.init(&args->usim, &usim_log);
 
+  rrc.init(&phy, &mac, &rlc, &pdcp, &nas, &usim, &mac, &rrc_log);
+  rrc.set_ue_category(args->expert.ue_cateogry);
+
   // Currently EARFCN list is set to only one frequency as indicated in ue.conf
   std::vector<uint32_t> earfcn_list;
   earfcn_list.push_back(args->rf.dl_earfcn);
   phy.set_earfcn(earfcn_list);
 
+  printf("Waiting PHY to initialize...\n");
+  phy.wait_initialize();
+  phy.configure_ul_params();
+
   printf("\n\nRequesting NAS Attach...\n");
   nas.attach_request();
 
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 11372e198..cb9adf3ad 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -107,14 +107,6 @@ void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_
   }
 }
 
-void nas::cell_selected() {
-  if (state == EMM_STATE_REGISTERED_INITIATED) {
-    rrc->connect();
-  } else {
-    nas_log->info("Cell selected in invalid state = %s\n", emm_state_text[state]);
-  }
-}
-
 bool nas::is_attached() {
   return state == EMM_STATE_REGISTERED;
 }
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 81577cce4..8d3f7cab2 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -156,33 +156,23 @@ void rrc::plmn_search() {
   rrc_log->info("Starting PLMN search procedure\n");
   state = RRC_STATE_PLMN_SELECTION;
   phy->cell_search_start();
+  plmn_select_timeout = 0;
 }
 
 void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
   rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
 
-  state = RRC_STATE_CELL_SELECTING;
 
   // Sort cells according to RSRP
 
   selected_plmn_id = plmn_id;
   last_selected_cell = -1;
+  select_cell_timeout = 0;
 
+  state = RRC_STATE_CELL_SELECTING;
   select_next_cell_in_plmn();
 }
 
-void rrc::connect() {
-  pthread_mutex_lock(&mutex);
-  if (RRC_STATE_CELL_SELECTED == state) {
-    rrc_log->info("RRC in IDLE state - sending connection request.\n");
-    state = RRC_STATE_CONNECTING;
-    send_con_request();
-  } else {
-    rrc_log->warning("Received connect() but cell is not selected\n");
-  }
-  pthread_mutex_unlock(&mutex);
-}
-
 void rrc::select_next_cell_in_plmn() {
   for (uint32_t i = last_selected_cell + 1; i < known_cells.size(); i++) {
     for (uint32_t j = 0; j < known_cells[i].sib1.N_plmn_ids; j++) {
@@ -196,25 +186,8 @@ void rrc::select_next_cell_in_plmn() {
                       known_cells[i].sib1.cell_id);
         // Check that cell satisfies S criteria
         if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
-          // Give time to the PHY to sync on the new cell
-          int cnt=0;
-          while(!phy->sync_status() && cnt<100) {
-            usleep(1000);
-            cnt++;
-          }
-          if (phy->sync_status()) {
-            if (!known_cells[i].has_valid_sib1) {
-              si_acquire_state = SI_ACQUIRE_SIB1;
-            } else if (!known_cells[i].has_valid_sib2) {
-              si_acquire_state = SI_ACQUIRE_SIB2;
-            } else {
-              si_acquire_state = SI_ACQUIRE_IDLE;
-            }
-            last_selected_cell = i;
-          } else {
-            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x: Could not synchronize\n",
-                           known_cells[i].earfcn, known_cells[i].sib1.cell_id);
-          }
+          last_selected_cell = i;
+          current_cell = &known_cells[i];
           return;
         } else {
           rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
@@ -223,6 +196,7 @@ void rrc::select_next_cell_in_plmn() {
       }
     }
   }
+  rrc_log->info("No more known cells...\n");
 }
 
 /*******************************************************************************
@@ -238,6 +212,14 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
       current_cell = &known_cells[i];
       rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
                     known_cells[i].phy_cell.id, known_cells[i].rsrp);
+
+      if (!known_cells[i].has_valid_sib1) {
+        si_acquire_state = SI_ACQUIRE_SIB1;
+      } else {
+        for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+          nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+        }
+      }
       return;
     }
   }
@@ -332,6 +314,12 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
 
       current_cell->has_valid_sib1 = true;
 
+      // Send PLMN and TAC to NAS
+      std::stringstream ss;
+      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+      }
+
       // Jump to next state
       switch(state) {
         case RRC_STATE_CELL_SELECTING:
@@ -347,12 +335,6 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
           si_acquire_state = SI_ACQUIRE_IDLE;
       }
 
-      // Send PLMN and TAC to NAS
-      std::stringstream ss;
-      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
-        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
-      }
-
     } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
                SI_ACQUIRE_SIB2 == si_acquire_state) {
       mac->bcch_stop_rx();
@@ -370,12 +352,10 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
         case RRC_STATE_CELL_SELECTING:
           si_acquire_state = SI_ACQUIRE_IDLE;
           state = RRC_STATE_CELL_SELECTED;
-          nas->cell_selected();
           break;
         default:
           si_acquire_state = SI_ACQUIRE_IDLE;
       }
-
     }
   }
 }
@@ -384,7 +364,7 @@ void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
 // Right now, this thread only controls System Information acquisition procedure
 void rrc::run_thread() {
   uint32_t tti;
-  uint32_t si_win_start, si_win_len;
+  uint32_t si_win_start=0, si_win_len=0, last_win_start=0;
   uint16_t period;
   uint32_t nof_sib1_trials = 0;
   const int SIB1_SEARCH_TIMEOUT = 30;
@@ -395,32 +375,94 @@ void rrc::run_thread() {
         // Instruct MAC to look for SIB1
         tti = mac->get_current_tti();
         si_win_start = sib_start_tti(tti, 2, 5);
-        mac->bcch_start_rx(si_win_start, 1);
-        rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                       si_win_start, 1);
-        nof_sib1_trials++;
-        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-          if (state == RRC_STATE_CELL_SELECTING) {
-            select_next_cell_in_plmn();
-            si_acquire_state = SI_ACQUIRE_IDLE;
-          } else if (state == RRC_STATE_PLMN_SELECTION) {
-            phy->cell_search_next();
+        if (tti > last_win_start + 10) {
+          last_win_start = si_win_start;
+          mac->bcch_start_rx(si_win_start, 1);
+          rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                         si_win_start, 1);
+          nof_sib1_trials++;
+          if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+            if (state == RRC_STATE_CELL_SELECTING) {
+              select_next_cell_in_plmn();
+              si_acquire_state = SI_ACQUIRE_IDLE;
+            } else if (state == RRC_STATE_PLMN_SELECTION) {
+              phy->cell_search_next();
+            }
+            nof_sib1_trials = 0;
           }
-          nof_sib1_trials = 0;
         }
-        usleep(20000);
         break;
       case SI_ACQUIRE_SIB2:
         // Instruct MAC to look for SIB2 only when selecting a cell
         tti = mac->get_current_tti();
         period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
         si_win_start = sib_start_tti(tti, period, 0);
-        si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+        if (tti > last_win_start + 10) {
+          last_win_start = si_win_start;
+          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
 
-        mac->bcch_start_rx(si_win_start, si_win_len);
-        rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                       si_win_start, si_win_len);
-        usleep(current_cell->sib1.si_window_length*1000);
+          mac->bcch_start_rx(si_win_start, si_win_len);
+          rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                         si_win_start, si_win_len);
+        }
+        break;
+      default:
+        break;
+    }
+    switch(state) {
+      case RRC_STATE_IDLE:
+        if (nas->is_attached()) {
+          usleep(100000);
+          rrc_log->info("RRC IDLE: NAS is attached, re-selecting cell...\n");
+          plmn_select(selected_plmn_id);
+        }
+        break;
+      case RRC_STATE_PLMN_SELECTION:
+        plmn_select_timeout++;
+        if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
+          rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
+          sleep(1);
+          rrc_log->console("RRC PLMN Search: timeout expired. Searching again\n");
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTING:
+        if (phy->sync_status()) {
+          if (!current_cell->has_valid_sib1) {
+            si_acquire_state = SI_ACQUIRE_SIB1;
+          } else if (!current_cell->has_valid_sib2) {
+            si_acquire_state = SI_ACQUIRE_SIB2;
+          } else {
+            apply_sib2_configs(&current_cell->sib2);
+            si_acquire_state = SI_ACQUIRE_IDLE;
+            state = RRC_STATE_CELL_SELECTED;
+          }
+        }
+        select_cell_timeout++;
+        if (select_cell_timeout >= RRC_SELECT_CELL_TIMEOUT) {
+          rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
+          state = RRC_STATE_PLMN_SELECTION;
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTED:
+        rrc_log->info("RRC Cell Selected: Sending connection request...\n");
+        send_con_request();
+        state = RRC_STATE_CONNECTING;
+        connecting_timeout = 0;
+        break;
+      case RRC_STATE_CONNECTING:
+        connecting_timeout++;
+        if (connecting_timeout >= RRC_CONNECTING_TIMEOUT) {
+          // Select another cell
+          rrc_log->info("RRC Connecting: timeout expired. Selecting next cell\n");
+          state = RRC_STATE_CELL_SELECTING;
+        }
+        break;
+      case RRC_STATE_CONNECTED:
+        // Take measurements, cell reselection, etc
         break;
       default:
         break;
@@ -442,26 +484,6 @@ void rrc::run_thread() {
 
 
 
-
-
-
-
-
-/*******************************************************************************
-*
-*
-*
-* Connection control and establishment/reestablishment procedures
-*
-*
-*
-*******************************************************************************/
-
-
-
-
-
-
 /*******************************************************************************
 NAS interface
 *******************************************************************************/
@@ -1074,6 +1096,7 @@ void rrc::rrc_connection_release() {
   pthread_mutex_lock(&mutex);
   drb_up = false;
   state = RRC_STATE_IDLE;
+  phy->sync_stop();
   set_phy_default();
   set_mac_default();
   mac_timers->get(t311)->run();

From 2979833364308a947f3e2f618013ea3f97802943 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 1 Sep 2017 13:30:28 +0200
Subject: [PATCH 120/269] Improved how data is displayed in pdsch_ue

---
 lib/examples/pdsch_ue.c | 94 ++++++++++++++++++++++++-----------------
 1 file changed, 55 insertions(+), 39 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 1a9f77c6f..0a5d2d81d 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -304,6 +304,12 @@ prog_args_t prog_args;
 uint32_t sfn = 0; // system frame number
 srslte_netsink_t net_sink, net_sink_signal; 
 
+/* Useful macros for printing lines which will disappear */
+#define PRINT_LINE_INIT() int this_nof_lines = 0; static int prev_nof_lines = 0
+#define PRINT_LINE(_fmt, ...) printf("\033[K" _fmt "\n", ##__VA_ARGS__); this_nof_lines++
+#define PRINT_LINE_RESET_CURSOR() printf("\033[%dA", this_nof_lines); prev_nof_lines = this_nof_lines
+#define PRINT_LINE_ADVANCE_CURSOR() printf("\033[%dB", prev_nof_lines + 1)
+
 int main(int argc, char **argv) {
   int ret; 
   int decimate = 1;
@@ -542,6 +548,7 @@ int main(int argc, char **argv) {
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
     bool acks [SRSLTE_MAX_CODEWORDS] = {false};
     char input[128];
+    PRINT_LINE_INIT();
 
     fd_set set;
     FD_ZERO(&set);
@@ -668,55 +675,64 @@ int main(int argc, char **argv) {
             if (gain < 0) {
               gain = 10*log10(srslte_agc_get_gain(&ue_sync.agc)); 
             }
-            if (cell.nof_ports == 1) {
-              printf("CFO: %+6.2f kHz, "
-                         "SNR: %4.1f dB, "
-                         "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
-
-                     srslte_ue_sync_get_cfo(&ue_sync) / 1000,
-                     10 * log10(rsrp0 / noise),
-                     100 * (1 - (float) ue_dl.nof_detected / nof_trials),
-                     (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+
+            /* Print transmission scheme */
+            if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+              PRINT_LINE("    Tx scheme: %s (codebook_idx=%d)", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type),
+                         ue_dl.pdsch_cfg.codebook_idx);
             } else {
+              PRINT_LINE("    Tx scheme: %s", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
+            }
+
+            /* Print basic Parameters */
+            PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
+            PRINT_LINE("nof codewords: %d", ue_dl.pdsch_cfg.grant.nof_tb);
+            PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
+            PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
+            PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
+            PRINT_LINE("   PDCCH-Miss: %5.2f%%", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
+            PRINT_LINE("   PDSCH-BLER: %5.2f%%", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+            PRINT_LINE("         TB 0: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[0].idx,
+                       ue_dl.pdsch_cfg.grant.mcs[0].tbs);
+            PRINT_LINE("         TB 1: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[1].idx,
+                       ue_dl.pdsch_cfg.grant.mcs[1].tbs);
+
+            /* MIMO: if tx and rx antennas are bigger than 1 */
+            if (cell.nof_ports > 1 && ue_dl.pdsch.nof_rx_antennas > 1) {
               /* Compute condition number */
               srslte_ue_dl_ri_select(&ue_dl, NULL, &cn);
 
+              /* Print condition number */
+              PRINT_LINE("            κ: %.1f dB (Condition number, 0 dB => Best)", cn);
+            }
+            PRINT_LINE("");
+
+            /* Spatial multiplex only */
+            if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+
               /* Compute Rank Indicator (RI) and Precoding Matrix Indicator (PMI) */
               srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, NULL);
               for (uint32_t nl = 0; nl < SRSLTE_MAX_LAYERS; nl++) {
-                for (uint32_t cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb ++) {
+                for (uint32_t cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb++) {
                   sinr[nl][cb] = SRSLTE_VEC_EMA(ue_dl.sinr[nl][cb], sinr[nl][cb], 0.5f);
                 }
               }
 
-              /* Print Results */
-              if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-                printf("\033[K    Tx scheme: %s (codebook_idx=%d)\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type),
-                       ue_dl.pdsch_cfg.codebook_idx);
-              } else {
-                printf("\033[K    Tx scheme: %s\n", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
-              }
-              printf("\033[K   nof layers: %d \n", ue_dl.pdsch_cfg.nof_layers);
-              printf("\033[Knof codewords: %d \n", ue_dl.pdsch_cfg.grant.nof_tb);
-              printf("\033[K          CFO: %+5.2f kHz\n", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
-              printf("\033[K          SNR: %+5.1f dB | %+5.1f dB\n", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
-              printf("\033[K           Rb: %6.2f / %6.2f Mbps (net/maximum)\n", uerate, enodebrate);
-              printf("\033[K   PDCCH-Miss: %5.2f%%\n", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
-              printf("\033[K   PDSCH-BLER: %5.2f%%\n", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
-              printf("\033[K         TB 0: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[0].idx, ue_dl.pdsch_cfg.grant.mcs[0].tbs);
-              printf("\033[K         TB 1: mcs=%d; tbs=%d\n", ue_dl.pdsch_cfg.grant.mcs[1].idx, ue_dl.pdsch_cfg.grant.mcs[1].tbs);
-              printf("\033[K            κ: %.1f dB (Condition number, 0 dB => Best)\n", cn);
-              printf("\033[K\n");
-              printf("\033[KSINR (dB) Vs RI and PMI (for TM4, close loop MIMO only):\n");
-              printf("\033[K   | RI |   1   |   2   |\n");
-              printf("\033[K -------+-------+-------+\n");
-              printf("\033[K P |  0 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][0]), (ri == 1 && pmi == 0)?'*':' ', 10 * log10(sinr[1][0]), (ri == 2 && pmi == 0)?'*':' ');
-              printf("\033[K M |  1 | %5.2f%c| %5.2f%c|\n", 10 * log10(sinr[0][1]), (ri == 1 && pmi == 1)?'*':' ', 10 * log10(sinr[1][1]), (ri == 2 && pmi == 1)?'*':' ');
-              printf("\033[K I |  2 | %5.2f%c|-------+ \n", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2)?'*':' ');
-              printf("\033[K   |  3 | %5.2f%c|         \n", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3)?'*':' ');
-              printf("\033[K\nPress enter maximum printing debug log of 1 subframe.\n");
-              printf("\033[21A");
+              /* Print Multiplex stats */
+              PRINT_LINE("SINR (dB) Vs RI and PMI:");
+              PRINT_LINE("   | RI |   1   |   2   |");
+              PRINT_LINE(" -------+-------+-------+");
+              PRINT_LINE(" P |  0 | %5.2f%c| %5.2f%c|", 10 * log10(sinr[0][0]), (ri == 1 && pmi == 0) ? '*' : ' ',
+                         10 * log10(sinr[1][0]), (ri == 2 && pmi == 0) ? '*' : ' ');
+              PRINT_LINE(" M |  1 | %5.2f%c| %5.2f%c|", 10 * log10(sinr[0][1]), (ri == 1 && pmi == 1) ? '*' : ' ',
+                         10 * log10(sinr[1][1]), (ri == 2 && pmi == 1) ? '*' : ' ');
+              PRINT_LINE(" I |  2 | %5.2f%c|-------+ ", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2) ? '*' : ' ');
+              PRINT_LINE("   |  3 | %5.2f%c|         ", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3) ? '*' : ' ');
+              PRINT_LINE("");
             }
+            PRINT_LINE("Press enter maximum printing debug log of 1 subframe.");
+            PRINT_LINE("");
+            PRINT_LINE_RESET_CURSOR();
           }
           break;
       }
@@ -724,7 +740,7 @@ int main(int argc, char **argv) {
         sfn++; 
         if (sfn == 1024) {
           sfn = 0; 
-          printf("\033[21B");
+          PRINT_LINE_ADVANCE_CURSOR();
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
@@ -756,7 +772,7 @@ int main(int argc, char **argv) {
         
     sf_cnt++;                  
   } // Main loop
-  printf("\033[21B\n");
+  printf("\033[30B\n");
 
 #ifndef DISABLE_GRAPHICS
   if (!prog_args.disable_plots) {

From 3dfe42099ce916503c7d19e8cd0aab67da9657e5 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 1 Sep 2017 13:49:04 +0200
Subject: [PATCH 121/269] Corrected prompt of Tx scheme selection

---
 lib/examples/pdsch_enodeb.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 4ded08fdf..a4157d872 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -194,7 +194,7 @@ void base_init() {
 
   /* Select transmission mode */
   if (srslte_str2mimotype(mimo_type_str, &pdsch_cfg.mimo_type)) {
-    ERROR("Wrong transmission mode! Allowed modes: single, diversity, cdd");
+    ERROR("Wrong transmission mode! Allowed modes: single, diversity, cdd and multiplex");
     exit(-1);
   }
 

From 2335d2a6447d29ee01eba2329084757577021cf2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 1 Sep 2017 14:24:48 +0200
Subject: [PATCH 122/269] remove boost::assign

---
 srsue/hdr/upper/rrc.h  | 33 +++++++++++++++++++++++----------
 srsue/src/upper/rrc.cc | 26 +++++---------------------
 2 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index c758622fb..9cbdd5201 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -42,14 +42,20 @@ using srslte::byte_buffer_t;
 
 namespace srsue {
 
+static std::string rb_id_str[] = {"SRB0", "SRB1", "SRB2",
+                                    "DRB1","DRB2","DRB3",
+                                    "DRB4","DRB5","DRB6",
+                                    "DRB7","DRB8"};
+
+
 class rrc
-    :public rrc_interface_nas
-    ,public rrc_interface_phy
-    ,public rrc_interface_mac
-    ,public rrc_interface_gw
-    ,public rrc_interface_pdcp
-    ,public rrc_interface_rlc
-    ,public srslte::timer_callback
+  :public rrc_interface_nas
+  ,public rrc_interface_phy
+  ,public rrc_interface_mac
+  ,public rrc_interface_gw
+  ,public rrc_interface_pdcp
+  ,public rrc_interface_rlc
+  ,public srslte::timer_callback
 {
 public:
   rrc();
@@ -156,10 +162,17 @@ private:
     RB_ID_DRB5,
     RB_ID_DRB6,
     RB_ID_DRB7,
-    RB_ID_DRB8
+    RB_ID_DRB8,
+    RB_ID_MAX
   } rb_id_t;
-  std::map<uint8_t, std::string> bearers;
-  std::string get_rb_name(uint32_t lcid) { return bearers.at(lcid); }
+
+  std::string get_rb_name(uint32_t lcid) {
+    if (lcid < RB_ID_MAX) {
+      return rb_id_str[lcid];
+    } else {
+      return std::string("INVALID_RB");
+    }
+  }
 
   // RLC interface
   void max_retx_attempted();
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 291024c05..8b417f40d 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -45,7 +45,6 @@ rrc::rrc()
   :state(RRC_STATE_IDLE)
   ,drb_up(false)
 {
-  set_bearers();
 }
 
 static void liblte_rrc_handler(void *ctx, char *str) {
@@ -127,7 +126,7 @@ void rrc::set_ue_category(int category)
 
 void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
 {
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", bearers.at(lcid).c_str());
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", rb_id_str[lcid].c_str());
 
   switch(state)
   {
@@ -238,7 +237,7 @@ bool rrc::have_drb()
 
 void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 {
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", bearers.at(lcid).c_str());
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", rb_id_str[lcid].c_str());
   rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
 
   switch(lcid)
@@ -804,7 +803,7 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu)
   liblte_rrc_unpack_dl_dcch_msg((LIBLTE_BIT_MSG_STRUCT*)&bit_buf, &dl_dcch_msg);
 
   rrc_log->info("%s - Received %s\n",
-                bearers.at(lcid).c_str(),
+                rb_id_str[lcid].c_str(),
                 liblte_rrc_dl_dcch_msg_type_text[dl_dcch_msg.msg_type]);
 
   // Reset and reuse pdu buffer if possible
@@ -1387,7 +1386,7 @@ void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg)
   }
 
   srbs[srb_cnfg->srb_id] = *srb_cnfg;
-  rrc_log->info("Added radio bearer %s\n", bearers.at(srb_cnfg->srb_id).c_str());
+  rrc_log->info("Added radio bearer %s\n", rb_id_str[srb_cnfg->srb_id].c_str());
 }
 
 void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
@@ -1448,7 +1447,7 @@ void rrc::add_drb(LIBLTE_RRC_DRB_TO_ADD_MOD_STRUCT *drb_cnfg)
   
   drbs[lcid] = *drb_cnfg;
   drb_up     = true;
-  rrc_log->info("Added radio bearer %s\n", bearers.at(lcid).c_str());
+  rrc_log->info("Added radio bearer %s\n", rb_id_str[lcid].c_str());
 }
 
 void rrc::release_drb(uint8_t lcid)
@@ -1504,19 +1503,4 @@ void rrc::set_rrc_default() {
   mac_timers->get(safe_reset_timer)->set(this, 10);
 }
 
-void rrc::set_bearers()
-{
-  boost::assign::insert(bearers) (RB_ID_SRB0, "SRB0")
-                                 (RB_ID_SRB1, "SRB1")
-                                 (RB_ID_SRB2, "SRB2")
-                                 (RB_ID_DRB1, "DRB1")
-                                 (RB_ID_DRB2, "DRB2")
-                                 (RB_ID_DRB3, "DRB3")
-                                 (RB_ID_DRB4, "DRB4")
-                                 (RB_ID_DRB5, "DRB5")
-                                 (RB_ID_DRB6, "DRB6")
-                                 (RB_ID_DRB7, "DRB7")
-                                 (RB_ID_DRB8, "DRB8");
-}
-
 } // namespace srsue

From a8bbc29ee6141be1bdffb603ed3551adcb5b6a53 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 1 Sep 2017 14:27:22 +0200
Subject: [PATCH 123/269] Reverted default number of rx antennas for UE config

---
 srsue/ue.conf.example | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 532bb9849..8c9ba7d3d 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -26,7 +26,7 @@ ul_freq = 2565000000
 tx_gain = 80
 rx_gain = 60
 
-nof_rx_ant = 2
+#nof_rx_ant = 1
 #device_name = auto
 #device_args = auto
 #time_adv_nsamples = auto

From f5e5e946819c73d97323acbde8ee6b004729f7ba Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 1 Sep 2017 14:27:37 +0200
Subject: [PATCH 124/269]       if (cell.nof_ports > 1) {

---
 srsue/src/phy/phch_worker.cc | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 78c5b2f71..265d3e005 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -40,6 +40,8 @@
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
+#include <srslte/srslte.h>
+
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
 sem_t plot_sem; 
@@ -451,7 +453,11 @@ int phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payloa
       mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_2:
-      mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      if (cell.nof_ports > 1) {
+        mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      } else {
+        mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
       if (grant->nof_tb == 1) {

From ae287dd89a29413d08fcd901ecb5d037a9de7e81 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 1 Sep 2017 18:58:52 +0200
Subject: [PATCH 125/269] fixed incorrect printf

---
 srsue/hdr/mac/dl_harq.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 4f6df9b51..963551150 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -280,7 +280,6 @@ private:
         }
       }
 
-
       void tb_decoded(bool ack_) {
         ack = ack_;
         if (ack) {
@@ -319,7 +318,7 @@ private:
         Info("DL %d (TB %d):  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n",
              pid, tid, is_new_transmission ? "newTX" : "reTX ",
              cur_grant.n_bytes[tid], cur_grant.rv[tid], ack ? "OK" : "KO",
-             cur_grant.ndi[tid], cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
+             cur_grant.ndi[tid], cur_grant.last_ndi[tid], cur_grant.tti, cur_grant.last_tti);
 
         if (ack && pid == HARQ_BCCH_PID) {
           reset();

From 8a3ff19b309a2ec146ef9abdd77dfefa1b32bf3d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 1 Sep 2017 18:59:28 +0200
Subject: [PATCH 126/269] set all pdsch functionts to MIMO by default (removed
 _multi)

---
 lib/examples/pdsch_enodeb.c         |   7 +-
 lib/include/srslte/phy/enb/enb_dl.h |   4 +-
 lib/include/srslte/phy/phch/pdsch.h |  59 ++----
 lib/src/phy/enb/enb_dl.c            |   6 +-
 lib/src/phy/phch/pdsch.c            | 288 ++++++++++------------------
 lib/src/phy/phch/test/pdsch_test.c  |  12 +-
 lib/src/phy/ue/ue_dl.c              |  12 +-
 srsenb/src/phy/phch_worker.cc       |   5 +-
 srsue/hdr/phy/phch_worker.h         |  19 +-
 srsue/src/phy/phch_worker.cc        |  29 +--
 10 files changed, 155 insertions(+), 286 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index a4157d872..35f7d3e9f 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -318,7 +318,7 @@ void base_init() {
   }
 
   bzero(&pdsch, sizeof(srslte_pdsch_t));
-  if (srslte_pdsch_init_tx_multi(&pdsch, cell)) {
+  if (srslte_pdsch_init_tx(&pdsch, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     exit(-1);
   }
@@ -820,14 +820,13 @@ int main(int argc, char **argv) {
         /* Configure pdsch_cfg parameters */
         srslte_ra_dl_grant_t grant; 
         srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
+        if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
           fprintf(stderr, "Error configuring PDSCH\n");
           exit(-1);
         }
        
         /* Encode PDSCH */
-        if (srslte_pdsch_encode_multi(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI,
-                                      sf_symbols)) {
+        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI, sf_symbols)) {
           fprintf(stderr, "Error encoding PDSCH\n");
           exit(-1);
         }        
diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index 08cb5da4f..d39eed1e1 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -152,11 +152,11 @@ SRSLTE_API void srslte_enb_dl_rem_rnti(srslte_enb_dl_t *q,
 
 SRSLTE_API int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, 
                                        srslte_ra_dl_grant_t *grant, 
-                                       srslte_softbuffer_tx_t *softbuffer,
+                                       srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                                        uint16_t rnti,
                                        uint32_t rv_idx, 
                                        uint32_t sf_idx, 
-                                       uint8_t *data); 
+                                       uint8_t *data[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_dl(srslte_enb_dl_t *q, 
                                           srslte_ra_dl_dci_t *grant, 
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index a30851648..44ed161e8 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -78,15 +78,12 @@ typedef struct SRSLTE_API {
 
 } srslte_pdsch_t;
 
-SRSLTE_API int srslte_pdsch_init(srslte_pdsch_t *q, 
-                                 srslte_cell_t cell);
+SRSLTE_API int srslte_pdsch_init_tx(srslte_pdsch_t *q,
+                                    srslte_cell_t cell);
 
-SRSLTE_API int srslte_pdsch_init_tx_multi(srslte_pdsch_t *q,
-                                          srslte_cell_t cell);
-
-SRSLTE_API int srslte_pdsch_init_rx_multi(srslte_pdsch_t *q,
-                                          srslte_cell_t cell,
-                                          uint32_t nof_antennas);
+SRSLTE_API int srslte_pdsch_init_rx(srslte_pdsch_t *q,
+                                    srslte_cell_t cell,
+                                    uint32_t nof_antennas);
 
 SRSLTE_API void srslte_pdsch_free(srslte_pdsch_t *q);
 
@@ -103,23 +100,16 @@ SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg,
                                 uint32_t sf_idx, 
                                 int rvidx);
 
-SRSLTE_API int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg,
-                                      srslte_cell_t cell,
-                                      srslte_ra_dl_grant_t *grant,
-                                      uint32_t cfi,
-                                      uint32_t sf_idx,
-                                      int rvidx[SRSLTE_MAX_CODEWORDS],
-                                      srslte_mimo_type_t mimo_type,
-                                      uint32_t pmi);
+SRSLTE_API int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg,
+                                     srslte_cell_t cell,
+                                     srslte_ra_dl_grant_t *grant,
+                                     uint32_t cfi,
+                                     uint32_t sf_idx,
+                                     int rvidx[SRSLTE_MAX_CODEWORDS],
+                                     srslte_mimo_type_t mimo_type,
+                                     uint32_t pmi);
 
 SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
-                                   srslte_pdsch_cfg_t *cfg,
-                                   srslte_softbuffer_tx_t *softbuffer,
-                                   uint8_t *data, 
-                                   uint16_t rnti,
-                                   cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
-
-SRSLTE_API int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
                                          srslte_pdsch_cfg_t *cfg,
                                          srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                                          uint8_t *data[SRSLTE_MAX_CODEWORDS],
@@ -127,23 +117,14 @@ SRSLTE_API int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
                                          cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
 
 SRSLTE_API int srslte_pdsch_decode(srslte_pdsch_t *q, 
-                                   srslte_pdsch_cfg_t *cfg, 
-                                   srslte_softbuffer_rx_t *softbuffer,
-                                   cf_t *sf_symbols, 
-                                   cf_t *ce[SRSLTE_MAX_PORTS],
-                                   float noise_estimate, 
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                                   cf_t *sf_symbols[SRSLTE_MAX_PORTS],
+                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                   float noise_estimate,
                                    uint16_t rnti,
-                                   uint8_t *data);
-
-SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
-                                         srslte_pdsch_cfg_t *cfg, 
-                                         srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
-                                         cf_t *sf_symbols[SRSLTE_MAX_PORTS], 
-                                         cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                         float noise_estimate, 
-                                         uint16_t rnti,
-                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                                         bool acks[SRSLTE_MAX_CODEWORDS]);
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                   bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                        srslte_pdsch_cfg_t *cfg,
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index 4cc33d3b8..39606ff7f 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -83,7 +83,7 @@ int srslte_enb_dl_init(srslte_enb_dl_t *q, srslte_cell_t cell)
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init(&q->pdsch, q->cell)) {
+    if (srslte_pdsch_init_tx(&q->pdsch, q->cell)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
@@ -262,9 +262,9 @@ int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, srslte_ra_ul_dci_t *grant,
   return SRSLTE_SUCCESS;
 }
 
-int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer,
+int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                             uint16_t rnti, uint32_t rv_idx, uint32_t sf_idx, 
-                            uint8_t *data) 
+                            uint8_t *data[SRSLTE_MAX_CODEWORDS])
 {  
   /* Configure pdsch_cfg parameters */
   if (srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx)) {
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 9f0e4ed12..bc62df7fd 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -198,7 +198,7 @@ int srslte_pdsch_get(srslte_pdsch_t *q, cf_t *sf_symbols, cf_t *symbols,
 }
 
 /** Initializes the PDCCH transmitter or receiver */
-int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas, bool is_receiver)
+int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas, bool is_receiver)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   int i;
@@ -243,7 +243,6 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
       if (!q->d[i]) {
         goto clean;
       }
-
     }
 
     /* Layer mapped symbols memory allocation */
@@ -289,16 +288,12 @@ int srslte_pdsch_init_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_
   return ret;
 }
 
-int srslte_pdsch_init(srslte_pdsch_t *q, srslte_cell_t cell) {
-  return srslte_pdsch_init_multi(q, cell, 1, true);
-}
-
-int srslte_pdsch_init_tx_multi(srslte_pdsch_t *q, srslte_cell_t cell) {
-  return srslte_pdsch_init_multi(q, cell, 0, false);
+int srslte_pdsch_init_tx(srslte_pdsch_t *q, srslte_cell_t cell) {
+  return srslte_pdsch_init(q, cell, 0, false);
 }
 
-int srslte_pdsch_init_rx_multi(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas) {
-  return srslte_pdsch_init_multi(q, cell, nof_antennas, true);
+int srslte_pdsch_init_rx(srslte_pdsch_t *q, srslte_cell_t cell, uint32_t nof_antennas) {
+  return srslte_pdsch_init(q, cell, nof_antennas, true);
 }
 
 void srslte_pdsch_free(srslte_pdsch_t *q) {
@@ -346,9 +341,75 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
   }
 
   bzero(q, sizeof(srslte_pdsch_t));
+}
 
+/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while
+ * to execute, so shall be called once the final C-RNTI has been allocated for the session.
+ */
+int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
+  uint32_t i, j;
+  if (!q->users[rnti]) {
+    q->users[rnti] = calloc(1, sizeof(srslte_pdsch_user_t));
+    if (q->users[rnti]) {
+      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+        for (j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+          if (srslte_sequence_pdsch(&q->users[rnti]->seq[j][i], rnti, j, 2 * i, q->cell.id,
+                                    q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+            ERROR("Generating scrambling sequence");
+            return SRSLTE_ERROR;
+          }
+        }
+      }
+      q->users[rnti]->sequence_generated = true;
+    }
+  }
+  return SRSLTE_SUCCESS;
+}
+
+void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
+{
+  if (q->users[rnti]) {
+    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+        srslte_sequence_free(&q->users[rnti]->seq[j][i]);
+      }
+    }
+    free(q->users[rnti]);
+    q->users[rnti] = NULL;
+  }
 }
 
+static void pdsch_decode_debug(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
+                               cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS])
+{
+  if (SRSLTE_VERBOSE_ISDEBUG()) {
+    char filename[FILENAME_MAX];
+    for (int j = 0; j < q->nof_rx_antennas; j++) {
+      if (snprintf(filename, FILENAME_MAX, "subframe_p%d.dat", j) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: received subframe symbols\n", filename);
+      srslte_vec_save_file(filename, sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+
+      for (int i = 0; i < q->cell.nof_ports; i++) {
+        if (snprintf(filename, FILENAME_MAX, "hest_%d%d.dat", i, j) < 0) {
+          ERROR("Generating file name");
+          break;
+        }
+        DEBUG("SAVED FILE %s: channel estimates for Tx %d and Rx %d\n", filename, j, i);
+        srslte_vec_save_file(filename, ce[i][j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      }
+    }
+    DEBUG("SAVED FILE pdsch_symbols.dat: symbols after equalization\n",0);
+    srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits[0].nof_re*sizeof(cf_t));
+
+    DEBUG("SAVED FILE llr.dat: LLR estimates after demodulation and descrambling\n",0);
+    srslte_vec_save_file("llr.dat", q->e, cfg->nbits[0].nof_bits*sizeof(int16_t));
+  }
+}
+
+
 /* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg. 
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
@@ -357,13 +418,13 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
   int _rvids[SRSLTE_MAX_CODEWORDS] = {1};
   _rvids[0] = rvidx;
 
-  return srslte_pdsch_cfg_multi(cfg, cell, grant, cfi, sf_idx, _rvids, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
+  return srslte_pdsch_cfg_mimo(cfg, cell, grant, cfi, sf_idx, _rvids, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
 }
 
 /* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg.
  * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
  */
-int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
+int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
                            uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
                            uint32_t pmi) {
   if (cfg) {
@@ -425,29 +486,6 @@ int srslte_pdsch_cfg_multi(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_r
   }
 }
 
-/* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
- * to execute, so shall be called once the final C-RNTI has been allocated for the session.
- */
-int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
-  uint32_t i, j;
-  if (!q->users[rnti]) {
-    q->users[rnti] = calloc(1, sizeof(srslte_pdsch_user_t));
-    if (q->users[rnti]) {
-      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        for (j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
-          if (srslte_sequence_pdsch(&q->users[rnti]->seq[j][i], rnti, j, 2 * i, q->cell.id,
-                                    q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-            ERROR("Generating scrambling sequence");
-            return SRSLTE_ERROR;
-          }
-        }
-      }
-      q->users[rnti]->sequence_generated = true;
-    }
-  }
-  return SRSLTE_SUCCESS;
-}
-
 static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
                                                uint32_t codeword_idx) {
@@ -529,45 +567,14 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
   return SRSLTE_SUCCESS;
 }
 
-void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
-{
-  if (q->users[rnti]) {
-    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
-        srslte_sequence_free(&q->users[rnti]->seq[j][i]);
-      }
-    }
-    free(q->users[rnti]);
-    q->users[rnti] = NULL;
-  }
-}
-
-int srslte_pdsch_decode(srslte_pdsch_t *q,
-                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
-                        cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate,
-                        uint16_t rnti, uint8_t *data) {
-  cf_t *_sf_symbols[SRSLTE_MAX_PORTS];
-  cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  bool acks[SRSLTE_MAX_CODEWORDS];
-  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB] = {NULL};
-
-  _sf_symbols[0] = sf_symbols;
-  for (int i = 0; i < q->cell.nof_ports; i++) {
-    _ce[i][0] = ce[i];
-  }
-
-  softbuffers[0] = softbuffer;
-
-  return srslte_pdsch_decode_multi(q, cfg, softbuffers, _sf_symbols, _ce, noise_estimate, rnti, &data, acks);
-}
-
 /** Decodes the PDSCH from the received symbols
  */
-int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
-                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                              float noise_estimate, uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                              bool acks[SRSLTE_MAX_CODEWORDS]) {
+int srslte_pdsch_decode(srslte_pdsch_t *q,
+                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                        cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                        float noise_estimate, uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                        bool acks[SRSLTE_MAX_CODEWORDS])
+{
 
   /* Set pointers for layermapping & precoding */
   uint32_t i;
@@ -579,18 +586,17 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       cfg          != NULL)
   {
 
-    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d\n",
-        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb);
+    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, mimo_type=%d, nof_layers=%d, nof_tb=%d\n",
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, cfg->grant.nof_tb);
 
+    // Extract Symbols and Channel Estimates
     for (int j=0;j<q->nof_rx_antennas;j++) {
-      /* extract symbols */
       int n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
       if (n != cfg->nbits[0].nof_re) {
         fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
         return SRSLTE_ERROR;
       }
 
-      /* extract channel estimates */
       for (i = 0; i < q->cell.nof_ports; i++) {
         n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
         if (n != cfg->nbits[0].nof_re) {
@@ -600,8 +606,7 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
       }
     }
 
-    INFO("PDSCH Layer demapper and predecode: mimo_type=%d, nof_layers=%d, nof_tb=%d\n", cfg->mimo_type,
-         cfg->nof_layers, cfg->grant.nof_tb);
+    // Prepare layers
     int nof_symbols [SRSLTE_MAX_CODEWORDS];
     nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
     nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
@@ -612,53 +617,30 @@ int srslte_pdsch_decode_multi(srslte_pdsch_t *q,
         x[i] = q->d[i];
       }
     } else {
-        /* number of layers equals number of ports */
-        for (i = 0; i < cfg->nof_layers; i++) {
-          x[i] = q->x[i];
-        }
-        memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
+      /* number of layers equals number of ports */
+      for (i = 0; i < cfg->nof_layers; i++) {
+        x[i] = q->x[i];
+      }
+      memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
     }
 
-      srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+    // Pre-decoder
+    srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
                                     cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
+    // Layer demapping only if necessary
     if (cfg->nof_layers != cfg->grant.nof_tb) {
         srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 
-    if (SRSLTE_VERBOSE_ISDEBUG()) {
-      char filename[FILENAME_MAX];
-      for (int j = 0; j < q->nof_rx_antennas; j++) {
-        if (snprintf(filename, FILENAME_MAX, "subframe_p%d.dat", j) < 0) {
-          ERROR("Generating file name");
-          break;
-        }
-        DEBUG("SAVED FILE %s: received subframe symbols\n", filename);
-        srslte_vec_save_file(filename, sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-
-        for (i = 0; i < q->cell.nof_ports; i++) {
-          if (snprintf(filename, FILENAME_MAX, "hest_%d%d.dat", i, j) < 0) {
-            ERROR("Generating file name");
-            break;
-          }
-          DEBUG("SAVED FILE %s: channel estimates for Tx %d and Rx %d\n", filename, j, i);
-          srslte_vec_save_file(filename, ce[i][j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-        }
-      }
-      DEBUG("SAVED FILE pdsch_symbols.dat: symbols after equalization\n",0);
-      srslte_vec_save_file("pdsch_symbols.dat", q->d, cfg->nbits[0].nof_re*sizeof(cf_t));
-    }
-
+    // Codeword decoding
     for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
       int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
       acks[tb] = (ret == SRSLTE_SUCCESS);
     }
 
-    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[0].nof_bits*sizeof(int16_t));
-    }
+    pdsch_decode_debug(q, cfg, sf_symbols, ce);
 
     return SRSLTE_SUCCESS;
 
@@ -696,88 +678,10 @@ int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
 }
 
 int srslte_pdsch_encode(srslte_pdsch_t *q,
-                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
-                        uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
+                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                        uint8_t *data[SRSLTE_MAX_CODEWORDS], uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
 {
 
-  int i;
-  /* Set pointers for layermapping & precoding */
-  cf_t *x[SRSLTE_MAX_LAYERS];
-  int ret = SRSLTE_ERROR_INVALID_INPUTS;
-
-   if (q             != NULL &&
-       cfg  != NULL)
-  {
-
-    for (i=0;i<q->cell.nof_ports;i++) {
-      if (sf_symbols[i] == NULL) {
-        return SRSLTE_ERROR_INVALID_INPUTS;
-      }
-    }
-
-    if (cfg->grant.mcs[0].tbs == 0) {
-      return SRSLTE_ERROR_INVALID_INPUTS;
-    }
-
-    if (cfg->nbits[0].nof_re > q->max_re) {
-      fprintf(stderr,
-          "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
-          cfg->nbits[0].nof_re, q->max_re, q->cell.nof_prb);
-      return SRSLTE_ERROR_INVALID_INPUTS;
-    }
-
-    INFO("Encoding PDSCH SF: %d, Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs[0].mod), cfg->grant.mcs[0].tbs,
-         cfg->nbits[0].nof_re, cfg->nbits[0].nof_bits, cfg->rv[0]);
-
-    /* number of layers equals number of ports */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      x[i] = q->x[i];
-    }
-    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
-
-    if (srslte_dlsch_encode(&q->dl_sch, cfg, softbuffer, data, q->e[0])) {
-      fprintf(stderr, "Error encoding TB\n");
-      return SRSLTE_ERROR;
-    }
-
-    /* scramble */
-    if (q->users[rnti] && q->users[rnti]->sequence_generated) {
-      srslte_scrambling_bytes(&q->users[rnti]->seq[0][cfg->sf_idx], (uint8_t*) q->e[0], cfg->nbits[0].nof_bits);
-    } else {
-      srslte_sequence_t seq;
-      if (srslte_sequence_pdsch(&seq, rnti, 0, 2 * cfg->sf_idx, q->cell.id, cfg->nbits[0].nof_bits)) {
-        return SRSLTE_ERROR;
-      }
-      srslte_scrambling_bytes(&seq, (uint8_t*) q->e[0], cfg->nbits[0].nof_bits);
-      srslte_sequence_free(&seq);
-    }
-
-    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs[0].mod], (uint8_t*) q->e[0], q->d[0], cfg->nbits[0].nof_bits);
-
-    /* TODO: only diversity supported */
-    if (q->cell.nof_ports > 1) {
-      srslte_layermap_diversity(q->d[0], x, q->cell.nof_ports, cfg->nbits[0].nof_re);
-      srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
-          cfg->nbits[0].nof_re / q->cell.nof_ports);
-    } else {
-      memcpy(q->symbols[0], q->d, cfg->nbits[0].nof_re * sizeof(cf_t));
-    }
-
-    /* mapping to resource elements */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
-    }
-
-    ret = SRSLTE_SUCCESS;
-  }
-  return ret;
-}
-
-int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
-                              uint8_t *data[SRSLTE_MAX_CODEWORDS], uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) {
-
   int i;
   /* Set pointers for layermapping & precoding */
   cf_t *x[SRSLTE_MAX_LAYERS];
@@ -808,6 +712,7 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
       ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
     }
 
+    // Layer mapping & precode if necessary
     if (q->cell.nof_ports > 1) {
       int nof_symbols;
       /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
@@ -836,7 +741,6 @@ int srslte_pdsch_encode_multi(srslte_pdsch_t *q,
     }
 
     /* mapping to resource elements */
-
     for (i = 0; i < q->cell.nof_ports; i++) {
       srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
     }
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index ea40ce824..b98d75c22 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -232,7 +232,7 @@ int main(int argc, char **argv) {
 #endif /* DO_OFDM */
 
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg_multi(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, mimo_type, pmi)) {
+  if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     goto quit;
   }
@@ -276,7 +276,7 @@ int main(int argc, char **argv) {
     }
   }
 
-  if (srslte_pdsch_init_rx_multi(&pdsch_rx, cell, nof_rx_antennas)) {
+  if (srslte_pdsch_init_rx(&pdsch_rx, cell, nof_rx_antennas)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
@@ -345,7 +345,7 @@ int main(int argc, char **argv) {
     srslte_filesource_free(&fsrc);
   } else {
 
-    if (srslte_pdsch_init_tx_multi(&pdsch_tx, cell)) {
+    if (srslte_pdsch_init_tx(&pdsch_tx, cell)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto quit;
     }
@@ -385,7 +385,7 @@ int main(int argc, char **argv) {
     if (rv_idx[0] != 0 || rv_idx[1] != 0) {
       /* Do 1st transmission for rv_idx!=0 */
       bzero(pdsch_cfg.rv, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
-      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
+      if (srslte_pdsch_encode(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
         fprintf(stderr, "Error encoding PDSCH\n");
         goto quit;
       }
@@ -393,7 +393,7 @@ int main(int argc, char **argv) {
     memcpy(pdsch_cfg.rv, rv_idx, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
     gettimeofday(&t[1], NULL);
     for (k = 0; k < M; k++) {
-      if (srslte_pdsch_encode_multi(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
+      if (srslte_pdsch_encode(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
         ERROR("Error encoding PDSCH");
         goto quit;
       }
@@ -449,7 +449,7 @@ int main(int argc, char **argv) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
-    r = srslte_pdsch_decode_multi(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data_rx, acks);
+    r = srslte_pdsch_decode(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data_rx, acks);
   }
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 55f65ad3d..dfe9a18a7 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -97,7 +97,7 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_rx_multi(&q->pdsch, q->cell, nof_rx_antennas)) {
+    if (srslte_pdsch_init_rx(&q->pdsch, q->cell, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
@@ -315,7 +315,7 @@ int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint3
       }
     }
   }
-  return srslte_pdsch_cfg_multi(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
+  return srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
 }
 
 int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti) {
@@ -434,10 +434,10 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   
     
     if (q->pdsch_cfg.grant.mcs[0].mod > 0 && q->pdsch_cfg.grant.mcs[0].tbs >= 0) {
-      ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
-                                    q->sf_symbols_m, q->ce_m, 
-                                    noise_estimate, 
-                                    rnti, data, acks);
+      ret = srslte_pdsch_decode(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
+                                q->sf_symbols_m, q->ce_m,
+                                noise_estimate,
+                                rnti, data, acks);
 
       for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
         if (!acks[tb]) {
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 5c5537c61..d86bce9c9 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -647,8 +647,9 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
                              rnti, phy_grant.nof_prb, grant_str, grants[i].grant.harq_process, 
                              phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
-      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, grants[i].softbuffer, rnti, grants[i].grant.rv_idx, sf_idx, 
-                                  grants[i].data)) 
+      srslte_softbuffer_tx_t *sb[SRSLTE_MAX_CODEWORDS] = {grants[i].softbuffer, NULL};
+      uint8_t *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
+      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, grants[i].grant.rv_idx, sf_idx, d))
       {
         fprintf(stderr, "Error putting PDSCH %d\n",i);
         return SRSLTE_ERROR; 
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index a47008629..7b196f948 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -79,19 +79,12 @@ private:
   bool decode_phich(bool *ack);
 
   int decode_pdsch(srslte_ra_dl_grant_t *grant,
-                    uint8_t *payload, srslte_softbuffer_rx_t *softbuffer,
-                    int rv,
-                    uint16_t rnti,
-                    uint32_t pid,
-                    bool acks[SRSLTE_MAX_CODEWORDS]);
-
-  int decode_pdsch_multi(srslte_ra_dl_grant_t *grant,
-                          uint8_t *payload[SRSLTE_MAX_CODEWORDS],
-                          srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
-                          int rv[SRSLTE_MAX_CODEWORDS],
-                          uint16_t rnti,
-                          uint32_t pid,
-                          bool acks[SRSLTE_MAX_CODEWORDS]);
+                   uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                   int rv[SRSLTE_MAX_CODEWORDS],
+                   uint16_t rnti,
+                   uint32_t pid,
+                   bool acks[SRSLTE_MAX_CODEWORDS]);
 
   /* ... for UL */
   void encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, srslte_softbuffer_tx_t *softbuffer, 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 265d3e005..1959ba479 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -41,6 +41,7 @@
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
 #include <srslte/srslte.h>
+#include <srslte/interfaces/ue_interfaces.h>
 
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
@@ -210,12 +211,14 @@ void phch_worker::work_imp()
 
       /* Decode PDSCH if instructed to do so */
       if (dl_action.decode_enabled) {
-        decode_pdsch_multi(&dl_action.phy_grant.dl, dl_action.payload_ptr,
-                              dl_action.softbuffers, dl_action.rv, dl_action.rnti,
-                              dl_mac_grant.pid, dl_ack);
+        decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr,
+                      dl_action.softbuffers, dl_action.rv, dl_action.rnti,
+                      dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-        for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+
+        // NOTE: Currently hard-coded to 1st TB only
+        for (uint32_t tb = 0; tb < 1; tb++) {
           phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
           dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
           Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack);
@@ -418,19 +421,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
   }
 }
 
-int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
-                               srslte_softbuffer_rx_t *softbuffer, int rv,
-                               uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
-  int _rv [SRSLTE_MAX_TB] = {1};
-  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB] = {NULL};
-
-  _rv[0] = rv;
-  softbuffers[0] = softbuffer;
-
-  return decode_pdsch_multi(grant, &payload, softbuffers, _rv, rnti, harq_pid, acks);
-}
-
-int phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
                                      srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
                                      int rv[SRSLTE_MAX_CODEWORDS],
                                      uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
@@ -518,8 +509,8 @@ int phch_worker::decode_pdsch_multi(srslte_ra_dl_grant_t *grant, uint8_t *payloa
         struct timeval t[3];
         gettimeofday(&t[1], NULL);
   #endif
-        ret = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
-                                        ue_dl.ce_m, noise_estimate, rnti, payload, acks);
+        ret = srslte_pdsch_decode(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
+                                  ue_dl.ce_m, noise_estimate, rnti, payload, acks);
         if (ret) {
           Error("Decoding PDSCH");
         }

From 6e0fd43c1fd1f865e74ceee3b4589f244a539855 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 10:45:13 +0200
Subject: [PATCH 127/269] Moved FFTW to WISDOM and save/load from file

---
 lib/include/srslte/phy/dft/dft.h |  6 ++++--
 lib/src/phy/dft/dft_fftw.c       | 18 ++++++++++++++++--
 srsenb/src/enb.cc                |  2 ++
 srsue/hdr/ue_base.h              |  2 +-
 srsue/src/ue.cc                  |  2 +-
 srsue/src/ue_base.cc             |  8 ++++++++
 6 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/lib/include/srslte/phy/dft/dft.h b/lib/include/srslte/phy/dft/dft.h
index 03a04fcd5..b7fc663d8 100644
--- a/lib/include/srslte/phy/dft/dft.h
+++ b/lib/include/srslte/phy/dft/dft.h
@@ -72,9 +72,11 @@ typedef struct SRSLTE_API {
   srslte_dft_mode_t mode;   // Complex/Real
 }srslte_dft_plan_t;
 
-/* Create DFT plans */
+SRSLTE_API void srslte_dft_load();
 
-SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan, 
+SRSLTE_API void srslte_dft_exit();
+
+SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan,
                                int dft_points, 
                                srslte_dft_dir_t dir,                         
                                srslte_dft_mode_t type);
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index d841d7d33..134221c1a 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -37,6 +37,20 @@
 #define dft_ceil(a,b) ((a-1)/b+1)
 #define dft_floor(a,b) (a/b)
 
+#define FFTW_WISDOM_FILE ".fftw_wisdom"
+
+void srslte_dft_load() {
+  if (!fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE)) {
+    fprintf(stderr, "Error loading FFTW wisdom from file %s\n", FFTW_WISDOM_FILE);
+  }
+}
+
+void srslte_dft_exit() {
+  if (!fftwf_export_wisdom_to_filename(FFTW_WISDOM_FILE)) {
+    fprintf(stderr, "Error saving FFTW wisdom to file %s\n", FFTW_WISDOM_FILE);
+  }
+}
+
 int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
              srslte_dft_mode_t mode) {
   if(mode == SRSLTE_DFT_COMPLEX){
@@ -85,7 +99,7 @@ int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
-  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
   if (!plan->p) {
     return -1;
   }
@@ -108,7 +122,7 @@ int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
     fftwf_destroy_plan(plan->p);
     plan->p = NULL;
   }
-  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
   if (!plan->p) {
     return -1;
   }
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index 9a747dba6..bc3e9d7af 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -54,11 +54,13 @@ void enb::cleanup(void)
 enb::enb()
     :started(false)
 {
+  srslte_dft_load();
   pool = srslte::byte_buffer_pool::get_instance();
 }
 
 enb::~enb()
 {
+  srslte_dft_exit();
   srslte::byte_buffer_pool::cleanup();
 }
 
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index a03afd5e6..7765d43bd 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -135,7 +135,7 @@ class ue_base
     ,public ue_metrics_interface
 {
 public:
-  ue_base() {}
+  ue_base();
   virtual ~ue_base() {}
 
   static ue_base* get_instance(srsue_instance_type_t type);
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 216c4db23..135bb0591 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -179,7 +179,7 @@ bool ue::init(all_args_t *args_)
   phy.wait_initialize();
   phy.configure_ul_params();
 
-  printf("\n\nRequesting NAS Attach...\n");
+  printf("...\n");
   nas.attach_request();
 
   started = true;
diff --git a/srsue/src/ue_base.cc b/srsue/src/ue_base.cc
index 61cbafb2c..c84393ae4 100644
--- a/srsue/src/ue_base.cc
+++ b/srsue/src/ue_base.cc
@@ -57,8 +57,16 @@ ue_base* ue_base::get_instance(srsue_instance_type_t type)
   return(instance);
 }
 
+ue_base::ue_base() {
+  // load FFTW wisdom
+  srslte_dft_load();
+}
+
 void ue_base::cleanup(void)
 {
+  // save FFTW wisdom
+  srslte_dft_exit();
+
   pthread_mutex_lock(&ue_instance_mutex);
   if(NULL != instance) {
     delete instance;

From 38c67dc410fe80e3cc0b67001554e2957fc74d06 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 10:45:13 +0200
Subject: [PATCH 128/269] Moved FFTW to WISDOM and save/load from file

---
 lib/include/srslte/phy/dft/dft.h |  6 ++++--
 lib/src/phy/dft/dft_fftw.c       | 16 ++++++++++++++--
 srsenb/src/enb.cc                |  2 ++
 srsenb/src/main.cc               |  1 +
 srsue/hdr/ue_base.h              |  2 +-
 srsue/src/ue_base.cc             |  8 ++++++++
 6 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/lib/include/srslte/phy/dft/dft.h b/lib/include/srslte/phy/dft/dft.h
index 12c06dd98..b90aab2e5 100644
--- a/lib/include/srslte/phy/dft/dft.h
+++ b/lib/include/srslte/phy/dft/dft.h
@@ -71,9 +71,11 @@ typedef struct SRSLTE_API {
   srslte_dft_mode_t mode;   // Complex/Real
 }srslte_dft_plan_t;
 
-/* Create DFT plans */
+SRSLTE_API void srslte_dft_load();
 
-SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan, 
+SRSLTE_API void srslte_dft_exit();
+
+SRSLTE_API int srslte_dft_plan(srslte_dft_plan_t *plan,
                                int dft_points, 
                                srslte_dft_dir_t dir,                         
                                srslte_dft_mode_t type);
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 347e04547..5fb34bb3c 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -36,6 +36,18 @@
 #define dft_ceil(a,b) ((a-1)/b+1)
 #define dft_floor(a,b) (a/b)
 
+#define FFTW_WISDOM_FILE ".fftw_wisdom"
+
+void srslte_dft_load() {
+  fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE);
+}
+
+void srslte_dft_exit() {
+  if (!fftwf_export_wisdom_to_filename(FFTW_WISDOM_FILE)) {
+    fprintf(stderr, "Error saving FFTW wisdom to file %s\n", FFTW_WISDOM_FILE);
+  }
+}
+
 int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
              srslte_dft_mode_t mode) {
   if(mode == SRSLTE_DFT_COMPLEX){
@@ -54,7 +66,7 @@ static void allocate(srslte_dft_plan_t *plan, int size_in, int size_out, int len
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
-  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
   if (!plan->p) {
     return -1;
   }
@@ -73,7 +85,7 @@ int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
-  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
   if (!plan->p) {
     return -1;
   }
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index 9a747dba6..1bd7240a2 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -44,6 +44,7 @@ enb* enb::get_instance(void)
 }
 void enb::cleanup(void)
 {
+  srslte_dft_exit();
   boost::mutex::scoped_lock lock(enb_instance_mutex);
   if(NULL != instance) {
       delete instance;
@@ -54,6 +55,7 @@ void enb::cleanup(void)
 enb::enb()
     :started(false)
 {
+  srslte_dft_load();
   pool = srslte::byte_buffer_pool::get_instance();
 }
 
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 1e337b3ba..528878388 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -373,6 +373,7 @@ int main(int argc, char *argv[])
   pthread_cancel(input);
   metrics.stop();
   enb->stop();
+  enb->cleanup();
   cout << "---  exiting  ---" << endl;
   exit(0);
 }
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index 06c3f3e24..4720d8062 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -134,7 +134,7 @@ class ue_base
     ,public ue_metrics_interface
 {
 public:
-  ue_base() {}
+  ue_base();
   virtual ~ue_base() {}
 
   static ue_base* get_instance(srsue_instance_type_t type);
diff --git a/srsue/src/ue_base.cc b/srsue/src/ue_base.cc
index 61cbafb2c..c84393ae4 100644
--- a/srsue/src/ue_base.cc
+++ b/srsue/src/ue_base.cc
@@ -57,8 +57,16 @@ ue_base* ue_base::get_instance(srsue_instance_type_t type)
   return(instance);
 }
 
+ue_base::ue_base() {
+  // load FFTW wisdom
+  srslte_dft_load();
+}
+
 void ue_base::cleanup(void)
 {
+  // save FFTW wisdom
+  srslte_dft_exit();
+
   pthread_mutex_lock(&ue_instance_mutex);
   if(NULL != instance) {
     delete instance;

From a993e725341d7951133b7e9e2cc04d82add0c55c Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 10:54:49 +0200
Subject: [PATCH 129/269] correction to previous commit

---
 lib/src/phy/dft/dft_fftw.c | 4 +---
 srsenb/src/enb.cc          | 2 +-
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 134221c1a..0f77c21d9 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -40,9 +40,7 @@
 #define FFTW_WISDOM_FILE ".fftw_wisdom"
 
 void srslte_dft_load() {
-  if (!fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE)) {
-    fprintf(stderr, "Error loading FFTW wisdom from file %s\n", FFTW_WISDOM_FILE);
-  }
+  fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE);
 }
 
 void srslte_dft_exit() {
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index bc3e9d7af..1bd7240a2 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -44,6 +44,7 @@ enb* enb::get_instance(void)
 }
 void enb::cleanup(void)
 {
+  srslte_dft_exit();
   boost::mutex::scoped_lock lock(enb_instance_mutex);
   if(NULL != instance) {
       delete instance;
@@ -60,7 +61,6 @@ enb::enb()
 
 enb::~enb()
 {
-  srslte_dft_exit();
   srslte::byte_buffer_pool::cleanup();
 }
 

From 741f5b7a0af4cd920f318a8b2ad189e41764f5a8 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 4 Sep 2017 11:24:52 +0200
Subject: [PATCH 130/269] Corrected some logging messages

---
 srsue/hdr/mac/ul_harq.h      | 6 +++---
 srsue/src/mac/mac.cc         | 2 +-
 srsue/src/mac/proc_ra.cc     | 2 +-
 srsue/src/phy/phch_worker.cc | 2 +-
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 13ec0b9b3..952b9f002 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -326,11 +326,11 @@ private:
         memcpy(&cur_grant, grant, sizeof(Tgrant));
         harq_feedback = false;
         Info("UL %d:  Adaptive retx=%d, RV=%d, TBS=%d\n",
-             pid, current_tx_nb, get_rv(), grant->n_bytes);
+             pid, current_tx_nb, get_rv(), grant->n_bytes[0]);
         generate_tx(tti_tx, action);
       } else {
         Info("UL %d:  Non-Adaptive retx=%d, RV=%d, TBS=%d\n",
-             pid, current_tx_nb, get_rv(), cur_grant.n_bytes);
+             pid, current_tx_nb, get_rv(), cur_grant.n_bytes[0]);
         // HARQ entity requests a non-adaptive transmission
         if (!harq_feedback) {
           generate_tx(tti_tx, action);
@@ -358,7 +358,7 @@ private:
         current_irv = 0;
         is_msg3 = is_msg3_;
         Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
-             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes, cur_grant.rnti);
+             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0], cur_grant.rnti);
         generate_tx(tti_tx, action);
       }
     }
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 7c569358a..6028ce7e8 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -288,7 +288,7 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
     action->rnti = grant.rnti;
     action->rv[0]   = grant.rv[0];
     if (grant.n_bytes[0] > pch_payload_buffer_sz) {
-      Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes, pch_payload_buffer_sz);
+      Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes[0], pch_payload_buffer_sz);
       action->decode_enabled = false; 
     }
   } else {
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 213042018..98fed02fc 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -285,7 +285,7 @@ void ra_proc::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_p
       srslte_softbuffer_rx_reset(&softbuffer_rar);
     }
   } else {
-    rError("Received RAR grant exceeds buffer length (%d>%d)\n", grant.n_bytes, MAX_RAR_PDU_LEN);
+    rError("Received RAR grant exceeds buffer length (%d>%d)\n", grant.n_bytes[0], MAX_RAR_PDU_LEN);
     action->decode_enabled = false; 
     state = RESPONSE_ERROR;
   }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 1959ba479..3c05cc463 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -221,7 +221,7 @@ void phch_worker::work_imp()
         for (uint32_t tb = 0; tb < 1; tb++) {
           phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
           dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack);
+          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
         }
       }
       Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);

From 9ae21dfd5dcb05400171c5d6bc9e541288a6b6cd Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 12:26:58 +0200
Subject: [PATCH 131/269] reattaching cells with different IDs and PRB

---
 lib/include/srslte/phy/phch/pdsch.h |  1 +
 lib/include/srslte/phy/phch/pusch.h |  3 +-
 lib/src/phy/enb/enb_ul.c            |  2 +-
 lib/src/phy/phch/pdsch.c            | 36 ++++++++++++++++-------
 lib/src/phy/phch/pusch.c            | 45 +++++++++++++++++++----------
 lib/src/phy/ue/ue_ul.c              |  2 +-
 srsue/hdr/phy/phch_recv.h           |  6 +++-
 srsue/hdr/upper/rrc.h               |  1 +
 srsue/src/main.cc                   |  1 -
 srsue/src/phy/phch_recv.cc          | 19 ++++++------
 srsue/src/phy/phch_worker.cc        |  2 +-
 srsue/src/upper/rrc.cc              | 28 +++++++++++-------
 12 files changed, 94 insertions(+), 52 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 4f396dd6d..b163f45bf 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -49,6 +49,7 @@
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  uint32_t cell_id;
   bool sequence_generated;
 } srslte_pdsch_user_t;
 
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index 3e7a5d258..834750e38 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -62,6 +62,7 @@ typedef struct {
 
 typedef struct {
   srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  uint32_t cell_id;
   bool sequence_generated;
 } srslte_pusch_user_t;
 
@@ -121,7 +122,7 @@ SRSLTE_API int srslte_pusch_cfg(srslte_pusch_t             *q,
 SRSLTE_API int srslte_pusch_set_rnti(srslte_pusch_t *q, 
                                      uint16_t rnti);
 
-SRSLTE_API void srslte_pusch_clear_rnti(srslte_pusch_t *q, 
+SRSLTE_API void srslte_pusch_free_rnti(srslte_pusch_t *q,
                                        uint16_t rnti);
 
 SRSLTE_API int srslte_pusch_encode(srslte_pusch_t *q, 
diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index 03b10e7cd..f756c021f 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -220,7 +220,7 @@ void srslte_enb_ul_rem_rnti(srslte_enb_ul_t *q, uint16_t rnti)
   if (q->users[rnti]) {
     free(q->users[rnti]); 
     q->users[rnti] = NULL; 
-    srslte_pusch_clear_rnti(&q->pusch, rnti);
+    srslte_pusch_free_rnti(&q->pusch, rnti);
   }
 }
 
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 4e28a7832..a10be3e09 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <srslte/phy/phch/pdsch.h>
 #include "srslte/phy/phch/pdsch.h"
 #include "prb_dl.h"
 #include "srslte/phy/utils/debug.h"
@@ -386,17 +387,27 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
   uint32_t i;
   uint32_t rnti_idx = q->is_ue?0:rnti;
 
-  if (!q->users[rnti_idx]) {
-    q->users[rnti_idx] = calloc(1, sizeof(srslte_pdsch_user_t));
-    if (q->users[rnti_idx]) {
-      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[i], rnti, 0, 2 * i, q->cell.id,
-            q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          return SRSLTE_ERROR; 
-        }
+  if (!q->users[rnti_idx] || q->is_ue) {
+    if (!q->users[rnti_idx]) {
+      q->users[rnti_idx] = calloc(1, sizeof(srslte_pdsch_user_t));
+      if(!q->users[rnti_idx]) {
+        perror("calloc");
+        return -1;
+      }
+    }
+    for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[i], rnti, 0, 2 * i, q->cell.id,
+          q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
+      {
+        fprintf(stderr, "Error initializing PDSCH scrambling sequence\n");
+        srslte_pdsch_free_rnti(q, rnti);
+        return SRSLTE_ERROR;
       }
-      q->users[rnti_idx]->sequence_generated = true;
     }
+    q->users[rnti_idx]->cell_id = q->cell.id;
+    q->users[rnti_idx]->sequence_generated = true;
+  } else {
+    fprintf(stderr, "Error generating PDSCH sequence: rnti=0x%x already generated\n", rnti);
   }
   return SRSLTE_SUCCESS;
 }
@@ -431,7 +442,12 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
 static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
 {
   uint32_t rnti_idx = q->is_ue?0:rnti;
-  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated) {
+
+  // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
+      q->users[rnti_idx]->cell_id == q->cell.id &&
+      ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
+  {
     return &q->users[rnti_idx]->seq[sf_idx];
   } else {
     srslte_sequence_pdsch(&q->tmp_seq, rnti, 0, 2 * sf_idx, q->cell.id, len);
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 237036a57..6769f43d4 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -293,10 +293,10 @@ void srslte_pusch_free(srslte_pusch_t *q) {
 
   if (q->users) {
     if (q->is_ue) {
-      srslte_pusch_clear_rnti(q, 0);
+      srslte_pusch_free_rnti(q, 0);
     } else {
       for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {
-        srslte_pusch_clear_rnti(q, rnti);
+        srslte_pusch_free_rnti(q, rnti);
       }
     }
     free(q->users);
@@ -435,25 +435,33 @@ int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
   uint32_t i;
 
   uint32_t rnti_idx = q->is_ue?0:rnti; 
-  
-  if (!q->users[rnti_idx]) {
-    q->users[rnti_idx] = calloc(1, sizeof(srslte_pusch_user_t));
-    if (q->users[rnti_idx]) {
-      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-        if (srslte_sequence_pusch(&q->users[rnti_idx]->seq[i], rnti, 2 * i, q->cell.id,
-            q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
-          fprintf(stderr, "Error initializing PUSCH scrambling sequence\n");
-          srslte_pusch_clear_rnti(q, rnti);
-          return SRSLTE_ERROR;
-        }
+
+  if (!q->users[rnti_idx] || q->is_ue) {
+    if (!q->users[rnti_idx]) {
+      q->users[rnti_idx] = calloc(1, sizeof(srslte_pusch_user_t));
+      if (!q->users[rnti_idx]) {
+        perror("calloc");
+        return -1;
       }
-      q->users[rnti_idx]->sequence_generated = true;
     }
+    for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      if (srslte_sequence_pusch(&q->users[rnti_idx]->seq[i], rnti, 2 * i, q->cell.id,
+                                q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
+      {
+        fprintf(stderr, "Error initializing PUSCH scrambling sequence\n");
+        srslte_pusch_free_rnti(q, rnti);
+        return SRSLTE_ERROR;
+      }
+    }
+    q->users[rnti_idx]->cell_id = q->cell.id;
+    q->users[rnti_idx]->sequence_generated = true;
+  } else {
+    fprintf(stderr, "Error generating PUSCH sequence: rnti=0x%x already generated\n", rnti);
   }
   return SRSLTE_SUCCESS;
 }
 
-void srslte_pusch_clear_rnti(srslte_pusch_t *q, uint16_t rnti) {
+void srslte_pusch_free_rnti(srslte_pusch_t *q, uint16_t rnti) {
 
   uint32_t rnti_idx = q->is_ue?0:rnti;
 
@@ -470,7 +478,12 @@ void srslte_pusch_clear_rnti(srslte_pusch_t *q, uint16_t rnti) {
 static srslte_sequence_t *get_user_sequence(srslte_pusch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
 {
   uint32_t rnti_idx = q->is_ue?0:rnti;
-  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated) {
+
+  // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
+  if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
+      q->users[rnti_idx]->cell_id == q->cell.id &&
+      ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
+  {
     return &q->users[rnti_idx]->seq[sf_idx];
   } else {
     srslte_sequence_pusch(&q->tmp_seq, rnti, 2 * sf_idx, q->cell.id, len);
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 2792cff09..82f0eec91 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -446,7 +446,7 @@ int srslte_ue_ul_pusch_encode_rnti_softbuffer(srslte_ue_ul_t *q,
                                              q->pusch_cfg.grant.n_prb_tilde, 
                                              q->sf_symbols);
     } else {
-   
+
       if (srslte_refsignal_dmrs_pusch_gen(&q->signals, q->pusch_cfg.grant.L_prb, 
                                           q->pusch_cfg.sf_idx,
                                           q->pusch_cfg.grant.ncs_dmrs,
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index c50252ae2..5bf9142d0 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -111,7 +111,11 @@ private:
   sync_metrics_t metrics;
 
   enum {
-    IDLE, CELL_SEARCH, CELL_MEASURE, CELL_SELECT, CELL_CAMP
+    IDLE = 0,
+    CELL_SEARCH,
+    CELL_SELECT,
+    CELL_MEASURE,
+    CELL_CAMP
   } phy_state;
 
   bool is_in_idle;
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 4eeb1f7d9..bafbdc03c 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -137,6 +137,7 @@ private:
     float    rsrp;
     bool     has_valid_sib1;
     bool     has_valid_sib2;
+    bool     in_sync;
     LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT sib1;
     LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT sib2;
   } cell_t;
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 78356c478..d2a75604d 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -349,7 +349,6 @@ int main(int argc, char *argv[])
   all_args_t args;
   parse_args(&args, argc, argv);
 
-
   srsue_instance_type_t type = LTE;
   ue_base *ue = ue_base::get_instance(type);
   if (!ue) {
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 6142cfaf0..55389059b 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -433,8 +433,7 @@ void phch_recv::cell_search_start() {
     cell_search_in_progress = true;
     cur_earfcn_index = -1;
     cell_search_next();
-    log_h->console("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
-    log_h->info("Cell Search: Starting procedure...\n");
+    log_h->info("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
   } else {
     log_h->info("Empty EARFCN list. Stopping cell search...\n");
     log_h->console("Empty EARFCN list. Stopping cell search...\n");
@@ -445,12 +444,12 @@ bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
   // Check if we are already camping in this cell
   if (earfcn == current_earfcn && this->cell.id == cell.id) {
-    log_h->info("Cell Select: Already in Cell EARFCN=%d\n", earfcn);
-      cell_search_in_progress = false;
+    log_h->info("Cell Select: Already in cell EARFCN=%d\n", earfcn);
+    cell_search_in_progress = false;
     if (srate_mode != SRATE_CAMP) {
       set_sampling_rate();
     }
-    if (phy_state != CELL_SELECT) {
+    if (phy_state < CELL_SELECT) {
       resync_sfn();
     }
     return true;
@@ -491,11 +490,11 @@ bool phch_recv::set_frequency()
     log_h->info("Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
 
-    log_h->console("Tunning to EARFCN=%d, F_dl=%.1f MHz, F_ul=%.1f MHz\n",
+    log_h->console("Searching cell in DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
 
-    radio_h->set_rx_freq(dl_freq);
-    radio_h->set_tx_freq(ul_freq);
+    radio_h->set_rx_freq(dl_freq-4000);
+    radio_h->set_tx_freq(ul_freq-4000);
     ul_dl_factor = ul_freq / dl_freq;
 
     srslte_ue_sync_reset(&ue_sync);
@@ -567,7 +566,7 @@ void phch_recv::run_thread() {
             srslte_ue_sync_set_agc_period(&ue_sync, 20);
             if (!cell_search_in_progress) {
               phy_state = CELL_CAMP;
-              log_h->console("Sync OK. Camping on cell PCI=%d...\n", cell.id);
+              log_h->info("Sync OK. Camping on cell PCI=%d...\n", cell.id);
             } else {
               measure_cnt  = 0;
               measure_rsrp = 0;
@@ -587,9 +586,9 @@ void phch_recv::run_thread() {
       case CELL_MEASURE:
         switch(cell_meas_rsrp()) {
           case 1:
+            log_h->info("Measured OK. Camping on cell PCI=%d...\n", cell.id);
             phy_state = CELL_CAMP;
             rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
-            log_h->info("Measured OK. Camping on cell PCI=%d...\n", cell.id);
             break;
           case 0:
             break;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 1d4128d1e..ecbbc8768 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -472,7 +472,7 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
   #endif
 
         Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n",
-              grant->nof_prb, harq_pid, 
+              grant->nof_prb, harq_pid,
               grant->mcs.tbs/8, grant->mcs.idx, rv, 
               ack?"OK":"KO", 
               10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), 
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 0b3991e16..1b24d05c1 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -30,6 +30,7 @@
 #include "srslte/asn1/liblte_rrc.h"
 #include "upper/rrc.h"
 #include <boost/assign.hpp>
+#include <upper/rrc.h>
 #include "srslte/common/security.h"
 #include "srslte/common/bcd_helpers.h"
 
@@ -187,17 +188,21 @@ void rrc::select_next_cell_in_plmn() {
         rrc_log->info("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
                       known_cells[i].phy_cell.id, known_cells[i].earfcn,
                       known_cells[i].sib1.cell_id);
-        rrc_log->console("Selecting cell PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
+        rrc_log->console("Select cell: PCI=%d, EARFCN=%d, Cell ID=0x%x\n",
                       known_cells[i].phy_cell.id, known_cells[i].earfcn,
                       known_cells[i].sib1.cell_id);
         // Check that cell satisfies S criteria
-        if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell)) {
-          last_selected_cell = i;
-          current_cell = &known_cells[i];
-          return;
-        } else {
-          rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
-                         known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+        if (known_cells[i].in_sync) { // %% rsrp > S dbm
+          // Try to select Cell
+          if (phy->cell_select(known_cells[i].earfcn, known_cells[i].phy_cell))
+          {
+            last_selected_cell = i;
+            current_cell = &known_cells[i];
+            return;
+          } else {
+            rrc_log->warning("Selecting cell EARFCN=%d, Cell ID=0x%x.\n",
+                           known_cells[i].earfcn, known_cells[i].sib1.cell_id);
+          }
         }
       }
     }
@@ -214,7 +219,8 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
   // find if cell_id-earfcn combination already exists
   for (uint32_t i = 0; i < known_cells.size(); i++) {
     if (earfcn == known_cells[i].earfcn && phy_cell.id == known_cells[i].phy_cell.id) {
-      known_cells[i].rsrp = rsrp;
+      known_cells[i].rsrp    = rsrp;
+      known_cells[i].in_sync = true;
       current_cell = &known_cells[i];
       rrc_log->info("Updating cell EARFCN=%d, PCI=%d, RSRP=%.1f dBm\n", known_cells[i].earfcn,
                     known_cells[i].phy_cell.id, known_cells[i].rsrp);
@@ -255,6 +261,7 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
 // Detection of physical layer problems (5.3.11.1)
 void rrc::out_of_sync() {
+    current_cell->in_sync = false;
   if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
     n310_cnt++;
     if (n310_cnt == N310) {
@@ -268,6 +275,7 @@ void rrc::out_of_sync() {
 
 // Recovery of physical layer problems (5.3.11.2)
 void rrc::in_sync() {
+  current_cell->in_sync = true;
   if (mac_timers->get(t310)->is_running()) {
     n311_cnt++;
     if (n311_cnt == N311) {
@@ -1087,7 +1095,7 @@ void rrc::reset_ue() {
   mac->pcch_start_rx();
   mac_timers->get(safe_reset_timer)->stop();
   mac_timers->get(safe_reset_timer)->reset();
-  rrc_log->console("RRC Connection released.\n");
+  rrc_log->console("RRC Connection Released.\n");
 }
 
 void rrc::rrc_connection_release() {

From 1486911e32e3a0c88ca2140544bc1bdeb1682c54 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 4 Sep 2017 15:28:18 +0200
Subject: [PATCH 132/269] DCI blind search takes Transmission mode in
 consideration and some _multi functions removed

---
 lib/examples/cell_measurement.c               |   4 +-
 lib/examples/pdsch_ue.c                       |  34 ++++--
 lib/include/srslte/phy/ue/ue_dl.h             |  52 ++++-----
 lib/src/phy/phch/test/pdsch_pdcch_file_test.c |  20 ++--
 lib/src/phy/ue/ue_dl.c                        | 105 ++++++++----------
 srsue/src/phy/phch_worker.cc                  |   7 +-
 6 files changed, 106 insertions(+), 116 deletions(-)

diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index c6135984c..cb6a9b78d 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -245,7 +245,7 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error initiating ue_sync\n");
     return -1; 
   }
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, 1)) { 
+  if (srslte_ue_dl_init(&ue_dl, cell, 1)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     return -1;
   }
@@ -314,7 +314,7 @@ int main(int argc, char **argv) {
         case DECODE_SIB:
           /* We are looking for SI Blocks, search only in appropiate places */
           if ((srslte_ue_sync_get_sfidx(&ue_sync) == 5 && (sfn%2)==0)) {
-            n = srslte_ue_dl_decode_multi(&ue_dl, sf_buffer, data, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
+            n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
             if (n < 0) {
               fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
               return -1;
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 0a5d2d81d..2ace49d6d 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -242,6 +242,7 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
       break;
     case 'v':
       srslte_verbose++;
+      args->verbose = srslte_verbose;
       break;
     case 'Z':
       args->decimate = atoi(argv[optind]);
@@ -323,7 +324,7 @@ int main(int argc, char **argv) {
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
   int sfn_offset;
   float cfo = 0; 
-  
+
   parse_args(&prog_args, argc, argv);
   
   for (int i = 0; i< SRSLTE_MAX_CODEWORDS; i++) {
@@ -486,7 +487,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }    
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, prog_args.rf_nof_rx_ant)) {  // This is the User RNTI
+  if (srslte_ue_dl_init(&ue_dl, cell, prog_args.rf_nof_rx_ant)) {  // This is the User RNTI
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     exit(-1);
   }
@@ -608,13 +609,30 @@ int main(int argc, char **argv) {
               decode_pdsch = false; 
             }
           }
-          if (decode_pdsch) {            
-            INFO("Attempting DL decode SFN=%d\n", sfn);
-            n = srslte_ue_dl_decode_multi(&ue_dl, 
-                                          sf_buffer, 
-                                          data, 
-                                          sfn*10+srslte_ue_sync_get_sfidx(&ue_sync),
+
+          INFO("Attempting DL decode SFN=%d\n", sfn);
+          if (decode_pdsch) {
+            if (cell.nof_ports == 1) {
+              /* Transmission mode 1 */
+              n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
+            } else {
+              if (prog_args.rf_nof_rx_ant == 1) {
+                /* Transmission mode 2 */
+                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 1, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                        acks);
+              } else {
+                /* Transmission mode 3 */
+                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 2, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                        acks);
+                if (n < 1) {
+                  /* Transmission mode 4 */
+                  n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 3, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
                                           acks);
+                }
+              }
+            }
+
+
           
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 416d467f8..d0723fae7 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -118,21 +118,13 @@ typedef struct SRSLTE_API {
 }srslte_ue_dl_t;
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                                 srslte_cell_t cell);
-
-SRSLTE_API int srslte_ue_dl_init_multi(srslte_ue_dl_t *q, 
-                                       srslte_cell_t cell, 
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q,
+                                 srslte_cell_t cell,
+                                 uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_ue_dl_free(srslte_ue_dl_t *q);
 
-SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, 
-                                                cf_t *input, 
-                                                uint32_t sf_idx, 
-                                                uint32_t *cfi); 
-
-SRSLTE_API int srslte_ue_dl_decode_fft_estimate_multi(srslte_ue_dl_t *q, 
+SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q,
                                                 cf_t *input[SRSLTE_MAX_PORTS], 
                                                 uint32_t sf_idx, 
                                                 uint32_t *cfi); 
@@ -155,12 +147,14 @@ SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q,
                                         srslte_dci_msg_t *dci_msg); 
 
 SRSLTE_API int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, 
+                                        uint32_t tm,
                                         uint32_t cfi, 
                                         uint32_t sf_idx, 
                                         uint16_t rnti, 
                                         srslte_dci_msg_t *dci_msg); 
 
 SRSLTE_API int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, 
+                                             uint32_t tm,
                                              uint32_t cfi, 
                                              uint32_t sf_idx, 
                                              uint16_t rnti, 
@@ -172,28 +166,20 @@ SRSLTE_API uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q);
 SRSLTE_API void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, 
                                                float sample_offset); 
 
-SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t * q, 
-                                   cf_t *input, 
-                                   uint8_t *data,
-                                   uint32_t tti);
-
-SRSLTE_API int srslte_ue_dl_decode_multi(srslte_ue_dl_t * q, 
-                                         cf_t *input[SRSLTE_MAX_PORTS], 
-                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                                         uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]);
-
-SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q, 
-                                        cf_t *input, 
-                                        uint8_t *data,
+SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t *q,
+                                   cf_t *input[SRSLTE_MAX_PORTS],
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                   uint32_t tm,
+                                   uint32_t tti,
+                                   bool acks[SRSLTE_MAX_CODEWORDS]);
+
+SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
+                                        cf_t *input[SRSLTE_MAX_PORTS],
+                                        uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                        uint32_t tm,
                                         uint32_t tti,
-                                        uint16_t rnti);
-
-SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q,
-                                              cf_t *input[SRSLTE_MAX_PORTS], 
-                                              uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                                              uint32_t tti,
-                                              uint16_t rnti,
-                                              bool acks[SRSLTE_MAX_CODEWORDS]);
+                                        uint16_t rnti,
+                                        bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           uint32_t *ri,
diff --git a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
index 0d19d689e..c32f6ac76 100644
--- a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
@@ -54,7 +54,7 @@ uint32_t sf_idx = 0;
 srslte_dci_format_t dci_format = SRSLTE_DCI_FORMAT1A;
 srslte_filesource_t fsrc;
 srslte_ue_dl_t ue_dl;
-cf_t *input_buffer;
+cf_t *input_buffer[SRSLTE_MAX_PORTS];
 
 void usage(char *prog) {
   printf("Usage: %s [rovfcenmps] -i input_file\n", prog);
@@ -131,13 +131,13 @@ int base_init() {
 
   flen = 2 * (SRSLTE_SLOT_LEN(srslte_symbol_sz(cell.nof_prb)));
 
-  input_buffer = malloc(flen * sizeof(cf_t));
-  if (!input_buffer) {
+  input_buffer[0] = malloc(flen * sizeof(cf_t));
+  if (!input_buffer[0]) {
     perror("malloc");
     exit(-1);
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, 1)) {
+  if (srslte_ue_dl_init(&ue_dl, cell, 1)) {
     fprintf(stderr, "Error initializing UE DL\n");
     return -1;
   }
@@ -151,12 +151,13 @@ int base_init() {
 void base_free() {
   srslte_filesource_free(&fsrc);
   srslte_ue_dl_free(&ue_dl);
-  free(input_buffer);  
+  free(input_buffer[0]);
 }
 
 int main(int argc, char **argv) {
   int nof_frames;
   int ret;
+  bool acks[SRSLTE_MAX_TB];
 
   if (argc < 3) {
     usage(argv[0]);
@@ -169,15 +170,15 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  uint8_t *data = malloc(100000);
+  uint8_t *data[] = {malloc(100000)};
 
   ret = -1;
   nof_frames = 0;
   do {
-    srslte_filesource_read(&fsrc, input_buffer, flen);
+    srslte_filesource_read(&fsrc, input_buffer[0], flen);
     INFO("Reading %d samples sub-frame %d\n", flen, sf_idx);
 
-    ret = srslte_ue_dl_decode(&ue_dl, input_buffer, data, sf_idx); 
+    ret = srslte_ue_dl_decode(&ue_dl, input_buffer, data, 0, sf_idx, acks);
     if(ret > 0) {
       printf("PDSCH Decoded OK!\n");       
     } else if (ret == 0) {
@@ -190,7 +191,8 @@ int main(int argc, char **argv) {
   } while (nof_frames <= max_frames && ret == 0);
 
   base_free();
-  if (ret > 0) {        
+  free(data[0]);
+  if (ret > 0) {
     exit(0);
   } else {
     exit(-1); 
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index dfe9a18a7..888b0d9b3 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -26,8 +26,6 @@
 
 #include "srslte/phy/ue/ue_dl.h"
 
-#include <complex.h>
-#include <math.h>
 #include <string.h>
 
 
@@ -38,21 +36,23 @@
 #define CURRENT_SFLEN_RE SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)
 
 
-static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT2A, SRSLTE_DCI_FORMAT2}; // Only TM1, TM2, TM3 and TM4 are currently supported
-const uint32_t nof_ue_formats = 4;
+const static srslte_dci_format_t ue_dci_formats[8][2] = {
+    /* Mode 1 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 2 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 3 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2A},
+    /* Mode 4 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2},
+    /* Mode 5 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1D},
+    /* Mode 6 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1B},
+    /* Mode 7 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 8 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2B}
+};
 
 static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
 
-int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                      srslte_cell_t cell) 
-{
-  return srslte_ue_dl_init_multi(q, cell, 1);
-}
-
-int srslte_ue_dl_init_multi(srslte_ue_dl_t *q, 
-                            srslte_cell_t cell, 
-                            uint32_t nof_rx_antennas) 
+int srslte_ue_dl_init(srslte_ue_dl_t *q,
+                      srslte_cell_t cell,
+                      uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
@@ -118,7 +118,7 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
       fprintf(stderr, "Error initiating SFO correct\n");
       goto clean_exit;
     }
-    srslte_cfo_set_tol(&q->sfo_correct, 1e-5/q->fft.symbol_sz);
+    srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft.symbol_sz);
     
     for (int j=0;j<nof_rx_antennas;j++) {
       q->sf_symbols_m[j] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
@@ -219,28 +219,12 @@ void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
  *    - PDCCH decoding: Find DCI for RNTI given by previous call to srslte_ue_dl_set_rnti()
  *    - PDSCH decoding: Decode TB scrambling with RNTI given by srslte_ue_dl_set_rnti()
  */
-int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti) {
-  uint8_t *_data[SRSLTE_MAX_CODEWORDS];
-  cf_t *_input[SRSLTE_MAX_PORTS];
-  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
-  _data[0] = data;
-  _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, q->current_rnti, acks);
-}
-
-int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS],
-                              uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]) {
-  return srslte_ue_dl_decode_rnti_multi(q, input, data, tti, q->current_rnti, acks);
-}
-
-int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input, uint32_t sf_idx, uint32_t *cfi) 
-{
-  cf_t *_input[SRSLTE_MAX_PORTS]; 
-  _input[0] = input; 
-  return srslte_ue_dl_decode_fft_estimate_multi(q, _input, sf_idx, cfi);
+int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                              uint32_t tm, uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]) {
+  return srslte_ue_dl_decode_rnti(q, input, data, tm, tti, q->current_rnti, acks);
 }
 
-int srslte_ue_dl_decode_fft_estimate_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi) 
+int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi)
 {
   if (input && q && cfi && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
     
@@ -318,19 +302,9 @@ int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint3
   return srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
 }
 
-int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti) {
-  uint8_t *_data[SRSLTE_MAX_CODEWORDS];
-  cf_t *_input[SRSLTE_MAX_PORTS];
-  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
-  _input[0] = input; 
-  _data[0] = data;
-  return srslte_ue_dl_decode_rnti_multi(q, _input, _data, tti, rnti, acks);
-}
-
-int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
-                                   uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tti, uint16_t rnti,
-                                   bool acks[SRSLTE_MAX_CODEWORDS])
-{
+int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
+                             uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tm, uint32_t tti, uint16_t rnti,
+                             bool acks[SRSLTE_MAX_CODEWORDS]) {
   srslte_mimo_type_t mimo_type;
   srslte_dci_msg_t dci_msg;
   srslte_ra_dl_dci_t dci_unpacked;
@@ -339,7 +313,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   uint32_t cfi;
   uint32_t sf_idx = tti%10;
   
-  if ((ret = srslte_ue_dl_decode_fft_estimate_multi(q, input, sf_idx, &cfi)) < 0) {
+  if ((ret = srslte_ue_dl_decode_fft_estimate(q, input, sf_idx, &cfi)) < 0) {
     return ret; 
   }
   
@@ -352,7 +326,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     return SRSLTE_ERROR;
   }
 
-  int found_dci = srslte_ue_dl_find_dl_dci(q, cfi, sf_idx, rnti, &dci_msg);   
+  int found_dci = srslte_ue_dl_find_dl_dci(q, tm, cfi, sf_idx, rnti, &dci_msg);
   if (found_dci == 1) {
     
     if (srslte_dci_msg_to_dl_grant(&dci_msg, rnti, q->cell.nof_prb, q->cell.nof_ports, &dci_unpacked, &grant)) {
@@ -630,7 +604,7 @@ int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
   }
 }
 
-int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
+int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
 {
   srslte_rnti_type_t rnti_type; 
   if (rnti == SRSLTE_SIRNTI) {
@@ -642,7 +616,7 @@ int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
   } else {
     rnti_type = SRSLTE_RNTI_USER;
   }
-  return srslte_ue_dl_find_dl_dci_type(q, cfi, sf_idx, rnti, rnti_type, dci_msg);
+  return srslte_ue_dl_find_dl_dci_type(q, tm, cfi, sf_idx, rnti, rnti_type, dci_msg);
 }
 
 // Blind search for SI/P/RA-RNTI
@@ -666,12 +640,17 @@ static int find_dl_dci_type_siprarnti(srslte_ue_dl_t *q, uint32_t cfi, uint16_t
 }
 
 // Blind search for C-RNTI
-static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
-{
+static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi,
+                                  uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg) {
   int ret = SRSLTE_SUCCESS; 
   dci_blind_search_t search_space; 
   dci_blind_search_t *current_ss = &search_space;
-  
+
+  if (cfi < 1 || cfi > 3) {
+    ERROR("CFI must be 1 ≤ cfi ≤ 3", cfi);
+    return SRSLTE_ERROR;
+  }
+
   // Search UE-specific search space 
   if (q->current_rnti == rnti) {
     current_ss = &q->current_ss_ue[cfi-1][sf_idx];
@@ -681,15 +660,19 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_i
   }
   
   srslte_pdcch_set_cfi(&q->pdcch, cfi);
-  
-  INFO("Searching DL C-RNTI in %d ue locations, %d formats\n", current_ss->nof_locations, nof_ue_formats);
-  for (int f=0;f<nof_ue_formats;f++) {
-    current_ss->format = ue_formats[f];   
+
+  for (int f = 0; f < 2; f++) {
+    srslte_dci_format_t format = ue_dci_formats[tm][f];
+
+    INFO("Searching DL C-RNTI %s in %d ue locations\n", srslte_dci_format_string(format),
+         current_ss->nof_locations);
+
+    current_ss->format = format;
     if ((ret = dci_blind_search(q, current_ss, rnti, dci_msg))) {
       return ret; 
     }
   }
-  
+
   // Search Format 1A in the Common SS also
   if (q->current_rnti == rnti) {
     current_ss = &q->current_ss_common[cfi-1];
@@ -709,13 +692,13 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_i
   return SRSLTE_SUCCESS; 
 }
 
-int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, 
+int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi, uint32_t sf_idx,
                                   uint16_t rnti, srslte_rnti_type_t rnti_type, srslte_dci_msg_t *dci_msg)
 {  
   if (rnti_type == SRSLTE_RNTI_SI || rnti_type == SRSLTE_RNTI_PCH || rnti_type == SRSLTE_RNTI_RAR) {
     return find_dl_dci_type_siprarnti(q, cfi, rnti, dci_msg);
   } else {
-    return find_dl_dci_type_crnti(q, cfi, sf_idx, rnti, dci_msg);
+    return find_dl_dci_type_crnti(q, tm, cfi, sf_idx, rnti, dci_msg);
   }
 }
 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 3c05cc463..53dc232dc 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -103,7 +103,7 @@ bool phch_worker::init_cell(srslte_cell_t cell_)
     }
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell, phy->args->nof_rx_ant)) {    
+  if (srslte_ue_dl_init(&ue_dl, cell, phy->args->nof_rx_ant)) {
     Error("Initiating UE DL\n");
     return false; 
   }
@@ -334,7 +334,7 @@ bool phch_worker::extract_fft_and_pdcch_llr() {
       srslte_chest_dl_set_noise_alg(&ue_dl.chest, SRSLTE_NOISE_ALG_PSS);      
     }
   
-    if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl, signal_buffer, tti%10, &cfi) < 0) {
+    if (srslte_ue_dl_decode_fft_estimate(&ue_dl, signal_buffer, tti%10, &cfi) < 0) {
       Error("Getting PDCCH FFT estimate\n");
       return false; 
     }        
@@ -383,7 +383,8 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     
     Debug("Looking for RNTI=0x%x\n", dl_rnti);
     
-    if (srslte_ue_dl_find_dl_dci_type(&ue_dl, cfi, tti%10, dl_rnti, type, &dci_msg) != 1) {
+    if (srslte_ue_dl_find_dl_dci_type(&ue_dl, phy->config->dedicated.antenna_info_explicit_value.tx_mode, cfi, tti%10,
+                                      dl_rnti, type, &dci_msg) != 1) {
       return false; 
     }
     

From 9dffa10550a2435102eab2bae87aca72f4d2e71a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 17:56:58 +0200
Subject: [PATCH 133/269] fixed compiling issues

---
 lib/include/srslte/phy/utils/mat.h |  4 ++++
 lib/src/phy/utils/mat.c            |  3 ++-
 lib/src/phy/utils/vector.c         | 10 ++++++++++
 3 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/lib/include/srslte/phy/utils/mat.h b/lib/include/srslte/phy/utils/mat.h
index 48d3b776d..d960590c4 100644
--- a/lib/include/srslte/phy/utils/mat.h
+++ b/lib/include/srslte/phy/utils/mat.h
@@ -30,6 +30,7 @@
 #include "srslte/phy/utils/simd.h"
 #include "srslte/config.h"
 
+
 /*
  * Generic Macros
  */
@@ -65,6 +66,7 @@ SRSLTE_API float srslte_mat_2x2_cn(cf_t h00,
 
 
 #ifdef LV_HAVE_SSE
+#include <smmintrin.h>
 
 /* SSE implementation for complex reciprocal */
 SRSLTE_API __m128 srslte_mat_cf_recip_sse(__m128 a);
@@ -88,6 +90,8 @@ SRSLTE_API void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1,
 
 #ifdef LV_HAVE_AVX
 
+#include <immintrin.h>
+
 /* AVX implementation for complex reciprocal */
 SRSLTE_API __m256 srslte_mat_cf_recip_avx(__m256 a);
 
diff --git a/lib/src/phy/utils/mat.c b/lib/src/phy/utils/mat.c
index 3c81d4a13..439daa2ce 100644
--- a/lib/src/phy/utils/mat.c
+++ b/lib/src/phy/utils/mat.c
@@ -25,7 +25,6 @@
  */
 
 #include <complex.h>
-#include <immintrin.h>
 #include <math.h>
 
 #include "srslte/phy/utils/mat.h"
@@ -116,6 +115,7 @@ inline float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
 }
 
 #ifdef LV_HAVE_SSE
+#include <smmintrin.h>
 
 /* SSE implementation for complex reciprocal */
 inline __m128 srslte_mat_cf_recip_sse(__m128 a) {
@@ -186,6 +186,7 @@ inline void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01
 #endif /* LV_HAVE_SSE */
 
 #ifdef LV_HAVE_AVX
+#include <immintrin.h>
 
 /* AVX implementation for complex reciprocal */
 inline __m256 srslte_mat_cf_recip_avx(__m256 a) {
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index 742b9573f..d78f5d707 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -35,6 +35,16 @@
 #include "srslte/phy/utils/vector_simd.h"
 #include "srslte/phy/utils/bit.h"
 
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+#endif
+
+#ifdef LV_HAVE_AVX
+#include <immintrin.h>
+#endif
+
+
 #ifdef HAVE_VOLK
 #include "volk/volk.h"
 #endif

From 4e8f0de6dae75d76a6ed6ed035f8d26812c4c5b3 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 4 Sep 2017 19:07:36 +0200
Subject: [PATCH 134/269] reduce PUSCH spectral efficiency if UCI is required

---
 srsenb/hdr/mac/scheduler_ue.h      |  5 ++++-
 srsenb/src/mac/scheduler.cc        | 15 +++++++++++----
 srsenb/src/mac/scheduler_harq.cc   |  2 +-
 srsenb/src/mac/scheduler_metric.cc |  3 ++-
 srsenb/src/mac/scheduler_ue.cc     |  5 +++++
 5 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/srsenb/hdr/mac/scheduler_ue.h b/srsenb/hdr/mac/scheduler_ue.h
index b95e5dda2..b59461140 100644
--- a/srsenb/hdr/mac/scheduler_ue.h
+++ b/srsenb/hdr/mac/scheduler_ue.h
@@ -40,7 +40,10 @@ class sched_ue {
 public: 
   
   // used by sched_metric
-  uint32_t ue_idx;   
+  uint32_t ue_idx;
+
+  bool has_pusch;
+  bool has_pucch;
   
   typedef struct {
     uint32_t cce_start[4][6];
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 12a7ea1cc..73dbc2d41 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -691,7 +691,10 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
   for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
     sched_ue *user = (sched_ue*) &iter->second;
     uint16_t rnti  = (uint16_t) iter->first; 
-    
+
+    user->has_pusch = false;
+    user->has_pucch = false;
+
     ul_harq_proc *h = user->get_ul_harq(current_tti);
   
     /* Indicate PHICH acknowledgment if needed */
@@ -717,9 +720,13 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
     uint32_t prb_idx[2] = {0, 0}; 
     uint32_t L = 0; 
     if (user->get_pucch_sched(current_tti, prb_idx, &L)) {
-      for (int i=0;i<2;i++) {
-        ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], L};
-        ul_metric->update_allocation(pucch);        
+      user->has_pucch = true;
+      // allocate PUCCH if no PUSCH for user
+      if (!user->has_pusch) {
+        for (int i=0;i<2;i++) {
+          ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], L};
+          ul_metric->update_allocation(pucch);
+        }
       }
     }
   }
diff --git a/srsenb/src/mac/scheduler_harq.cc b/srsenb/src/mac/scheduler_harq.cc
index 009872153..a6ae70d19 100644
--- a/srsenb/src/mac/scheduler_harq.cc
+++ b/srsenb/src/mac/scheduler_harq.cc
@@ -100,7 +100,7 @@ void harq_proc::set_ack(bool ack_)
   ack = ack_;   
   ack_received = true;
   log_h->debug("ACK=%d received pid=%d, n_rtx=%d, max_retx=%d\n", ack_, id, n_rtx, max_retx);
-  if (n_rtx >= max_retx) {
+  if (n_rtx + 1 >= max_retx) {
     Warning("SCHED: discarting TB pid=%d, tti=%d, maximum number of retx exceeded (%d)\n", id, tti, max_retx);
     active = false;      
   }
diff --git a/srsenb/src/mac/scheduler_metric.cc b/srsenb/src/mac/scheduler_metric.cc
index 2a9a4432b..309eed45a 100644
--- a/srsenb/src/mac/scheduler_metric.cc
+++ b/srsenb/src/mac/scheduler_metric.cc
@@ -214,7 +214,8 @@ void ul_metric_rr::new_tti(std::map<uint16_t,sched_ue> &ue_db, uint32_t nof_rb_,
   for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
     sched_ue *user      = (sched_ue*) &iter->second;
     if (user->get_pending_ul_new_data(current_tti) || !user->get_ul_harq(current_tti)->is_empty()) {
-      user->ue_idx = nof_users_with_data;
+      user->ue_idx    = nof_users_with_data;
+      user->has_pusch = true;
       nof_users_with_data++; 
     }
   }
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 21aa61b19..4c6d467a2 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -813,6 +813,11 @@ int sched_ue::alloc_tbs(uint32_t nof_prb,
   uint32_t max_mcs = is_ul?max_mcs_ul:max_mcs_dl;
   uint32_t max_Qm  = is_ul?4:6; // Allow 16-QAM in PUSCH Only
 
+  // TODO: Compute real spectral efficiency based on PUSCH-UCI configuration
+  if (has_pucch) {
+    cqi-=2;
+  }
+
   int tbs = cqi_to_tbs(cqi, nof_prb, nof_re, max_mcs, max_Qm, &sel_mcs)/8;
   
   /* If less bytes are requested, lower the MCS */

From 14ab68e652311d0ae703a27835b1d6ba3aae12c2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 10:05:53 +0200
Subject: [PATCH 135/269] added CQI to metrics

---
 srsenb/hdr/mac/mac_metrics.h   |  1 +
 srsenb/hdr/mac/ue.h            | 18 ++++++++++--------
 srsenb/src/mac/mac.cc          |  1 +
 srsenb/src/mac/scheduler_ue.cc |  6 +++---
 srsenb/src/mac/ue.cc           | 12 +++++++++---
 srsenb/src/metrics_stdout.cc   |  7 ++++---
 6 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/srsenb/hdr/mac/mac_metrics.h b/srsenb/hdr/mac/mac_metrics.h
index 4e3452ae1..35a65f8ae 100644
--- a/srsenb/hdr/mac/mac_metrics.h
+++ b/srsenb/hdr/mac/mac_metrics.h
@@ -43,6 +43,7 @@ struct mac_metrics_t
   int rx_brate;
   int ul_buffer;
   int dl_buffer;
+  float dl_cqi;
   float phr; 
 };
 
diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index b98b07f9d..5805e5472 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -48,8 +48,9 @@ public:
     log_h = NULL; 
     rnti  = 0; 
     pcap  = NULL;
-    nof_failures = 0; 
-    phr_counter = 0; 
+    nof_failures   = 0;
+    phr_counter    = 0;
+    dl_cqi_counter = 0;
     is_phy_added = false; 
     for (int i=0;i<NOF_HARQ_PROCESSES;i++) {
       pending_buffers[i] = NULL; 
@@ -91,8 +92,10 @@ public:
   void metrics_read(srsenb::mac_metrics_t* metrics);
   void metrics_rx(bool crc, uint32_t tbs);
   void metrics_tx(bool crc, uint32_t tbs);
-  
-  
+  void metrics_phr(float phr);
+  void metrics_dl_cqi(uint32_t dl_cqi);
+
+
   bool is_phy_added;
 
 private: 
@@ -100,12 +103,11 @@ private:
   void allocate_sdu(srslte::sch_pdu *pdu, uint32_t lcid, uint32_t sdu_len);   
   bool process_ce(srslte::sch_subh *subh); 
   void allocate_ce(srslte::sch_pdu *pdu, uint32_t lcid);
-  
-  void metrics_phr(float phr); 
-  uint32_t phr_counter;
 
   std::vector<uint32_t> lc_groups[4];
-  
+
+  uint32_t phr_counter;
+  uint32_t dl_cqi_counter;
   mac_metrics_t metrics;
   
   srslte::mac_pcap* pcap;
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index fb2c3110e..63d189c20 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -329,6 +329,7 @@ int mac::cqi_info(uint32_t tti, uint16_t rnti, uint32_t cqi_value)
 
   if (ue_db.count(rnti)) {         
     scheduler.dl_cqi_info(tti, rnti, cqi_value);
+    ue_db[rnti]->metrics_dl_cqi(cqi_value);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     return -1;
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 4c6d467a2..9a258a1a3 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -257,7 +257,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
       if (L) {
         *L = 1; 
       }
-      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
+      Debug("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
       return true; 
     }
   }
@@ -271,7 +271,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
     if (L) {
       *L = 1; 
     }
-    Info("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
+    Debug("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
     return true; 
   }
   // Finally check Format2 (periodic CQI)
@@ -284,7 +284,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
     if(L) {
       *L = 2;
     }
-    Info("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
+    Debug("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
          rnti, prb_idx[0], prb_idx[1], cfg.cqi_pucch, cfg.cqi_idx);
     return true;
   }
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index 0b9776918..14944b6d4 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -396,14 +396,20 @@ void ue::metrics_read(mac_metrics_t* metrics_)
   metrics.dl_buffer = sched->get_dl_buffer(rnti);
   
   memcpy(metrics_, &metrics, sizeof(mac_metrics_t));
-  
-  phr_counter = 0; 
+
+  phr_counter = 0;
+  dl_cqi_counter = 0;
   bzero(&metrics, sizeof(mac_metrics_t));  
 }
 
 void ue::metrics_phr(float phr) {
   metrics.phr = SRSLTE_VEC_CMA(phr, metrics.phr, phr_counter);
-  phr_counter++; 
+  phr_counter++;
+}
+
+void ue::metrics_dl_cqi(uint32_t dl_cqi) {
+  metrics.dl_cqi = SRSLTE_VEC_CMA((float) dl_cqi, metrics.dl_cqi, dl_cqi_counter);
+  dl_cqi_counter++;
 }
 
 void ue::metrics_rx(bool crc, uint32_t tbs)
diff --git a/srsenb/src/metrics_stdout.cc b/srsenb/src/metrics_stdout.cc
index dc6b6d575..6c294b96d 100644
--- a/srsenb/src/metrics_stdout.cc
+++ b/srsenb/src/metrics_stdout.cc
@@ -108,8 +108,8 @@ void metrics_stdout::print_metrics()
   {
     n_reports = 0;
     cout << endl;
-    cout << "------DL-------------------UL----------------" << endl;
-    cout << "rnti   mcs   brate   bler  snr  phr  turbo  mcs  brate   bler" << endl;
+    cout << "------DL-------------------------UL-------------------------------" << endl;
+    cout << "rnti   cqi   mcs  brate   bler   snr  phr   mcs  brate   bler   bsr" << endl;
   }
   if (metrics.rrc.n_ues > 0) {
     
@@ -122,6 +122,7 @@ void metrics_stdout::print_metrics()
       }
     
       cout << std::hex << metrics.mac[i].rnti << " ";
+      cout << float_to_string(metrics.mac[i].dl_cqi, 2);
       cout << float_to_string(metrics.phy[i].dl.mcs, 2);
       if (metrics.mac[i].tx_brate > 0 && metrics_report_period) {
         cout << float_to_eng_string((float) metrics.mac[i].tx_brate/metrics_report_period, 2);
@@ -135,7 +136,6 @@ void metrics_stdout::print_metrics()
       }
       cout << float_to_string(metrics.phy[i].ul.sinr, 2);
       cout << float_to_string(metrics.mac[i].phr, 2);
-      cout << float_to_string(metrics.phy[i].ul.turbo_iters, 2);
       cout << float_to_string(metrics.phy[i].ul.mcs, 2);
       if (metrics.mac[i].rx_brate > 0 && metrics_report_period) {
         cout << float_to_eng_string((float) metrics.mac[i].rx_brate/metrics_report_period, 2);
@@ -147,6 +147,7 @@ void metrics_stdout::print_metrics()
       } else {
         cout << float_to_string(0, 2) << "%";
       }
+      cout << float_to_eng_string(metrics.mac[i].ul_buffer, 2);
       cout << endl;
     }
   } else {

From 5341d79b8a0a618f7d1e3c715bdbd659554e9b25 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 10:54:36 +0200
Subject: [PATCH 136/269] Fix memory alignment in PUCCH processing. Fixes #94

---
 lib/include/srslte/phy/phch/uci.h |  8 +++++---
 lib/src/phy/phch/pucch.c          |  1 +
 lib/src/phy/phch/uci.c            | 17 ++++++++++++++++-
 3 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index bad87866e..01eae8e89 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -57,8 +57,8 @@ typedef struct SRSLTE_API {
 } srslte_uci_cqi_pusch_t;
 
 typedef struct SRSLTE_API {
-  uint8_t cqi_table[16][32];
-  int16_t cqi_table_s[16][32]; // aligned for simd
+  uint8_t *cqi_table[16];
+  int16_t *cqi_table_s[16];
 } srslte_uci_cqi_pucch_t;
 
 typedef struct SRSLTE_API {
@@ -85,7 +85,9 @@ typedef struct {
 
 SRSLTE_API void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q);
 
-SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, 
+SRSLTE_API void srslte_uci_cqi_pucch_free(srslte_uci_cqi_pucch_t *q);
+
+SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data,
                                            uint32_t cqi_len, 
                                            uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B]);
 
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 70b500cd0..56349dee4 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -465,6 +465,7 @@ void srslte_pucch_free(srslte_pucch_t *q) {
     }
     free(q->users);
   }
+  srslte_uci_cqi_pucch_free(&q->cqi);
   if (q->z) {
     free(q->z);
   }
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 5bf40cc17..3b22eb792 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/phch/uci.h>
 
 #include "srslte/phy/phch/uci.h"
 #include "srslte/phy/fec/cbsegm.h"
@@ -109,7 +110,9 @@ void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q) {
     
   uint32_t nwords      = 16;
   for (uint32_t w=0;w<nwords;w++) {
-    uint8_t *ptr = word; 
+    q->cqi_table[w]   = srslte_vec_malloc(SRSLTE_UCI_CQI_CODED_PUCCH_B*sizeof(int8_t));
+    q->cqi_table_s[w] = srslte_vec_malloc(SRSLTE_UCI_CQI_CODED_PUCCH_B*sizeof(int16_t));
+    uint8_t *ptr = word;
     srslte_bit_unpack(w, &ptr, 4);
     srslte_uci_encode_cqi_pucch(word, 4, q->cqi_table[w]);    
     for (int j=0;j<SRSLTE_UCI_CQI_CODED_PUCCH_B;j++) {
@@ -118,6 +121,18 @@ void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q) {
   }
 }
 
+void srslte_uci_cqi_pucch_free(srslte_uci_cqi_pucch_t *q) {
+  uint32_t nwords      = 16;
+  for (uint32_t w=0;w<nwords;w++) {
+    if (q->cqi_table[w]) {
+      free(q->cqi_table[w]);
+    }
+    if (q->cqi_table_s[w]) {
+      free(q->cqi_table_s[w]);
+    }
+  }
+}
+
 /* Encode UCI CQI/PMI as described in 5.2.3.3 of 36.212 
  */
 int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, uint32_t cqi_len, uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B])

From 79b0ca81d0cb824e43531a753404196eaba34dc4 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 10:54:36 +0200
Subject: [PATCH 137/269] Fix memory alignment in PUCCH processing. Fixes #94

---
 lib/include/srslte/phy/phch/uci.h |  8 +++++---
 lib/src/phy/phch/pucch.c          | 12 +++++++++++-
 lib/src/phy/phch/uci.c            | 17 ++++++++++++++++-
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index bad87866e..01eae8e89 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -57,8 +57,8 @@ typedef struct SRSLTE_API {
 } srslte_uci_cqi_pusch_t;
 
 typedef struct SRSLTE_API {
-  uint8_t cqi_table[16][32];
-  int16_t cqi_table_s[16][32]; // aligned for simd
+  uint8_t *cqi_table[16];
+  int16_t *cqi_table_s[16];
 } srslte_uci_cqi_pucch_t;
 
 typedef struct SRSLTE_API {
@@ -85,7 +85,9 @@ typedef struct {
 
 SRSLTE_API void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q);
 
-SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, 
+SRSLTE_API void srslte_uci_cqi_pucch_free(srslte_uci_cqi_pucch_t *q);
+
+SRSLTE_API int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data,
                                            uint32_t cqi_len, 
                                            uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B]);
 
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 4e10622a5..eadf8b911 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -452,7 +452,17 @@ void srslte_pucch_free(srslte_pucch_t *q) {
     }
     free(q->users);
   }
-
+  srslte_uci_cqi_pucch_free(&q->cqi);
+  if (q->z) {
+    free(q->z);
+  }
+  if (q->z_tmp) {
+    free(q->z_tmp);
+  }
+  if (q->ce) {
+    free(q->ce);
+  }
+  
   srslte_modem_table_free(&q->mod);
   bzero(q, sizeof(srslte_pucch_t));
 }
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 5bf40cc17..3b22eb792 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -32,6 +32,7 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
+#include <srslte/phy/phch/uci.h>
 
 #include "srslte/phy/phch/uci.h"
 #include "srslte/phy/fec/cbsegm.h"
@@ -109,7 +110,9 @@ void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q) {
     
   uint32_t nwords      = 16;
   for (uint32_t w=0;w<nwords;w++) {
-    uint8_t *ptr = word; 
+    q->cqi_table[w]   = srslte_vec_malloc(SRSLTE_UCI_CQI_CODED_PUCCH_B*sizeof(int8_t));
+    q->cqi_table_s[w] = srslte_vec_malloc(SRSLTE_UCI_CQI_CODED_PUCCH_B*sizeof(int16_t));
+    uint8_t *ptr = word;
     srslte_bit_unpack(w, &ptr, 4);
     srslte_uci_encode_cqi_pucch(word, 4, q->cqi_table[w]);    
     for (int j=0;j<SRSLTE_UCI_CQI_CODED_PUCCH_B;j++) {
@@ -118,6 +121,18 @@ void srslte_uci_cqi_pucch_init(srslte_uci_cqi_pucch_t *q) {
   }
 }
 
+void srslte_uci_cqi_pucch_free(srslte_uci_cqi_pucch_t *q) {
+  uint32_t nwords      = 16;
+  for (uint32_t w=0;w<nwords;w++) {
+    if (q->cqi_table[w]) {
+      free(q->cqi_table[w]);
+    }
+    if (q->cqi_table_s[w]) {
+      free(q->cqi_table_s[w]);
+    }
+  }
+}
+
 /* Encode UCI CQI/PMI as described in 5.2.3.3 of 36.212 
  */
 int srslte_uci_encode_cqi_pucch(uint8_t *cqi_data, uint32_t cqi_len, uint8_t b_bits[SRSLTE_UCI_CQI_CODED_PUCCH_B])

From 283cb115ac3dffa6a0401bbc879c9c21dbba6b82 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 12:08:43 +0200
Subject: [PATCH 138/269] fixed bugs in enb

---
 lib/src/phy/common/sequence.c  |  3 ---
 lib/src/phy/enb/enb_dl.c       | 20 +++++++++++---------
 lib/src/phy/phch/pbch.c        |  1 -
 lib/src/phy/phch/prach.c       |  3 +++
 lib/src/phy/phch/pusch.c       |  2 +-
 srsenb/src/phy/phch_worker.cc  | 12 +++++++-----
 srsenb/src/phy/prach_worker.cc |  5 +++--
 7 files changed, 25 insertions(+), 21 deletions(-)

diff --git a/lib/src/phy/common/sequence.c b/lib/src/phy/common/sequence.c
index 675dc9df3..d7f702149 100644
--- a/lib/src/phy/common/sequence.c
+++ b/lib/src/phy/common/sequence.c
@@ -136,9 +136,6 @@ int srslte_sequence_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed) {
     q->c_float[i] = (1-2*q->c[i]);
     q->c_short[i] = (int16_t) q->c_float[i];
   }
-
-
-
   return SRSLTE_SUCCESS;
 }
 
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index 095312621..5648490bb 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -30,6 +30,7 @@
 #include <math.h>
 #include <string.h>
 #include <srslte/phy/common/phy_common.h>
+#include <srslte/srslte.h>
 
 
 #define CURRENT_FFTSIZE   srslte_symbol_sz(q->cell.nof_prb)
@@ -89,12 +90,12 @@ int srslte_enb_dl_init(srslte_enb_dl_t *q, uint32_t max_prb)
     }
     
     for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
-      q->sf_symbols[i] = srslte_vec_malloc(CURRENT_SFLEN_RE * sizeof(cf_t));
+      q->sf_symbols[i] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(max_prb, SRSLTE_CP_NORM) * sizeof(cf_t));
       if (!q->sf_symbols[i]) {
         perror("malloc");
         goto clean_exit; 
       }
-      q->slot1_symbols[i] = &q->sf_symbols[i][CURRENT_SLOTLEN_RE];
+      q->slot1_symbols[i] = &q->sf_symbols[i][SRSLTE_SLOT_LEN_RE(max_prb, SRSLTE_CP_NORM)];
     }
     
     ret = SRSLTE_SUCCESS;
@@ -115,6 +116,7 @@ void srslte_enb_dl_free(srslte_enb_dl_t *q)
   if (q) {
     srslte_ofdm_tx_free(&q->ifft);
     srslte_regs_free(&q->regs);
+    srslte_pbch_free(&q->pbch);
     srslte_pcfich_free(&q->pcfich);
     srslte_phich_free(&q->phich);
     srslte_pdcch_free(&q->pdcch);
@@ -138,18 +140,18 @@ int srslte_enb_dl_set_cell(srslte_enb_dl_t *q, srslte_cell_t cell)
   if (q                 != NULL &&
       srslte_cell_isvalid(&cell))
   {
-    q->cfi  = 3;
+    srslte_enb_dl_set_cfi(q, 3);
     q->tx_amp = SRSLTE_ENB_RF_AMP;
 
     if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
-      if (q->cell.nof_prb) {
-        if (srslte_regs_init(&q->regs, q->cell)) {
-          fprintf(stderr, "Error initiating REGs\n");
-          return SRSLTE_ERROR;
-        }
+      if (q->cell.nof_prb != 0) {
+        srslte_regs_free(&q->regs);
       }
       memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
-
+      if (srslte_regs_init(&q->regs, q->cell)) {
+        fprintf(stderr, "Error resizing REGs\n");
+        return SRSLTE_ERROR;
+      }
       if (srslte_ofdm_rx_set_prb(&q->ifft, q->cell.cp, q->cell.nof_prb)) {
         fprintf(stderr, "Error initiating FFT\n");
         return SRSLTE_ERROR;
diff --git a/lib/src/phy/phch/pbch.c b/lib/src/phy/phch/pbch.c
index dfd094dec..2eaa20501 100644
--- a/lib/src/phy/phch/pbch.c
+++ b/lib/src/phy/phch/pbch.c
@@ -253,7 +253,6 @@ int srslte_pbch_set_cell(srslte_pbch_t *q, srslte_cell_t cell) {
 
     if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
       memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
-
       if (srslte_sequence_pbch(&q->seq, q->cell.cp, q->cell.id)) {
         return SRSLTE_ERROR;
       }
diff --git a/lib/src/phy/phch/prach.c b/lib/src/phy/phch/prach.c
index f2ddd48db..ffd8b51e7 100644
--- a/lib/src/phy/phch/prach.c
+++ b/lib/src/phy/phch/prach.c
@@ -331,6 +331,9 @@ int srslte_prach_gen_seqs(srslte_prach_t *p)
 
 int srslte_prach_init_cfg(srslte_prach_t *p, srslte_prach_cfg_t *cfg, uint32_t nof_prb)
 {
+  if (srslte_prach_init(p, srslte_symbol_sz(nof_prb))) {
+    return -1;
+  }
   return srslte_prach_set_cell(p,
                            srslte_symbol_sz(nof_prb), 
                            cfg->config_idx, 
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 6769f43d4..24e86e9a3 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -434,7 +434,7 @@ int srslte_pusch_cfg(srslte_pusch_t             *q,
 int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
   uint32_t i;
 
-  uint32_t rnti_idx = q->is_ue?0:rnti; 
+  uint32_t rnti_idx = q->is_ue?0:rnti;
 
   if (!q->users[rnti_idx] || q->is_ue) {
     if (!q->users[rnti_idx]) {
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index bca09a519..9b31d6a43 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -103,8 +103,10 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     fprintf(stderr, "Error initiating ENB DL\n");
     return;
   }
-  srslte_enb_ul_init(&enb_ul, phy->cell.nof_prb);
-
+  if (srslte_enb_ul_init(&enb_ul, phy->cell.nof_prb)) {
+    fprintf(stderr, "Error initiating ENB UL\n");
+    return;
+  }
   if (srslte_enb_ul_set_cell(&enb_ul,
                               phy->cell,
                               NULL,
@@ -112,7 +114,7 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
                               &phy->hopping_cfg,
                               &phy->pucch_cfg))
   {
-    fprintf(stderr, "Error initiating ENB DL\n");
+    fprintf(stderr, "Error initiating ENB UL\n");
     return;
   }
   
@@ -154,14 +156,14 @@ void phch_worker::set_time(uint32_t tti_, uint32_t tx_mutex_cnt_, srslte_timesta
 
 int phch_worker::add_rnti(uint16_t rnti)
 {
-  
+
   if (srslte_enb_dl_add_rnti(&enb_dl, rnti)) {
     return -1; 
   }
   if (srslte_enb_ul_add_rnti(&enb_ul, rnti)) {
     return -1; 
   }
-  
+
   // Create user 
   ue_db[rnti].rnti = rnti; 
     
diff --git a/srsenb/src/phy/prach_worker.cc b/srsenb/src/phy/prach_worker.cc
index 218f7cc21..cc924bf0b 100644
--- a/srsenb/src/phy/prach_worker.cc
+++ b/srsenb/src/phy/prach_worker.cc
@@ -24,6 +24,7 @@
  *
  */
 
+#include "srslte/srslte.h"
 #include "phy/prach_worker.h"
 
 namespace srsenb {
@@ -48,7 +49,7 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
   srslte_prach_set_detect_factor(&prach, 60);    
 
   nof_sf = (uint32_t) ceilf(prach.T_tot*1000); 
-  
+
   signal_buffer_rx = (cf_t*) srslte_vec_malloc(sizeof(cf_t)*nof_sf*SRSLTE_SF_LEN_PRB(cell.nof_prb));
   if (!signal_buffer_rx) {
     perror("malloc");
@@ -108,7 +109,7 @@ int prach_worker::run_tti(uint32_t tti_rx)
 {
   if (srslte_prach_tti_opportunity(&prach, tti_rx, -1)) 
   {
-    // Detect possible PRACHs 
+    // Detect possible PRACHs
     if (srslte_prach_detect_offset(&prach,
                                    prach_cfg.freq_offset,
                                    &signal_buffer_rx[prach.N_cp],

From 338be7d0c2b97421a7fa4374dc3f7200867eb456 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 13:17:33 +0200
Subject: [PATCH 139/269] Refactored grant: removed nof_tb from grant. Use
 tb_en instead.

---
 lib/examples/pdsch_enodeb.c                   | 18 ++--
 lib/examples/pdsch_ue.c                       |  9 +-
 lib/include/srslte/interfaces/ue_interfaces.h |  2 +
 lib/include/srslte/phy/phch/ra.h              |  4 +-
 lib/src/phy/phch/pdsch.c                      | 72 +++++++++-------
 lib/src/phy/phch/ra.c                         | 29 ++++---
 lib/src/phy/phch/sch.c                        |  4 +-
 lib/src/phy/phch/test/pdsch_test.c            | 39 +++++----
 lib/src/phy/ue/ue_dl.c                        | 61 +++++++------
 srsue/hdr/mac/dl_harq.h                       |  6 +-
 srsue/hdr/phy/phch_worker.h                   |  2 +-
 srsue/src/phy/phch_worker.cc                  | 85 +++++++++++--------
 12 files changed, 191 insertions(+), 140 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 35f7d3e9f..afbe44b91 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -774,9 +774,11 @@ int main(int argc, char **argv) {
         }
       } else {
         INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs);
-        for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-          for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
-            data[tb][i] = (uint8_t) rand();
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+          if (pdsch_cfg.grant.tb_en[tb]) {
+            for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
+              data[tb][i] = (uint8_t) rand();
+            }
           }
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
@@ -832,10 +834,12 @@ int main(int argc, char **argv) {
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-              srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
-              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
-                fprintf(stderr, "Error sending data through UDP socket\n");
+            for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+              if (pdsch_cfg.grant.tb_en[tb]) {
+                srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
+                if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
+                  fprintf(stderr, "Error sending data through UDP socket\n");
+                }
               }
             }
           }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 2ace49d6d..aa3e13257 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -641,9 +641,10 @@ int main(int argc, char **argv) {
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
                 // FIXME: UDP Data transmission does not work
-                for (uint32_t tb = 0; tb < ue_dl.pdsch_cfg.grant.nof_tb; tb++) {
-                  srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
-
+                for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+                  if (ue_dl.pdsch_cfg.grant.tb_en[tb]) {
+                    srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+                  }
                 }
               }
               
@@ -704,7 +705,7 @@ int main(int argc, char **argv) {
 
             /* Print basic Parameters */
             PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
-            PRINT_LINE("nof codewords: %d", ue_dl.pdsch_cfg.grant.nof_tb);
+            PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
             PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
             PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 879118021..5907d164b 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -281,6 +281,8 @@ public:
     bool        last_ndi[SRSLTE_MAX_CODEWORDS];
     uint32_t    n_bytes[SRSLTE_MAX_CODEWORDS];
     int         rv[SRSLTE_MAX_CODEWORDS];
+    bool        tb_en[SRSLTE_MAX_CODEWORDS];
+    bool        tb_cw_swap;
     uint16_t    rnti; 
     bool        is_from_rar;
     bool        is_sps_release;
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 9f31a0218..94cc9a084 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -104,10 +104,12 @@ typedef struct SRSLTE_API {
   uint32_t nof_prb;  
   uint32_t Qm[SRSLTE_MAX_CODEWORDS];
   srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
-  uint32_t nof_tb;
+  bool tb_en[SRSLTE_MAX_CODEWORDS];
   uint32_t pinfo;
 } srslte_ra_dl_grant_t;
 
+#define SRSLTE_RA_DL_GRANT_NOF_TB(G) ((((G)->tb_en[0])?1:0)+(((G)->tb_en[1])?1:0))
+
 /** Unpacked DCI message for DL grant */
 typedef struct SRSLTE_API {
   
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index bc62df7fd..fe10791d7 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -427,15 +427,17 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
 int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
                            uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
                            uint32_t pmi) {
-  if (cfg) {
-    if (grant) {
-      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
-    }
+  if (cfg && grant) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
+    memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
 
-    for (int i = 0; i < grant->nof_tb; i++) {
-      if (srslte_cbsegm(&cfg->cb_segm[i], (uint32_t) cfg->grant.mcs[i].tbs)) {
-        fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[i].tbs);
-        return SRSLTE_ERROR;
+
+    for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
+      if (grant->tb_en[cw]) {
+        if (srslte_cbsegm(&cfg->cb_segm[cw], (uint32_t) cfg->grant.mcs[cw].tbs)) {
+          fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[cw].tbs);
+          return SRSLTE_ERROR;
+        }
       }
     }
     srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
@@ -447,33 +449,36 @@ int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra
     /* Check and configure PDSCH transmission modes */
     switch(mimo_type) {
       case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for single transmission mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for single antenna.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 1;
         break;
       case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for transmit diversity mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for transmit diversity.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
         break;
       case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-        if (grant->nof_tb == 1) {
+        if (nof_tb == 1) {
           cfg->codebook_idx = pmi;
           cfg->nof_layers = 1;
-        } else {
+        } else if (nof_tb == 2) {
           cfg->codebook_idx = pmi + 1;
           cfg->nof_layers = 2;
+        } else {
+          ERROR("Wrong number of transport blocks (%d) for spatial multiplexing.", nof_tb);
+          return SRSLTE_ERROR;
         }
         INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n",
-             grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+             nof_tb, cfg->nof_layers, cfg->codebook_idx);
         break;
       case SRSLTE_MIMO_TYPE_CDD:
-        if (grant->nof_tb != 2) {
-          ERROR("Number of transport blocks (%d) is not supported for CDD transmission mode.", grant->nof_tb);
+        if (nof_tb != 2) {
+          ERROR("Wrong number of transport blocks (%d) for CDD.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
@@ -585,9 +590,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
       data         != NULL &&
       cfg          != NULL)
   {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
 
     INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, mimo_type=%d, nof_layers=%d, nof_tb=%d\n",
-        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, cfg->grant.nof_tb);
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, nof_tb);
 
     // Extract Symbols and Channel Estimates
     for (int j=0;j<q->nof_rx_antennas;j++) {
@@ -608,10 +614,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
 
     // Prepare layers
     int nof_symbols [SRSLTE_MAX_CODEWORDS];
-    nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
-    nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+    nof_symbols[0] = cfg->nbits[0].nof_re * nof_tb / cfg->nof_layers;
+    nof_symbols[1] = cfg->nbits[1].nof_re * nof_tb / cfg->nof_layers;
 
-    if (cfg->nof_layers == cfg->grant.nof_tb) {
+    if (cfg->nof_layers == nof_tb) {
       /* Skip layer demap */
       for (i = 0; i < cfg->nof_layers; i++) {
         x[i] = q->d[i];
@@ -629,15 +635,17 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
                                     cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
     // Layer demapping only if necessary
-    if (cfg->nof_layers != cfg->grant.nof_tb) {
-        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+    if (cfg->nof_layers != nof_tb) {
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 
     // Codeword decoding
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
-      acks[tb] = (ret == SRSLTE_SUCCESS);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+        acks[tb] = (ret == SRSLTE_SUCCESS);
+      }
     }
 
     pdsch_decode_debug(q, cfg, sf_symbols, ce);
@@ -689,6 +697,8 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
 
   if (q != NULL &&
       cfg != NULL) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
+
 
     for (i = 0; i < q->cell.nof_ports; i++) {
       if (sf_symbols[i] == NULL) {
@@ -708,15 +718,17 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+      }
     }
 
     // Layer mapping & precode if necessary
     if (q->cell.nof_ports > 1) {
       int nof_symbols;
       /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
-      if (cfg->nof_layers == cfg->grant.nof_tb) {
+      if (cfg->nof_layers == nof_tb) {
         for (i = 0; i < cfg->nof_layers; i++) {
           x[i] = q->d[i];
         }
@@ -728,7 +740,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
         }
         memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-        nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
+        nof_symbols = srslte_layermap_type(q->d, x, nof_tb, cfg->nof_layers,
                                            (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits[0].nof_re, cfg->nbits[1].nof_re},
                                            cfg->mimo_type);
       }
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 00713e6c1..b85d2ea2f 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -521,11 +521,10 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       grant->mcs[1].tbs = 0;
     }
   }
-  grant->nof_tb = 0;
   for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    grant->tb_en[tb] = dci->tb_en[tb];
     if (dci->tb_en[tb]) {
       grant->Qm[tb] = srslte_mod_bits_x_symbol(grant->mcs[tb].mod);
-      grant->nof_tb++;
     }
   }
   grant->pinfo = dci->pinfo;
@@ -541,12 +540,14 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
                                  srslte_ra_nbits_t nbits [SRSLTE_MAX_CODEWORDS])
 {
   // Compute number of RE 
-  for (int i = 0; i < grant->nof_tb; i++) {
-    /* Compute number of RE for first transport block */
-    nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
-    nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
-    nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
-    nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      /* Compute number of RE for first transport block */
+      nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
+      nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
+      nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+      nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+    }
   }
 }
 
@@ -820,11 +821,13 @@ void srslte_ra_pdsch_fprint(FILE *f, srslte_ra_dl_dci_t *dci, uint32_t nof_prb)
 void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
-  fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
-  for (int i = 0; i < grant->nof_tb; i++) {
-    fprintf(f, "  - Transport block:\t\t\t%d\n", i);
-    fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
-    fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+  fprintf(f, " - Number of TBs:\t\t\t%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      fprintf(f, "  - Transport block:\t\t\t%d\n", i);
+      fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
+      fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+    }
   }
 }
 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index e0002429e..72ee50ee9 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -524,7 +524,7 @@ int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                          int16_t *e_bits, uint8_t *data, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
@@ -553,7 +553,7 @@ int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                               uint8_t *data, uint8_t *e_bits, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index b98d75c22..ae0bff64e 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -257,8 +257,8 @@ int main(int argc, char **argv) {
   }
 
 
-  for (int i = 0; i < grant.nof_tb; i++) {
-    if (grant.mcs[i].tbs) {
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant.tb_en[i]) {
       data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
       if (!data_tx[i]) {
         perror("srslte_vec_malloc");
@@ -303,12 +303,11 @@ int main(int argc, char **argv) {
   INFO("              cp=%s\n", srslte_cp_string(cell.cp));
   INFO("    phich_length=%d\n", (int) cell.phich_length);
   INFO(" phich_resources=%d\n", (int) cell.phich_resources);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
   INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
   INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
   INFO("       mimo_type=%s\n", srslte_mimotype2str(pdsch_cfg.mimo_type));
   INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("          nof_tb=%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(&pdsch_cfg.grant));
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     INFO(" Tranport block index %d:\n", i);
     INFO("              Qm=%d\n", pdsch_cfg.grant.Qm[i]);
@@ -372,9 +371,11 @@ int main(int argc, char **argv) {
       }
     }
 
-    for (int tb = 0; tb < grant.nof_tb; tb++) {
-      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-        data_tx[tb][byte] = (uint8_t)(rand() % 256);
+    for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+      if (grant.tb_en[tb]) {
+        for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+          data_tx[tb][byte] = (uint8_t) (rand() % 256);
+        }
       }
     }
 
@@ -444,8 +445,8 @@ int main(int argc, char **argv) {
       srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
     }
 #endif
-    for (i = 0; i < grant.nof_tb; i++) {
-      if (grant.mcs[i].tbs) {
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (grant.tb_en[i]) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
@@ -464,19 +465,23 @@ int main(int argc, char **argv) {
   }
 
   /* Check Tx and Rx bytes */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-      if (data_tx[tb][byte] != data_rx[tb][byte]) {
-        ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
-        ret = SRSLTE_ERROR;
-        goto quit;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        if (data_tx[tb][byte] != data_rx[tb][byte]) {
+          ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+          ret = SRSLTE_ERROR;
+          goto quit;
+        }
       }
     }
   }
 
   /* Check all transport blocks have been decoded OK */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+    }
   }
 
   ret = SRSLTE_SUCCESS;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 888b0d9b3..a67538ff7 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -280,21 +280,22 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
                                  int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type) {
   uint32_t pmi = 0;
+  uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
   if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-    if (grant->nof_tb == 1) {
+    if (nof_tb == 1) {
       if (grant->pinfo > 0 && grant->pinfo < 5) {
         pmi = grant->pinfo - 1;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     } else {
       if (grant->pinfo < 2) {
         pmi = grant->pinfo;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     }
@@ -336,29 +337,33 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
     
     /* ===== These lines of code are supposed to be MAC functionality === */
 
-    
+
     int rvidx[SRSLTE_MAX_CODEWORDS] = {1};
     if (dci_unpacked.rv_idx < 0) {
-      uint32_t sfn = tti/10; 
-      uint32_t k   = (sfn/2)%4;
-      for (int i = 0; i < grant.nof_tb; i++) {
-        rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+      uint32_t sfn = tti / 10;
+      uint32_t k = (sfn / 2) % 4;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+        }
       }
     } else {
-      for (int i = 0; i < grant.nof_tb; i++) {
-        switch(i) {
-          case 0:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
-            break;
-          case 1:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
-            break;
-          default:
-            ERROR("Wrong number of transport blocks");
-            return SRSLTE_ERROR;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          switch (i) {
+            case 0:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
+              break;
+            case 1:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
+              break;
+            default:
+              ERROR("Wrong number of transport blocks");
+              return SRSLTE_ERROR;
+          }
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
         }
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
 
@@ -372,14 +377,14 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
         }
         break;
       case SRSLTE_DCI_FORMAT2:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
         }
         break;
       case SRSLTE_DCI_FORMAT2A:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_CDD;
@@ -413,11 +418,13 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
                                 noise_estimate,
                                 rnti, data, acks);
 
-      for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
-        if (!acks[tb]) {
-          q->pkt_errors++;
+      for (int tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          if (!acks[tb]) {
+            q->pkt_errors++;
+          }
+          q->pkts_total++;
         }
-        q->pkts_total++;
       }
 
       if (ret == SRSLTE_ERROR) {
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 963551150..a9949cc3a 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -166,8 +166,10 @@ private:
     }
 
     void new_grant_dl(Tgrant grant, Taction *action) {
-      for (uint32_t tb = 0; tb < grant.phy_grant.dl.nof_tb; tb++) {
-        subproc[tb].new_grant_dl(grant, action);
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          subproc[tb].new_grant_dl(grant, action);
+        }
       }
     }
 
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 7b196f948..145f70e46 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -95,7 +95,7 @@ private:
   void set_uci_sr();
   void set_uci_periodic_cqi();
   void set_uci_aperiodic_cqi();
-  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb);
+  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS]);
   bool srs_is_ready_to_send();
   float set_power(float tx_power);
   void setup_tx_gain();
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 53dc232dc..8f9726a46 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -218,15 +218,17 @@ void phch_worker::work_imp()
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
 
         // NOTE: Currently hard-coded to 1st TB only
-        for (uint32_t tb = 0; tb < 1; tb++) {
-          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
-          dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+          if (dl_mac_grant.tb_en[tb]) {
+            phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+            dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
+            Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+          }
         }
       }
       Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);
       if (dl_action.generate_ack) {
-        set_uci_ack(dl_ack, dl_mac_grant.phy_grant.dl.nof_tb);
+        set_uci_ack(dl_ack, dl_mac_grant.tb_en);
       }
     }
   }
@@ -292,8 +294,10 @@ void phch_worker::work_imp()
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
       phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
-      for (uint32_t tb = 0; tb < dl_mac_grant.phy_grant.dl.nof_tb; tb++) {
-        phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (dl_mac_grant.tb_en[tb]) {
+          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+        }
       }
     }
   }
@@ -405,7 +409,15 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     grant->rnti = dl_rnti;
     grant->rnti_type = type;
     grant->last_tti = 0;
-    
+    grant->tb_en[0] = dci_unpacked.tb_en[0];
+    grant->tb_en[1] = dci_unpacked.tb_en[1];
+    grant->tb_cw_swap = dci_unpacked.tb_cw_swap;
+
+    if (grant->tb_cw_swap) {
+      Info("tb_cw_swap = true\n");
+      printf("tb_cw_swap = true\n");
+    }
+
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
     char hexstr[16];
@@ -432,8 +444,8 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
   srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
   int ret = SRSLTE_SUCCESS;
 
-  for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
-    if (rv[tb] < 0 || rv[tb] > 3) {
+  for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant->tb_en[tb] && (rv[tb] < 0 || rv[tb] > 3)) {
       valid_config = false;
       Error("Wrong RV (%d) for TB index %d", rv[tb], tb);
     }
@@ -452,22 +464,22 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM3\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM3\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM4\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM4\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
     break;
@@ -521,13 +533,14 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
-             grant->nof_prb, harq_pid,
-             grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
-             rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
-             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
-             srslte_pdsch_last_noi(&ue_dl.pdsch),
-             timestr);
+        Info(
+            "PDSCH: l_crb=%2d, harq=%d, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
+            grant->nof_prb, harq_pid, grant->tb_en[0] ? "on" : "off", grant->tb_en[1] ? "on" : "off",
+            grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
+            grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
+            10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+            srslte_pdsch_last_noi(&ue_dl.pdsch),
+            timestr);
 
         //printf("tti=%d, cfo=%f\n", tti, cfo*15000);
         //srslte_vec_save_file("pdsch", signal_buffer, sizeof(cf_t)*SRSLTE_SF_LEN_PRB(cell.nof_prb));
@@ -660,22 +673,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
-    if (nof_tb > 0) {
-      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    }
-
-    if (nof_tb > 1) {
-      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    }
+void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS])
+{
+  uint32_t nof_tb = 0;
+  if (tb_en[0]) {
+    uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    nof_tb++;
+  }
 
-    if (nof_tb > 2) {
-      Error("Number of transport blocks is not supported");
-    }
+  if (tb_en[1]) {
+    uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    nof_tb++;
+  }
 
-    uci_data.uci_ack_len = nof_tb;
+  uci_data.uci_ack_len = nof_tb;
 
-  }
+}
 
 void phch_worker::set_uci_sr()
 {

From a9d03ebb42d4fbdf1e63438f2bb6ed7e9f4dfa71 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 15:09:19 +0200
Subject: [PATCH 140/269] fixed offset length in pdsch scrambling sequence

---
 lib/src/phy/phch/pdsch.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index bc62df7fd..f761dbc3d 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -557,7 +557,7 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
         ERROR("Initialising scrambling sequence");
         return SRSLTE_ERROR;
       }
-      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], codeword_idx, nbits->nof_bits);
+      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], 0, nbits->nof_bits);
       srslte_sequence_free(&seq);
     }
 

From 30c6c8d21bdb62e81e376ec683024cbc53f896a1 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 15:09:19 +0200
Subject: [PATCH 141/269] fixed offset length in pdsch scrambling sequence

---
 lib/src/phy/phch/pdsch.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index bc62df7fd..f761dbc3d 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -557,7 +557,7 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
         ERROR("Initialising scrambling sequence");
         return SRSLTE_ERROR;
       }
-      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], codeword_idx, nbits->nof_bits);
+      srslte_scrambling_s_offset(&seq, pdsch->e[codeword_idx], 0, nbits->nof_bits);
       srslte_sequence_free(&seq);
     }
 

From b0639ab394b870796dfee13857d68c08a8d310dd Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 15:26:36 +0200
Subject: [PATCH 142/269] resolved merge conflicts

---
 cmake/modules/FindSSE.cmake                   |   40 +-
 lib/examples/cell_measurement.c               |   22 +-
 lib/examples/pdsch_enodeb.c                   |  360 ++-
 lib/examples/pdsch_ue.c                       |  212 +-
 lib/examples/usrp_capture.c                   |   36 +-
 lib/examples/usrp_capture_sync.c              |   25 +-
 lib/include/srslte/interfaces/ue_interfaces.h |   22 +-
 .../srslte/phy/ch_estimation/chest_dl.h       |    3 +
 lib/include/srslte/phy/common/phy_common.h    |   24 +-
 lib/include/srslte/phy/enb/enb_dl.h           |    4 +-
 lib/include/srslte/phy/io/filesink.h          |    5 +
 lib/include/srslte/phy/io/filesource.h        |    5 +
 lib/include/srslte/phy/mimo/precoding.h       |   22 +-
 lib/include/srslte/phy/phch/cqi.h             |    9 +-
 lib/include/srslte/phy/phch/pcfich.h          |    6 +-
 lib/include/srslte/phy/phch/pdcch.h           |   11 +-
 lib/include/srslte/phy/phch/pdsch.h           |   86 +-
 lib/include/srslte/phy/phch/pdsch_cfg.h       |   13 +-
 lib/include/srslte/phy/phch/phich.h           |    6 +-
 lib/include/srslte/phy/phch/pucch.h           |    6 +-
 lib/include/srslte/phy/phch/ra.h              |   13 +-
 lib/include/srslte/phy/phch/sch.h             |   14 +
 lib/include/srslte/phy/rf/rf.h                |   20 +-
 lib/include/srslte/phy/ue/ue_dl.h             |   80 +-
 lib/include/srslte/phy/ue/ue_sync.h           |    7 +
 lib/include/srslte/phy/utils/debug.h          |   10 +-
 lib/include/srslte/phy/utils/mat.h            |  115 +
 lib/include/srslte/phy/utils/simd.h           |   81 +
 lib/include/srslte/phy/utils/vector.h         |    3 +
 lib/src/phy/ch_estimation/chest_dl.c          |   15 +-
 lib/src/phy/ch_estimation/test/CMakeLists.txt |    1 -
 lib/src/phy/common/phy_common.c               |   46 +-
 lib/src/phy/dft/dft_fftw.c                    |    2 +-
 lib/src/phy/enb/enb_dl.c                      |   10 +-
 lib/src/phy/io/filesink.c                     |   57 +
 lib/src/phy/io/filesource.c                   |   26 +
 lib/src/phy/mimo/layermap.c                   |    8 +-
 lib/src/phy/mimo/precoding.c                  | 1936 ++++++++++++++++-
 lib/src/phy/mimo/test/CMakeLists.txt          |   28 +-
 lib/src/phy/mimo/test/layermap_test.c         |    8 +-
 lib/src/phy/mimo/test/pmi_select_test.c       |  158 ++
 lib/src/phy/mimo/test/pmi_select_test.h       |  237 ++
 lib/src/phy/mimo/test/precoder_mex.c          |   25 +-
 lib/src/phy/mimo/test/precoder_test.c         |  246 ++-
 lib/src/phy/mimo/test/predecoder_mex.c        |  161 +-
 lib/src/phy/phch/cqi.c                        |  103 +-
 lib/src/phy/phch/dci.c                        |   10 +
 lib/src/phy/phch/pcfich.c                     |    6 +-
 lib/src/phy/phch/pdcch.c                      |   54 +-
 lib/src/phy/phch/pdsch.c                      |  600 +++--
 lib/src/phy/phch/phich.c                      |    7 +-
 lib/src/phy/phch/pucch.c                      |    6 +-
 lib/src/phy/phch/ra.c                         |   74 +-
 lib/src/phy/phch/sch.c                        |   47 +-
 lib/src/phy/phch/test/CMakeLists.txt          |   74 +-
 lib/src/phy/phch/test/dlsch_encode_test_mex.c |   30 +-
 lib/src/phy/phch/test/pcfich_file_test.c      |    2 +-
 lib/src/phy/phch/test/pcfich_test.c           |    2 +-
 lib/src/phy/phch/test/pdcch_file_test.c       |    2 +-
 lib/src/phy/phch/test/pdcch_test.c            |  230 +-
 lib/src/phy/phch/test/pdcch_test_mex.c        |   14 +-
 lib/src/phy/phch/test/pdsch_pdcch_file_test.c |   20 +-
 lib/src/phy/phch/test/pdsch_test.c            |  460 +++-
 lib/src/phy/phch/test/pdsch_test_mex.c        |   27 +-
 lib/src/phy/phch/test/phich_file_test.c       |    2 +-
 lib/src/phy/phch/test/phich_test.c            |    2 +-
 lib/src/phy/phch/test/prach_detect_test_mex.c |    2 +
 lib/src/phy/phch/test/pucch_encode_test_mex.c |    2 +-
 lib/src/phy/phch/test/pucch_test_mex.c        |    6 +-
 lib/src/phy/rf/rf_blade_imp.c                 |   16 +-
 lib/src/phy/rf/rf_blade_imp.h                 |   12 +-
 lib/src/phy/rf/rf_dev.h                       |   12 +-
 lib/src/phy/rf/rf_imp.c                       |   32 +-
 lib/src/phy/rf/rf_uhd_imp.c                   |   47 +-
 lib/src/phy/rf/rf_uhd_imp.h                   |   12 +-
 lib/src/phy/ue/ue_dl.c                        |  348 ++-
 lib/src/phy/ue/ue_sync.c                      |   31 +-
 lib/src/phy/utils/mat.c                       |  284 +++
 lib/src/phy/utils/test/CMakeLists.txt         |    9 +
 lib/src/phy/utils/test/mat_test.c             |  415 ++++
 lib/src/phy/utils/vector.c                    |   31 +
 lib/src/radio/radio.cc                        |    9 +-
 srsenb/hdr/mac/mac_metrics.h                  |    1 +
 srsenb/hdr/mac/scheduler_ue.h                 |    5 +-
 srsenb/hdr/mac/ue.h                           |   18 +-
 srsenb/src/mac/mac.cc                         |    1 +
 srsenb/src/mac/scheduler.cc                   |   15 +-
 srsenb/src/mac/scheduler_harq.cc              |    2 +-
 srsenb/src/mac/scheduler_metric.cc            |    3 +-
 srsenb/src/mac/scheduler_ue.cc                |   11 +-
 srsenb/src/mac/ue.cc                          |   12 +-
 srsenb/src/main.cc                            |    1 +
 srsenb/src/metrics_stdout.cc                  |    7 +-
 srsenb/src/phy/phch_worker.cc                 |   11 +-
 srsue/hdr/mac/dl_harq.h                       |  378 ++--
 srsue/hdr/mac/mac.h                           |    2 +-
 srsue/hdr/mac/ul_harq.h                       |   32 +-
 srsue/hdr/phy/phch_worker.h                   |   13 +-
 srsue/src/mac/mac.cc                          |   18 +-
 srsue/src/mac/proc_ra.cc                      |   14 +-
 srsue/src/phy/phch_recv.cc                    |    4 +-
 srsue/src/phy/phch_worker.cc                  |  209 +-
 srsue/test/phy/ue_itf_test_prach.cc           |   76 +-
 srsue/test/phy/ue_itf_test_sib1.cc            |   29 +-
 srsue/ue.conf.example                         |    2 +-
 105 files changed, 6657 insertions(+), 1471 deletions(-)
 create mode 100644 lib/include/srslte/phy/utils/mat.h
 create mode 100644 lib/include/srslte/phy/utils/simd.h
 create mode 100644 lib/src/phy/mimo/test/pmi_select_test.c
 create mode 100644 lib/src/phy/mimo/test/pmi_select_test.h
 create mode 100644 lib/src/phy/utils/mat.c
 create mode 100644 lib/src/phy/utils/test/mat_test.c

diff --git a/cmake/modules/FindSSE.cmake b/cmake/modules/FindSSE.cmake
index 30be8a206..3440f01c1 100644
--- a/cmake/modules/FindSSE.cmake
+++ b/cmake/modules/FindSSE.cmake
@@ -7,6 +7,7 @@ include(CheckCSourceRuns)
 option(ENABLE_SSE "Enable compile-time SSE4.1 support." ON)
 option(ENABLE_AVX "Enable compile-time AVX support."  ON)
 option(ENABLE_AVX2 "Enable compile-time AVX2 support."  ON)
+option(ENABLE_FMA "Enable compile-time FMA support."  ON)
 
 if (ENABLE_SSE)
     #
@@ -97,8 +98,43 @@ if (ENABLE_SSE)
       if (HAVE_AVX2)
           message(STATUS "AVX2 is enabled - target CPU must support it")
       endif()
-  endif()
+    endif()
+
+    if (ENABLE_FMA)
+
+        #
+        # Check compiler for AVX intrinsics
+        #
+        if (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_CLANG )
+            set(CMAKE_REQUIRED_FLAGS "-mfma")
+            check_c_source_runs("
+            #include <immintrin.h>
+            int main()
+            {
+              __m256 a, b, c, r;
+              const float src[8] = { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f };
+              float dst[8];
+              a = _mm256_loadu_ps( src );
+              b = _mm256_loadu_ps( src );
+              c = _mm256_loadu_ps( src );
+              r = _mm256_fmadd_ps( a, b, c );
+              _mm256_storeu_ps( dst, r );
+              int i = 0;
+              for( i = 0; i < 8; i++ ){
+                if( ( src[i] * src[i] + src[i] ) != dst[i] ){
+                  return -1;
+                }
+              }
+              return 0;
+            }"
+                    HAVE_FMA)
+        endif()
+
+        if (HAVE_FMA)
+            message(STATUS "FMA is enabled - target CPU must support it")
+        endif()
+    endif()
 
 endif()
 
-mark_as_advanced(HAVE_SSE, HAVE_AVX, HAVE_AVX2)
+mark_as_advanced(HAVE_SSE, HAVE_AVX, HAVE_AVX2, HAVE_FMA)
diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index 96e98f055..143ee784e 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -118,7 +118,7 @@ int  parse_args(prog_args_t *args, int argc, char **argv) {
 /**********************************************************************/
 
 /* TODO: Do something with the output data */
-uint8_t data[1000000];
+uint8_t *data[SRSLTE_MAX_CODEWORDS];
 
 bool go_exit = false; 
 void sig_int_handler(int signo)
@@ -160,7 +160,8 @@ int main(int argc, char **argv) {
   int sfn_offset; 
   float rssi_utra=0,rssi=0, rsrp=0, rsrq=0, snr=0;
   cf_t *ce[SRSLTE_MAX_PORTS];
-  float cfo = 0; 
+  float cfo = 0;
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
 
   if (parse_args(&prog_args, argc, argv)) {
     exit(-1);
@@ -183,6 +184,9 @@ int main(int argc, char **argv) {
   }
   
   sf_buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t) * 1500*8);
+  }
 
   sigset_t sigset;
   sigemptyset(&sigset);
@@ -245,7 +249,7 @@ int main(int argc, char **argv) {
     fprintf(stderr, "Error initiating ue_sync\n");
     return -1;
   }
-  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, 1)) {
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, 1)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     return -1;
   }
@@ -326,19 +330,19 @@ int main(int argc, char **argv) {
         case DECODE_SIB:
           /* We are looking for SI Blocks, search only in appropiate places */
           if ((srslte_ue_sync_get_sfidx(&ue_sync) == 5 && (sfn%2)==0)) {
-            n = srslte_ue_dl_decode_multi(&ue_dl, sf_buffer, data, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync));
+            n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
             if (n < 0) {
               fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
               return -1;
             } else if (n == 0) {
               printf("CFO: %+6.4f kHz, SFO: %+6.4f kHz, NOI: %.2f, PDCCH-Det: %.3f\r",
                       srslte_ue_sync_get_cfo(&ue_sync)/1000, srslte_ue_sync_get_sfo(&ue_sync)/1000, 
-                      srslte_sch_average_noi(&ue_dl.pdsch.dl_sch),
+                      srslte_pdsch_average_noi(&ue_dl.pdsch),
                       (float) ue_dl.nof_detected/nof_trials);                
               nof_trials++; 
             } else {
               printf("Decoded SIB1. Payload: ");
-              srslte_vec_fprint_byte(stdout, data, n/8);;
+              srslte_vec_fprint_byte(stdout, data[0], n/8);;
               state = MEASURE;
             }
           }
@@ -402,6 +406,12 @@ int main(int argc, char **argv) {
     sf_cnt++;                  
   } // Main loop
 
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
+  }
+
   srslte_ue_sync_free(&ue_sync);
   srslte_rf_close(&rf);
   printf("\nBye\n");
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 2067519a6..3be278180 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -27,12 +27,12 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
-#include <sys/select.h>
 #include <pthread.h>
 #include <semaphore.h>
 #include <signal.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
 
 #include "srslte/srslte.h"
 
@@ -55,19 +55,23 @@ char *output_file_name = NULL;
 #define DOWN_KEY  66
 
 srslte_cell_t cell = {
-  25,            // nof_prb
-  1,            // nof_ports
-  0,            // cell_id
-  SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+    25,                 // nof_prb
+    1,                  // nof_ports
+    0,                  // cell_id
+    SRSLTE_CP_NORM,     // cyclic prefix
+    SRSLTE_PHICH_NORM,  // PHICH length
+    SRSLTE_PHICH_R_1    // PHICH resources
 };
-  
+
 int net_port = -1; // -1 generates random dataThat means there is some problem sending samples to the device
 
-uint32_t cfi=3;
+uint32_t cfi = 1;
 uint32_t mcs_idx = 1, last_mcs_idx = 1;
 int nof_frames = -1;
+char mimo_type_str[32] = "single";
+uint32_t nof_tb = 1;
+uint32_t multiplex_pmi = 0;
+uint32_t multiplex_nof_layers = 1;
 
 char *rf_args = "";
 float rf_amp = 0.8, rf_gain = 70.0, rf_freq = 2400000000;
@@ -80,14 +84,15 @@ srslte_pcfich_t pcfich;
 srslte_pdcch_t pdcch;
 srslte_pdsch_t pdsch;
 srslte_pdsch_cfg_t pdsch_cfg; 
-srslte_softbuffer_tx_t softbuffer; 
+srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
 srslte_regs_t regs;
-srslte_ra_dl_dci_t ra_dl;  
+srslte_ra_dl_dci_t ra_dl;
+int rvidx[SRSLTE_MAX_CODEWORDS] = {0, 0};
 
-cf_t *sf_buffer = NULL, *output_buffer = NULL;
+cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL}, *output_buffer [SRSLTE_MAX_PORTS] = {NULL};
 int sf_n_re, sf_n_samples;
 
-pthread_t net_thread; 
+pthread_t net_thread;
 void *net_thread_fnc(void *arg);
 sem_t net_sem;
 bool net_packet_ready = false; 
@@ -97,9 +102,12 @@ srslte_netsink_t net_sink;
 int prbset_num = 1, last_prbset_num = 1; 
 int prbset_orig = 0; 
 
+#define DATA_BUFF_SZ    1024*1024
+uint8_t *data[2], data2[DATA_BUFF_SZ];
+uint8_t data_tmp[DATA_BUFF_SZ];
 
 void usage(char *prog) {
-  printf("Usage: %s [agmfoncvpu]\n", prog);
+  printf("Usage: %s [agmfoncvpuxb]\n", prog);
 #ifndef DISABLE_RF
   printf("\t-a RF args [Default %s]\n", rf_args);
   printf("\t-l RF amplitude [Default %.2f]\n", rf_amp);
@@ -113,13 +121,18 @@ void usage(char *prog) {
   printf("\t-n number of frames [Default %d]\n", nof_frames);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-p nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-x Transmission mode[single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
+  printf("\t-b Precoding Matrix Index (multiplex mode only)* [Default %d]\n", multiplex_pmi);
+  printf("\t-w Number of codewords/layers (multiplex mode only)* [Default %d]\n", multiplex_nof_layers);
   printf("\t-u listen TCP port for input data (-1 is random) [Default %d]\n", net_port);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
+  printf("\n");
+  printf("\t*: See 3GPP 36.212 Table  5.3.3.1.5-4 for more information\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "aglfmoncpvu")) != -1) {
+  while ((opt = getopt(argc, argv, "aglfmoncpvutxbw")) != -1) {
     switch (opt) {
     case 'a':
       rf_args = argv[optind];
@@ -151,6 +164,15 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
+    case 'x':
+      strncpy(mimo_type_str, argv[optind], 32);
+      break;
+    case 'b':
+      multiplex_pmi = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'w':
+      multiplex_nof_layers = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -168,18 +190,61 @@ void parse_args(int argc, char **argv) {
 }
 
 void base_init() {
-  
-  /* init memory */
-  sf_buffer = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
-  if (!sf_buffer) {
-    perror("malloc");
+  int i;
+
+  /* Select transmission mode */
+  if (srslte_str2mimotype(mimo_type_str, &pdsch_cfg.mimo_type)) {
+    ERROR("Wrong transmission mode! Allowed modes: single, diversity, cdd and multiplex");
     exit(-1);
   }
-  output_buffer = srslte_vec_malloc(sizeof(cf_t) * sf_n_samples);
-  if (!output_buffer) {
-    perror("malloc");
-    exit(-1);
+
+  /* Configure cell and PDSCH in function of the transmission mode */
+  switch(pdsch_cfg.mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      cell.nof_ports = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      cell.nof_ports = 2;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      cell.nof_ports = 2;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      cell.nof_ports = 2;
+      break;
+    default:
+      ERROR("Transmission mode not implemented.");
+      exit(-1);
   }
+
+  /* Allocate memory */
+  for(i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t) * SOFTBUFFER_SIZE);
+    if (!data[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+    bzero(data[i], sizeof(uint8_t) * SOFTBUFFER_SIZE);
+  }
+
+  /* init memory */
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    sf_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_re);
+    if (!sf_buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+  }
+
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    output_buffer[i] = srslte_vec_malloc(sizeof(cf_t) * sf_n_samples);
+    if (!output_buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
+    bzero(output_buffer[i], sizeof(cf_t) * sf_n_samples);
+  }
+
   /* open file or USRP */
   if (output_file_name) {
     if (strcmp(output_file_name, "NULL")) {
@@ -194,7 +259,7 @@ void base_init() {
   } else {
 #ifndef DISABLE_RF
     printf("Opening RF device...\n");
-    if (srslte_rf_open(&rf, rf_args)) {
+    if (srslte_rf_open_multi(&rf, rf_args, cell.nof_ports)) {
       fprintf(stderr, "Error opening rf\n");
       exit(-1);
     }
@@ -241,7 +306,7 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pcfich_init(&pcfich)) {
+  if (srslte_pcfich_init(&pcfich, 1)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
@@ -255,7 +320,7 @@ void base_init() {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
+  if (srslte_pdcch_init_enb(&pdcch, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
@@ -274,16 +339,29 @@ void base_init() {
   }
 
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
-  
-  if (srslte_softbuffer_tx_init(&softbuffer, cell.nof_prb)) {
-    fprintf(stderr, "Error initiating soft buffer\n");
-    exit(-1);
+
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    softbuffers[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
+    if (!softbuffers[i]) {
+      fprintf(stderr, "Error allocating soft buffer\n");
+      exit(-1);
+    }
+
+    if (srslte_softbuffer_tx_init(softbuffers[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating soft buffer\n");
+      exit(-1);
+    }
   }
 }
 
 void base_free() {
-
-  srslte_softbuffer_tx_free(&softbuffer);
+  int i;
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    srslte_softbuffer_tx_free(softbuffers[i]);
+    if (softbuffers[i]) {
+      free(softbuffers[i]);
+    }
+  }
   srslte_pdsch_free(&pdsch);
   srslte_pdcch_free(&pdcch);
   srslte_regs_free(&regs);
@@ -291,11 +369,20 @@ void base_free() {
 
   srslte_ofdm_tx_free(&ifft);
 
-  if (sf_buffer) {
-    free(sf_buffer);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
   }
-  if (output_buffer) {
-    free(output_buffer);
+
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
+
+    if (output_buffer[i]) {
+      free(output_buffer[i]);
+    }
   }
   if (output_file_name) {
     if (!null_file_sink) {
@@ -348,15 +435,45 @@ uint32_t prbset_to_bitmask() {
 }
 
 int update_radl() {
-  
+
+  /* Configure cell and PDSCH in function of the transmission mode */
+  switch(pdsch_cfg.mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      pdsch_cfg.nof_layers = 1;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      pdsch_cfg.nof_layers = 2;
+      nof_tb = 2;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      pdsch_cfg.nof_layers = multiplex_nof_layers;
+      nof_tb = multiplex_nof_layers;
+      break;
+    default:
+      ERROR("Transmission mode not implemented.");
+      exit(-1);
+  }
+
   bzero(&ra_dl, sizeof(srslte_ra_dl_dci_t));
   ra_dl.harq_process = 0;
   ra_dl.mcs_idx = mcs_idx;
   ra_dl.ndi = 0;
-  ra_dl.rv_idx = 0;
+  ra_dl.rv_idx = rvidx[0];
   ra_dl.alloc_type = SRSLTE_RA_ALLOC_TYPE0;
   ra_dl.type0_alloc.rbg_bitmask = prbset_to_bitmask();
-  ra_dl.tb_en[0] = 1; 
+  ra_dl.tb_en[0] = 1;
+
+  if (nof_tb > 1) {
+    ra_dl.mcs_idx_1 = mcs_idx;
+    ra_dl.ndi_1 = 0;
+    ra_dl.rv_idx_1 = rvidx[1];
+    ra_dl.tb_en[1] = 1;
+  }
 
   srslte_ra_pdsch_fprint(stdout, &ra_dl, cell.nof_prb);
   srslte_ra_dl_grant_t dummy_grant; 
@@ -364,8 +481,21 @@ int update_radl() {
   srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &dummy_grant);
   srslte_ra_dl_grant_to_nbits(&dummy_grant, cfi, cell, 0, &dummy_nbits);
   srslte_ra_dl_grant_fprint(stdout, &dummy_grant);
-  printf("Type new MCS index and press Enter: "); fflush(stdout);
- 
+
+  if (pdsch_cfg.mimo_type != SRSLTE_MIMO_TYPE_SINGLE_ANTENNA) {
+    printf("\nTransmission mode key table:\n");
+    printf("   Mode   |   1TB   | 2TB |\n");
+    printf("----------+---------+-----+\n");
+    printf("Diversity |    x    |     |\n");
+    printf("      CDD |         |  z  |\n");
+    printf("Multiplex | q,w,e,r | a,s |\n");
+    printf("\n");
+    printf("Type new MCS index (0-28) or mode key and press Enter: ");
+  } else {
+    printf("Type new MCS index (0-28) and press Enter: ");
+  }
+  fflush(stdout);
+
   return 0; 
 }
 
@@ -406,8 +536,47 @@ int update_control() {
             break;          
         }
       } else {
-        last_mcs_idx = mcs_idx; 
-        mcs_idx = atoi(input);          
+        switch (input[0]) {
+          case 'q':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 0;
+            multiplex_nof_layers = 1;
+            break;
+          case 'w':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 1;
+            multiplex_nof_layers = 1;
+            break;
+          case 'e':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 2;
+            multiplex_nof_layers = 1;
+            break;
+          case 'r':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 3;
+            multiplex_nof_layers = 1;
+            break;
+          case 'a':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 0;
+            multiplex_nof_layers = 2;
+            break;
+          case 's':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+            multiplex_pmi = 1;
+            multiplex_nof_layers = 2;
+            break;
+          case 'z':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_CDD;
+            break;
+          case 'x':
+            pdsch_cfg.mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+            break;
+          default:
+            last_mcs_idx = mcs_idx;
+            mcs_idx = atoi(input);
+        }
       }
       bzero(input,sizeof(input));
       if (update_radl()) {
@@ -427,10 +596,6 @@ int update_control() {
   }
 }
 
-#define DATA_BUFF_SZ    1024*128
-uint8_t data[8*DATA_BUFF_SZ], data2[DATA_BUFF_SZ];
-uint8_t data_tmp[DATA_BUFF_SZ];
-
 /** Function run in a separate thread to receive UDP data */
 void *net_thread_fnc(void *arg) {
   int n; 
@@ -439,14 +604,16 @@ void *net_thread_fnc(void *arg) {
   do {
     n = srslte_netsource_read(&net_source, &data2[rpm], DATA_BUFF_SZ-rpm);
     if (n > 0) {
-      int nbytes = 1+(pdsch_cfg.grant.mcs.tbs-1)/8;
+      // FIXME: I assume that both transport blocks have same size in case of 2 tb are active
+      int nbytes = 1 + (pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs - 1) / 8;
       rpm += n; 
       INFO("received %d bytes. rpm=%d/%d\n",n,rpm,nbytes);
       wpm = 0; 
       while (rpm >= nbytes) {
         // wait for packet to be transmitted
         sem_wait(&net_sem);
-        memcpy(data, &data2[wpm], nbytes);          
+        memcpy(data[0], &data2[wpm], nbytes / (size_t) 2);
+        memcpy(data[1], &data2[wpm], nbytes / (size_t) 2);
         INFO("Sent %d/%d bytes ready\n", nbytes, rpm);
         rpm -= nbytes;          
         wpm += nbytes; 
@@ -517,9 +684,9 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  for (i = 0; i < SRSLTE_MAX_PORTS; i++) { // now there's only 1 port
-    sf_symbols[i] = sf_buffer;
-    slot1_symbols[i] = &sf_buffer[SRSLTE_SLOT_LEN_RE(cell.nof_prb, cell.cp)];
+  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    sf_symbols[i] = sf_buffer[i%cell.nof_ports];
+    slot1_symbols[i] = &sf_buffer[i%cell.nof_ports][SRSLTE_SLOT_LEN_RE(cell.nof_prb, cell.cp)];
   }
 
 #ifndef DISABLE_RF
@@ -576,7 +743,9 @@ int main(int argc, char **argv) {
   nf = 0;
   
   bool send_data = false; 
-  srslte_softbuffer_tx_reset(&softbuffer);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    srslte_softbuffer_tx_reset(softbuffers[i]);
+  }
 
 #ifndef DISABLE_RF
   bool start_of_burst = true; 
@@ -584,15 +753,25 @@ int main(int argc, char **argv) {
   
   while ((nf < nof_frames || nof_frames == -1) && !go_exit) {
     for (sf_idx = 0; sf_idx < SRSLTE_NSUBFRAMES_X_FRAME && (nf < nof_frames || nof_frames == -1); sf_idx++) {
-      bzero(sf_buffer, sizeof(cf_t) * sf_n_re);
+      /* Set Antenna port resource elements to zero */
+      bzero(sf_symbols[0], sizeof(cf_t) * sf_n_re);
 
+      /* Populate Synchronization signals if required */
       if (sf_idx == 0 || sf_idx == 5) {
-        srslte_pss_put_slot(pss_signal, sf_buffer, cell.nof_prb, SRSLTE_CP_NORM);
-        srslte_sss_put_slot(sf_idx ? sss_signal5 : sss_signal0, sf_buffer, cell.nof_prb,
+        srslte_pss_put_slot(pss_signal, sf_symbols[0], cell.nof_prb, SRSLTE_CP_NORM);
+        srslte_sss_put_slot(sf_idx ? sss_signal5 : sss_signal0, sf_symbols[0], cell.nof_prb,
             SRSLTE_CP_NORM);
       }
 
-      srslte_refsignal_cs_put_sf(cell, 0, est.csr_signal.pilots[0][sf_idx], sf_buffer);
+      /* Copy zeros, SSS, PSS into the rest of antenna ports */
+      for (i = 1; i < cell.nof_ports; i++) {
+        memcpy(sf_symbols[i], sf_symbols[0], sizeof(cf_t) * sf_n_re);
+      }
+
+      /* Put reference signals */
+      for (i = 0; i < cell.nof_ports; i++) {
+        srslte_refsignal_cs_put_sf(cell, (uint32_t) i, est.csr_signal.pilots[i / 2][sf_idx], sf_symbols[i]);
+      }
 
       srslte_pbch_mib_pack(&cell, sfn, bch_payload);
       if (sf_idx == 0) {
@@ -613,9 +792,11 @@ int main(int argc, char **argv) {
           INFO("Transmitting packet\n",0);
         }
       } else {
-        INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs.tbs);
-        for (i=0;i<pdsch_cfg.grant.mcs.tbs/8;i++) {
-          data[i] = rand()%256;
+        INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs);
+        for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
+          for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
+            data[tb][i] = (uint8_t) rand();
+          }
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
         if (sf_idx != 0 && sf_idx != 5) {
@@ -626,10 +807,30 @@ int main(int argc, char **argv) {
       }        
       
       if (send_data) {
-              
+        srslte_dci_format_t dci_format;
+        switch(pdsch_cfg.mimo_type) {
+          case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+            dci_format = SRSLTE_DCI_FORMAT1;
+            break;
+          case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+          case SRSLTE_MIMO_TYPE_CDD:
+            dci_format = SRSLTE_DCI_FORMAT2A;
+            break;
+          case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+            dci_format = SRSLTE_DCI_FORMAT2;
+            if (multiplex_nof_layers == 1) {
+              ra_dl.pinfo = (uint8_t) (multiplex_pmi + 1);
+            } else {
+              ra_dl.pinfo = (uint8_t) multiplex_pmi;
+            }
+            break;
+          default:
+            fprintf(stderr, "Wrong MIMO configuration\n");
+            exit(SRSLTE_ERROR);
+        }
         /* Encode PDCCH */
         INFO("Putting DCI to location: n=%d, L=%d\n", locations[sf_idx][0].ncce, locations[sf_idx][0].L);
-        srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_msg, cell.nof_prb, cell.nof_ports, false);
+        srslte_dci_msg_pack_pdsch(&ra_dl, dci_format, &dci_msg, cell.nof_prb, cell.nof_ports, false);
         if (srslte_pdcch_encode(&pdcch, &dci_msg, locations[sf_idx][0], UE_CRNTI, sf_symbols, sf_idx, cfi)) {
           fprintf(stderr, "Error encoding DCI message\n");
           exit(-1);
@@ -638,22 +839,24 @@ int main(int argc, char **argv) {
         /* Configure pdsch_cfg parameters */
         srslte_ra_dl_grant_t grant; 
         srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, sf_idx, 0)) {
+        if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
           fprintf(stderr, "Error configuring PDSCH\n");
           exit(-1);
         }
        
         /* Encode PDSCH */
-        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer, data, UE_CRNTI, sf_symbols)) {
+        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI, sf_symbols)) {
           fprintf(stderr, "Error encoding PDSCH\n");
           exit(-1);
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            srslte_bit_pack_vector(data, data_tmp, pdsch_cfg.grant.mcs.tbs);
-            if (srslte_netsink_write(&net_sink, data_tmp, 1+(pdsch_cfg.grant.mcs.tbs-1)/8) < 0) {
-              fprintf(stderr, "Error sending data through UDP socket\n");
-            }            
+            for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
+              srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
+              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
+                fprintf(stderr, "Error sending data through UDP socket\n");
+              }
+            }
           }
           net_packet_ready = false; 
           sem_post(&net_sem);
@@ -661,21 +864,24 @@ int main(int argc, char **argv) {
       }
       
       /* Transform to OFDM symbols */
-      srslte_ofdm_tx_sf(&ifft, sf_buffer, output_buffer);
-      
+      for (i = 0; i < cell.nof_ports; i++) {
+        srslte_ofdm_tx_sf(&ifft, sf_buffer[i], output_buffer[i]);
+      }
+
       /* send to file or usrp */
       if (output_file_name) {
         if (!null_file_sink) {
-          srslte_filesink_write(&fsink, output_buffer, sf_n_samples);          
+          srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);
         }
         usleep(1000);
       } else {
 #ifndef DISABLE_RF
-        // FIXME
         float norm_factor = (float) cell.nof_prb/15/sqrtf(pdsch_cfg.grant.nof_prb);
-        srslte_vec_sc_prod_cfc(output_buffer, rf_amp*norm_factor, output_buffer, SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        srslte_rf_send2(&rf, output_buffer, sf_n_samples, true, start_of_burst, false);
-        start_of_burst=false; 
+        for (i = 0; i < cell.nof_ports; i++) {
+          srslte_vec_sc_prod_cfc(output_buffer[i], rf_amp * norm_factor, output_buffer[i], SRSLTE_SF_LEN_PRB(cell.nof_prb));
+        }
+        srslte_rf_send_multi(&rf, (void**) output_buffer, sf_n_samples, true, start_of_burst, false);
+        start_of_burst=false;
 #endif
       }
     }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 3b58124cb..cf7bed2e4 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -36,6 +36,9 @@
 #include <signal.h>
 #include <pthread.h>
 #include <semaphore.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/phch/ra.h>
 
 #include "srslte/srslte.h"
 
@@ -98,6 +101,7 @@ typedef struct {
   int net_port_signal; 
   char *net_address_signal;
   int decimate;
+  int verbose;
 }prog_args_t;
 
 void args_default(prog_args_t *args) {
@@ -238,6 +242,7 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
       break;
     case 'v':
       srslte_verbose++;
+      args->verbose = srslte_verbose;
       break;
     case 'Z':
       args->decimate = atoi(argv[optind]);
@@ -258,7 +263,7 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
 /**********************************************************************/
 
 /* TODO: Do something with the output data */
-uint8_t data[20000];
+uint8_t *data[SRSLTE_MAX_CODEWORDS];
 
 bool go_exit = false; 
 void sig_int_handler(int signo)
@@ -266,10 +271,12 @@ void sig_int_handler(int signo)
   printf("SIGINT received. Exiting...\n");
   if (signo == SIGINT) {
     go_exit = true;
+  } else if (signo == SIGSEGV) {
+    exit(1);
   }
 }
 
-cf_t *sf_buffer[2] = {NULL, NULL}; 
+cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL};
 
 #ifndef DISABLE_RF
 int srslte_rf_recv_wrapper(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *t) {
@@ -298,6 +305,12 @@ prog_args_t prog_args;
 uint32_t sfn = 0; // system frame number
 srslte_netsink_t net_sink, net_sink_signal; 
 
+/* Useful macros for printing lines which will disappear */
+#define PRINT_LINE_INIT() int this_nof_lines = 0; static int prev_nof_lines = 0
+#define PRINT_LINE(_fmt, ...) printf("\033[K" _fmt "\n", ##__VA_ARGS__); this_nof_lines++
+#define PRINT_LINE_RESET_CURSOR() printf("\033[%dA", this_nof_lines); prev_nof_lines = this_nof_lines
+#define PRINT_LINE_ADVANCE_CURSOR() printf("\033[%dB", prev_nof_lines + 1)
+
 int main(int argc, char **argv) {
   int ret; 
   int decimate = 1;
@@ -308,13 +321,20 @@ int main(int argc, char **argv) {
   srslte_rf_t rf; 
 #endif
   uint32_t nof_trials = 0; 
-  int n; 
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
   int sfn_offset;
   float cfo = 0; 
-  
+
   parse_args(&prog_args, argc, argv);
   
+  for (int i = 0; i< SRSLTE_MAX_CODEWORDS; i++) {
+    data[i] = srslte_vec_malloc(sizeof(uint8_t)*1500*8);
+    if (!data[i]) {
+      ERROR("Allocating data");
+      go_exit = true;
+    }
+  }
+
   if(prog_args.cpu_affinity > -1) {
     
     cpu_set_t cpuset;
@@ -379,8 +399,8 @@ int main(int argc, char **argv) {
     srslte_rf_set_master_clock_rate(&rf, 30.72e6);        
 
     /* set receiver frequency */
-    printf("Tunning receiver to %.3f MHz\n", prog_args.rf_freq/1000000);
-    srslte_rf_set_rx_freq(&rf, prog_args.rf_freq);
+    printf("Tunning receiver to %.3f MHz\n", (prog_args.rf_freq + prog_args.file_offset_freq)/1000000);
+    srslte_rf_set_rx_freq(&rf, prog_args.rf_freq + prog_args.file_offset_freq);
     srslte_rf_rx_wait_lo_locked(&rf);
 
     uint32_t ntrial=0; 
@@ -435,8 +455,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_time, prog_args.file_offset_freq)) {
+    if (srslte_ue_sync_init_file_multi(&ue_sync, prog_args.file_nof_prb,
+      prog_args.input_file_name, prog_args.file_offset_time, prog_args.file_offset_freq, prog_args.rf_nof_rx_ant)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
     }
@@ -482,7 +502,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, prog_args.rf_nof_rx_ant)) {
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, prog_args.rf_nof_rx_ant)) {
     fprintf(stderr, "Error initiating UE downlink processing module\n");
     exit(-1);
   }
@@ -517,9 +537,15 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  float rsrp=0.0, rsrq=0.0, noise=0.0;
+  uint32_t ri = 0, pmi = 0;
+  float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
+      sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
 
+  for (int i = 0; i < SRSLTE_MAX_LAYERS; i++) {
+    bzero(sinr[i], sizeof(float)*SRSLTE_MAX_CODEBOOKS);
+  }
+
 #ifndef DISABLE_RF
   if (prog_args.rf_gain < 0) {
     srslte_ue_sync_start_agc(&ue_sync, srslte_rf_set_rx_gain_th_wrapper_, cell_detect_config.init_agc);
@@ -540,7 +566,29 @@ int main(int argc, char **argv) {
   INFO("\nEntering main loop...\n\n", 0);
   /* Main loop */
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
-    
+    bool acks [SRSLTE_MAX_CODEWORDS] = {false};
+    char input[128];
+    PRINT_LINE_INIT();
+
+    fd_set set;
+    FD_ZERO(&set);
+    FD_SET(0, &set);
+
+    struct timeval to;
+    to.tv_sec = 0;
+    to.tv_usec = 0;
+
+    /* Set default verbose level */
+    srslte_verbose = prog_args.verbose;
+    int n = select(1, &set, NULL, NULL, &to);
+    if (n == 1) {
+      /* If a new line is detected set verbose level to Debug */
+      if (fgets(input, sizeof(input), stdin)) {
+        srslte_verbose = SRSLTE_VERBOSE_DEBUG;
+      }
+    }
+
+
     ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
     if (ret < 0) {
       fprintf(stderr, "Error calling srslte_ue_sync_work()\n");
@@ -580,12 +628,30 @@ int main(int argc, char **argv) {
               decode_pdsch = false; 
             }
           }
-          if (decode_pdsch) {            
-            INFO("Attempting DL decode SFN=%d\n", sfn);
-            n = srslte_ue_dl_decode_multi(&ue_dl, 
-                                          sf_buffer, 
-                                          data, 
-                                          sfn*10+srslte_ue_sync_get_sfidx(&ue_sync));
+
+          INFO("Attempting DL decode SFN=%d\n", sfn);
+          if (decode_pdsch) {
+            if (cell.nof_ports == 1) {
+              /* Transmission mode 1 */
+              n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
+            } else {
+              if (prog_args.rf_nof_rx_ant == 1) {
+                /* Transmission mode 2 */
+                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 1, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                        acks);
+              } else {
+                /* Transmission mode 3 */
+                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 2, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                        acks);
+                if (n < 1) {
+                  /* Transmission mode 4 */
+                  n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 3, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                          acks);
+                }
+              }
+            }
+
+
           
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
@@ -593,7 +659,11 @@ int main(int argc, char **argv) {
               
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
-                srslte_netsink_write(&net_sink, data, 1+(n-1)/8);
+                // FIXME: UDP Data transmission does not work
+                for (uint32_t tb = 0; tb < ue_dl.pdsch_cfg.grant.nof_tb; tb++) {
+                  srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+
+                }
               }
               
               #ifdef PRINT_CHANGE_SCHEDULIGN
@@ -614,9 +684,13 @@ int main(int argc, char **argv) {
                                     
             nof_trials++; 
             
-            rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1);
-            rsrp = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp(&ue_dl.chest), rsrp, 0.05);      
-            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05);      
+            rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1f);
+            rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05f);
+            rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05f);
+            noise = SRSLTE_VEC_EMA(srslte_chest_dl_get_noise_estimate(&ue_dl.chest), noise, 0.05f);
+            enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f, enodebrate, 0.05f);
+            uerate = SRSLTE_VEC_EMA(((acks[0]?ue_dl.pdsch_cfg.grant.mcs[0].tbs:0) + (acks[1]?ue_dl.pdsch_cfg.grant.mcs[1].tbs:0))/1000.0f, uerate, 0.01f);
+
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -624,25 +698,78 @@ int main(int argc, char **argv) {
             if (isnan(noise)) {
               noise = 0; 
             }
-            if (isnan(rsrp)) {
-              rsrp = 0; 
-            }        
+            if (isnan(rsrp0)) {
+              rsrp1 = 0;
+            }
+            if (isnan(rsrp0)) {
+              rsrp1 = 0;
+            }
           }
 
           // Plot and Printf
-          if (srslte_ue_sync_get_sfidx(&ue_sync) == 5) {
+          if (srslte_ue_sync_get_sfidx(&ue_sync) == 5 && sfn % 20 == 0) {
             float gain = prog_args.rf_gain; 
             if (gain < 0) {
               gain = 10*log10(srslte_agc_get_gain(&ue_sync.agc)); 
             }
-            printf("CFO: %+6.2f kHz, "
-                   "SNR: %4.1f dB, "
-                   "PDCCH-Miss: %5.2f%%, PDSCH-BLER: %5.2f%%\r",
-                   
-                  srslte_ue_sync_get_cfo(&ue_sync)/1000,
-                  10*log10(rsrp/noise), 
-                  100*(1-(float) ue_dl.nof_detected/nof_trials), 
-                  (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total);                        
+
+            /* Print transmission scheme */
+            if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+              PRINT_LINE("    Tx scheme: %s (codebook_idx=%d)", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type),
+                         ue_dl.pdsch_cfg.codebook_idx);
+            } else {
+              PRINT_LINE("    Tx scheme: %s", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type));
+            }
+
+            /* Print basic Parameters */
+            PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
+            PRINT_LINE("nof codewords: %d", ue_dl.pdsch_cfg.grant.nof_tb);
+            PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
+            PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
+            PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
+            PRINT_LINE("   PDCCH-Miss: %5.2f%%", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
+            PRINT_LINE("   PDSCH-BLER: %5.2f%%", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+            PRINT_LINE("         TB 0: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[0].idx,
+                       ue_dl.pdsch_cfg.grant.mcs[0].tbs);
+            PRINT_LINE("         TB 1: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[1].idx,
+                       ue_dl.pdsch_cfg.grant.mcs[1].tbs);
+
+            /* MIMO: if tx and rx antennas are bigger than 1 */
+            if (cell.nof_ports > 1 && ue_dl.pdsch.nof_rx_antennas > 1) {
+              /* Compute condition number */
+              srslte_ue_dl_ri_select(&ue_dl, NULL, &cn);
+
+              /* Print condition number */
+              PRINT_LINE("            κ: %.1f dB (Condition number, 0 dB => Best)", cn);
+            }
+            PRINT_LINE("");
+
+            /* Spatial multiplex only */
+            if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+
+              /* Compute Rank Indicator (RI) and Precoding Matrix Indicator (PMI) */
+              srslte_ue_dl_ri_pmi_select(&ue_dl, &ri, &pmi, NULL);
+              for (uint32_t nl = 0; nl < SRSLTE_MAX_LAYERS; nl++) {
+                for (uint32_t cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb++) {
+                  sinr[nl][cb] = SRSLTE_VEC_EMA(ue_dl.sinr[nl][cb], sinr[nl][cb], 0.5f);
+                }
+              }
+
+              /* Print Multiplex stats */
+              PRINT_LINE("SINR (dB) Vs RI and PMI:");
+              PRINT_LINE("   | RI |   1   |   2   |");
+              PRINT_LINE(" -------+-------+-------+");
+              PRINT_LINE(" P |  0 | %5.2f%c| %5.2f%c|", 10 * log10(sinr[0][0]), (ri == 1 && pmi == 0) ? '*' : ' ',
+                         10 * log10(sinr[1][0]), (ri == 2 && pmi == 0) ? '*' : ' ');
+              PRINT_LINE(" M |  1 | %5.2f%c| %5.2f%c|", 10 * log10(sinr[0][1]), (ri == 1 && pmi == 1) ? '*' : ' ',
+                         10 * log10(sinr[1][1]), (ri == 2 && pmi == 1) ? '*' : ' ');
+              PRINT_LINE(" I |  2 | %5.2f%c|-------+ ", 10 * log10(sinr[0][2]), (ri == 1 && pmi == 2) ? '*' : ' ');
+              PRINT_LINE("   |  3 | %5.2f%c|         ", 10 * log10(sinr[0][3]), (ri == 1 && pmi == 3) ? '*' : ' ');
+              PRINT_LINE("");
+            }
+            PRINT_LINE("Press enter maximum printing debug log of 1 subframe.");
+            PRINT_LINE("");
+            PRINT_LINE_RESET_CURSOR();
           }
           break;
       }
@@ -650,7 +777,7 @@ int main(int argc, char **argv) {
         sfn++; 
         if (sfn == 1024) {
           sfn = 0; 
-          printf("\n");
+          PRINT_LINE_ADVANCE_CURSOR();
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
@@ -682,7 +809,8 @@ int main(int argc, char **argv) {
         
     sf_cnt++;                  
   } // Main loop
-  
+  printf("\033[30B\n");
+
 #ifndef DISABLE_GRAPHICS
   if (!prog_args.disable_plots) {
     if (!pthread_kill(plot_thread, 0)) {
@@ -693,6 +821,16 @@ int main(int argc, char **argv) {
 #endif
   srslte_ue_dl_free(&ue_dl);
   srslte_ue_sync_free(&ue_sync);
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data[i]) {
+      free(data[i]);
+    }
+  }
+  for (int i = 0; i < prog_args.rf_nof_rx_ant; i++) {
+    if (sf_buffer[i]) {
+      free(sf_buffer[i]);
+    }
+  }
   
 #ifndef DISABLE_RF
   if (!prog_args.input_file_name) {
@@ -761,7 +899,7 @@ void *plot_thread_run(void *arg) {
   while(1) {
     sem_wait(&plot_sem);
     
-    uint32_t nof_symbols = ue_dl.pdsch_cfg.nbits.nof_re;
+    uint32_t nof_symbols = ue_dl.pdsch_cfg.nbits[0].nof_re;
     if (!prog_args.disable_plots_except_constellation) {      
       for (i = 0; i < nof_re; i++) {
         tmp_plot[i] = 20 * log10f(cabsf(ue_dl.sf_symbols[i]));
@@ -803,7 +941,7 @@ void *plot_thread_run(void *arg) {
       plot_scatter_setNewData(&pscatequal_pdcch, ue_dl.pdcch.d, 36*ue_dl.pdcch.nof_cce);
     }
     
-    plot_scatter_setNewData(&pscatequal, ue_dl.pdsch.d, nof_symbols);
+    plot_scatter_setNewData(&pscatequal, ue_dl.pdsch.d[0], nof_symbols);
     
     if (plot_sf_idx == 1) {
       if (prog_args.net_port_signal > 0) {
diff --git a/lib/examples/usrp_capture.c b/lib/examples/usrp_capture.c
index 2692deb0d..02b543c0e 100644
--- a/lib/examples/usrp_capture.c
+++ b/lib/examples/usrp_capture.c
@@ -44,6 +44,7 @@ char *output_file_name;
 char *rf_args="";
 float rf_gain=40.0, rf_freq=-1.0, rf_rate=0.96e6;
 int nof_samples = -1;
+int nof_rx_antennas = 1;
 
 void int_handler(int dummy) {
   keep_running = false;
@@ -55,12 +56,13 @@ void usage(char *prog) {
   printf("\t-g RF Gain [Default %.2f dB]\n", rf_gain);
   printf("\t-r RF Rate [Default %.6f Hz]\n", rf_rate);
   printf("\t-n nof_samples [Default %d]\n", nof_samples);
+  printf("\t-A nof_rx_antennas [Default %d]\n", nof_rx_antennas);
   printf("\t-v srslte_verbose\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "agrnvfo")) != -1) {
+  while ((opt = getopt(argc, argv, "agrnvfoA")) != -1) {
     switch (opt) {
     case 'o':
       output_file_name = argv[optind];
@@ -80,6 +82,9 @@ void parse_args(int argc, char **argv) {
     case 'n':
       nof_samples = atoi(argv[optind]);
       break;
+    case 'A':
+      nof_rx_antennas = atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -95,11 +100,11 @@ void parse_args(int argc, char **argv) {
 }
 
 int main(int argc, char **argv) {
-  cf_t *buffer; 
+  cf_t *buffer[SRSLTE_MAX_PORTS];
   int sample_count, n;
   srslte_rf_t rf;
   srslte_filesink_t sink;
-  int32_t buflen;
+  uint32_t buflen;
 
   signal(SIGINT, int_handler);
 
@@ -107,17 +112,19 @@ int main(int argc, char **argv) {
   
   buflen = 4800;
   sample_count = 0;
-  
-  buffer = malloc(sizeof(cf_t) * buflen);
-  if (!buffer) {
-    perror("malloc");
-    exit(-1);
+
+  for (int i = 0; i < nof_rx_antennas; i++) {
+    buffer[i] = malloc(sizeof(cf_t) * buflen);
+    if (!buffer[i]) {
+      perror("malloc");
+      exit(-1);
+    }
   }
 
   srslte_filesink_init(&sink, output_file_name, SRSLTE_COMPLEX_FLOAT_BIN);
 
   printf("Opening RF device...\n");
-  if (srslte_rf_open(&rf, rf_args)) {
+  if (srslte_rf_open_multi(&rf, rf_args, nof_rx_antennas)) {
     fprintf(stderr, "Error opening rf\n");
     exit(-1);
   }
@@ -151,18 +158,23 @@ int main(int argc, char **argv) {
   
   while((sample_count < nof_samples || nof_samples == -1)
         && keep_running){
-    n = srslte_rf_recv(&rf, buffer, buflen, 1);
+    n = srslte_rf_recv_with_time_multi(&rf, (void**) buffer, buflen, true, NULL, NULL);
     if (n < 0) {
       fprintf(stderr, "Error receiving samples\n");
       exit(-1);
     }
     
-    srslte_filesink_write(&sink, buffer, buflen);
+    srslte_filesink_write_multi(&sink, (void**) buffer, buflen, nof_rx_antennas);
     sample_count += buflen;
   }
   
+  for (int i = 0; i < nof_rx_antennas; i++) {
+    if (buffer[i]) {
+      free(buffer[i]);
+    }
+  }
+
   srslte_filesink_free(&sink);
-  free(buffer);
   srslte_rf_close(&rf);
 
   printf("Ok - wrote %d samples\n", sample_count);
diff --git a/lib/examples/usrp_capture_sync.c b/lib/examples/usrp_capture_sync.c
index bf7595c01..bac17698a 100644
--- a/lib/examples/usrp_capture_sync.c
+++ b/lib/examples/usrp_capture_sync.c
@@ -45,6 +45,7 @@ float rf_gain=60.0, rf_freq=-1.0;
 int nof_prb = 6;
 int nof_subframes = -1;
 int N_id_2 = -1; 
+uint32_t nof_rx_antennas = 1;
 
 void int_handler(int dummy) {
   keep_running = false;
@@ -56,12 +57,13 @@ void usage(char *prog) {
   printf("\t-g RF Gain [Default %.2f dB]\n", rf_gain);
   printf("\t-p nof_prb [Default %d]\n", nof_prb);
   printf("\t-n nof_subframes [Default %d]\n", nof_subframes);
+  printf("\t-A nof_rx_antennas [Default %d]\n", nof_rx_antennas);
   printf("\t-v verbose\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "agpnvfol")) != -1) {
+  while ((opt = getopt(argc, argv, "agpnvfolA")) != -1) {
     switch (opt) {
     case 'o':
       output_file_name = argv[optind];
@@ -84,6 +86,9 @@ void parse_args(int argc, char **argv) {
     case 'l':
       N_id_2 = atoi(argv[optind]);
       break;
+    case 'A':
+      nof_rx_antennas = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -100,7 +105,7 @@ void parse_args(int argc, char **argv) {
 
 int srslte_rf_recv_wrapper(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *t) {
   DEBUG(" ----  Receive %d samples  ---- \n", nsamples);
-  return srslte_rf_recv(h, data[0], nsamples, 1);
+  return srslte_rf_recv_with_time_multi(h, (void**) data, nsamples, true, NULL, NULL);
 }
 
 int main(int argc, char **argv) {
@@ -118,13 +123,15 @@ int main(int argc, char **argv) {
   srslte_filesink_init(&sink, output_file_name, SRSLTE_COMPLEX_FLOAT_BIN);
 
   printf("Opening RF device...\n");
-  if (srslte_rf_open(&rf, rf_args)) {
+  if (srslte_rf_open_multi(&rf, rf_args, nof_rx_antennas)) {
     fprintf(stderr, "Error opening rf\n");
     exit(-1);
   }
   srslte_rf_set_master_clock_rate(&rf, 30.72e6);        
 
-  buffer[0] = srslte_vec_malloc(3*sizeof(cf_t)*SRSLTE_SF_LEN_PRB(100));
+  for (int i = 0; i< SRSLTE_MAX_PORTS; i++) {
+    buffer[i] = srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(100));
+  }
   
   sigset_t sigset;
   sigemptyset(&sigset);
@@ -158,7 +165,7 @@ int main(int argc, char **argv) {
   cell.nof_prb = nof_prb; 
   cell.nof_ports = 1; 
   
-  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, 1, (void*) &rf)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, cell.nof_prb, cell.id==1000, srslte_rf_recv_wrapper, nof_rx_antennas, (void*) &rf)) {
     fprintf(stderr, "Error initiating ue_sync\n");
     exit(-1); 
   }
@@ -185,7 +192,7 @@ int main(int argc, char **argv) {
         }        
       } else {
         printf("Writing to file %6d subframes...\r", subframe_count);
-        srslte_filesink_write(&sink, buffer[0], SRSLTE_SF_LEN_PRB(nof_prb));
+        srslte_filesink_write_multi(&sink, (void**) buffer, SRSLTE_SF_LEN_PRB(nof_prb),nof_rx_antennas);
         subframe_count++;                              
       }      
     }
@@ -200,6 +207,12 @@ int main(int argc, char **argv) {
   srslte_rf_close(&rf);
   srslte_ue_sync_free(&ue_sync);
 
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    if (buffer[i]) {
+      free(buffer[i]);
+    }
+  }
+
   printf("Ok - wrote %d subframes\n", subframe_count);
   exit(0);
 }
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index bf6a06be4..dcf477283 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -280,10 +280,10 @@ public:
     uint32_t    pid;    
     uint32_t    tti;
     uint32_t    last_tti;
-    bool        ndi; 
-    bool        last_ndi; 
-    uint32_t    n_bytes;
-    int         rv; 
+    bool        ndi[SRSLTE_MAX_CODEWORDS];
+    bool        last_ndi[SRSLTE_MAX_CODEWORDS];
+    uint32_t    n_bytes[SRSLTE_MAX_CODEWORDS];
+    int         rv[SRSLTE_MAX_CODEWORDS];
     uint16_t    rnti; 
     bool        is_from_rar;
     bool        is_sps_release;
@@ -294,28 +294,28 @@ public:
   
   typedef struct {
     bool                    decode_enabled;
-    int                     rv;
+    int                     rv[SRSLTE_MAX_TB];
     uint16_t                rnti; 
     bool                    generate_ack; 
     bool                    default_ack; 
     // If non-null, called after tb_decoded_ok to determine if ack needs to be sent
     bool                  (*generate_ack_callback)(void*); 
     void                   *generate_ack_callback_arg;
-    uint8_t                *payload_ptr; 
-    srslte_softbuffer_rx_t *softbuffer;
+    uint8_t                *payload_ptr[SRSLTE_MAX_TB];
+    srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_TB];
     srslte_phy_grant_t      phy_grant;
   } tb_action_dl_t;
 
   typedef struct {
     bool                    tx_enabled;
     bool                    expect_ack;
-    uint32_t                rv;
+    uint32_t                rv[SRSLTE_MAX_TB];
     uint16_t                rnti; 
     uint32_t                current_tx_nb;
     int32_t                 tti_offset;     // relative offset between grant and UL tx/HARQ rx
-    srslte_softbuffer_tx_t *softbuffer;
+    srslte_softbuffer_tx_t *softbuffers;
     srslte_phy_grant_t      phy_grant;
-    uint8_t                *payload_ptr; 
+    uint8_t                *payload_ptr[SRSLTE_MAX_TB];
   } tb_action_ul_t;
   
   /* Indicate reception of UL grant. 
@@ -332,7 +332,7 @@ public:
   virtual void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) = 0;
   
   /* Indicate successfull decoding of PDSCH TB. */
-  virtual void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid) = 0;
+  virtual void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) = 0;
   
   /* Indicate successfull decoding of BCH TB through PBCH */
   virtual void bch_decoded_ok(uint8_t *payload, uint32_t len) = 0;  
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index cecb099ce..c3fd795cf 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -135,6 +135,9 @@ SRSLTE_API float srslte_chest_dl_get_rssi(srslte_chest_dl_t *q);
 
 SRSLTE_API float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q);
 
+SRSLTE_API float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q,
+                                                     uint32_t port);
+
 SRSLTE_API float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q);
 
 #endif
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index 65dc2319c..29bf8d3a4 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -52,9 +52,12 @@
 #define SRSLTE_MAX_PORTS     4
 #define SRSLTE_MAX_LAYERS    4
 #define SRSLTE_MAX_CODEWORDS 2
+#define SRSLTE_MAX_TB        SRSLTE_MAX_CODEWORDS
 
 #define SRSLTE_MAX_CODEBLOCKS 32
 
+#define SRSLTE_MAX_CODEBOOKS 4
+
 #define SRSLTE_LTE_CRC24A  0x1864CFB
 #define SRSLTE_LTE_CRC24B  0X1800063
 #define SRSLTE_LTE_CRC16   0x11021
@@ -148,12 +151,12 @@ typedef enum SRSLTE_API {
 } srslte_phich_resources_t;
 
 typedef enum {
-  SRSLTE_RNTI_USER = 0,
-  SRSLTE_RNTI_SI,
-  SRSLTE_RNTI_RAR,
-  SRSLTE_RNTI_TEMP,
-  SRSLTE_RNTI_SPS,
-  SRSLTE_RNTI_PCH,  
+  SRSLTE_RNTI_USER = 0, /* Cell RNTI */
+  SRSLTE_RNTI_SI,       /* System Information RNTI */
+  SRSLTE_RNTI_RAR,      /* Random Access RNTI */
+  SRSLTE_RNTI_TEMP,     /* Temporary C-RNTI */
+  SRSLTE_RNTI_SPS,      /* Semi-Persistent Scheduling C-RNTI */
+  SRSLTE_RNTI_PCH,      /* Paging RNTI */
   SRSLTE_RNTI_NOF_TYPES
 } srslte_rnti_type_t;
 
@@ -173,6 +176,11 @@ typedef enum SRSLTE_API {
   SRSLTE_MIMO_TYPE_CDD
 } srslte_mimo_type_t;
 
+typedef enum SRSLTE_API {
+  SRSLTE_MIMO_DECODER_ZF,
+  SRSLTE_MIMO_DECODER_MMSE
+} srslte_mimo_decoder_t;
+
 typedef enum SRSLTE_API {
   SRSLTE_MOD_BPSK = 0, 
   SRSLTE_MOD_QPSK, 
@@ -247,6 +255,8 @@ SRSLTE_API float srslte_coderate(uint32_t tbs,
 
 SRSLTE_API char *srslte_cp_string(srslte_cp_t cp); 
 
+SRSLTE_API srslte_mod_t srslte_str2mod (char * mod_str);
+
 SRSLTE_API char *srslte_mod_string(srslte_mod_t mod);
 
 SRSLTE_API uint32_t srslte_mod_bits_x_symbol(srslte_mod_t mod);
@@ -276,6 +286,8 @@ SRSLTE_API int srslte_band_get_fd_region(enum band_geographical_area region,
 SRSLTE_API int srslte_str2mimotype(char *mimo_type_str, 
                                    srslte_mimo_type_t *type);
 
+SRSLTE_API char *srslte_mimotype2str(srslte_mimo_type_t mimo_type);
+
 SRSLTE_API uint32_t srslte_tti_interval(uint32_t tti1, 
                                         uint32_t tti2); 
 
diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index e6b9ae9ba..62062cca8 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -155,11 +155,11 @@ SRSLTE_API void srslte_enb_dl_rem_rnti(srslte_enb_dl_t *q,
 
 SRSLTE_API int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, 
                                        srslte_ra_dl_grant_t *grant, 
-                                       srslte_softbuffer_tx_t *softbuffer,
+                                       srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                                        uint16_t rnti,
                                        uint32_t rv_idx, 
                                        uint32_t sf_idx, 
-                                       uint8_t *data); 
+                                       uint8_t *data[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_dl(srslte_enb_dl_t *q, 
                                           srslte_ra_dl_dci_t *grant, 
diff --git a/lib/include/srslte/phy/io/filesink.h b/lib/include/srslte/phy/io/filesink.h
index b8ca8db51..921f1e440 100644
--- a/lib/include/srslte/phy/io/filesink.h
+++ b/lib/include/srslte/phy/io/filesink.h
@@ -59,4 +59,9 @@ SRSLTE_API int srslte_filesink_write(srslte_filesink_t *q,
                                      void *buffer, 
                                      int nsamples);
 
+SRSLTE_API int srslte_filesink_write_multi(srslte_filesink_t *q,
+                                           void **buffer,
+                                           int nsamples,
+                                           int nchannels);
+
 #endif // FILESINK_
diff --git a/lib/include/srslte/phy/io/filesource.h b/lib/include/srslte/phy/io/filesource.h
index ee8bc080d..f515d7034 100644
--- a/lib/include/srslte/phy/io/filesource.h
+++ b/lib/include/srslte/phy/io/filesource.h
@@ -62,4 +62,9 @@ SRSLTE_API int srslte_filesource_read(srslte_filesource_t *q,
                                       void *buffer, 
                                       int nsamples);
 
+SRSLTE_API int srslte_filesource_read_multi(srslte_filesource_t *q,
+                                            void **buffer,
+                                            int nsamples,
+                                            int nof_channels);
+
 #endif // FILESOURCE_
diff --git a/lib/include/srslte/phy/mimo/precoding.h b/lib/include/srslte/phy/mimo/precoding.h
index 62fad08ef..58fe22a40 100644
--- a/lib/include/srslte/phy/mimo/precoding.h
+++ b/lib/include/srslte/phy/mimo/precoding.h
@@ -65,8 +65,9 @@ SRSLTE_API int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS],
 SRSLTE_API int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], 
                                      cf_t *y[SRSLTE_MAX_PORTS], 
                                      int nof_layers,
-                                     int nof_ports, 
-                                     int nof_symbols, 
+                                     int nof_ports,
+                                     int codebook_idx,
+                                     int nof_symbols,
                                      srslte_mimo_type_t type);
 
 /* Estimates the vector "x" based on the received signal "y" and the channel estimates "h"
@@ -106,14 +107,31 @@ SRSLTE_API int srslte_predecoding_type(cf_t *y,
                                        srslte_mimo_type_t type, 
                                        float noise_estimate);
 
+SRSLTE_API void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo_decoder);
+
 SRSLTE_API int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], 
                                              cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], 
                                              cf_t *x[SRSLTE_MAX_LAYERS],
                                              int nof_rxant,
                                              int nof_ports, 
                                              int nof_layers, 
+                                             int codebook_idx,
                                              int nof_symbols, 
                                              srslte_mimo_type_t type, 
                                              float noise_estimate);
 
+SRSLTE_API int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                           uint32_t nof_symbols,
+                                           float noise_estimate,
+                                           int nof_layers,
+                                           uint32_t *pmi,
+                                           float sinr[SRSLTE_MAX_CODEBOOKS]);
+
+SRSLTE_API int srslte_precoding_cn(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                   uint32_t nof_tx_antennas,
+                                   uint32_t nof_rx_antennas,
+                                   uint32_t nof_symbols,
+                                   float *cn);
+
+
 #endif /* PRECODING_H_ */
diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index f6857dfa0..593592f1c 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -45,7 +45,8 @@
 typedef struct {
   bool     configured; 
   uint32_t pmi_idx; 
-  bool     simul_cqi_ack; 
+  uint32_t ri_idx;
+  bool     simul_cqi_ack;
   bool     format_is_subband; 
   uint32_t subband_size; 
 } srslte_cqi_periodic_cfg_t; 
@@ -137,7 +138,11 @@ SRSLTE_API int srslte_cqi_format2_subband_unpack(uint8_t buff[SRSLTE_CQI_MAX_BIT
                                                  srslte_cqi_format2_subband_t *msg);
 
 SRSLTE_API bool srslte_cqi_send(uint32_t I_cqi_pmi, 
-                                uint32_t tti); 
+                                uint32_t tti);
+
+SRSLTE_API bool srslte_ri_send(uint32_t I_cqi_pmi,
+                               uint32_t I_ri,
+                               uint32_t tti);
 
 SRSLTE_API uint8_t srslte_cqi_from_snr(float snr);
 
diff --git a/lib/include/srslte/phy/phch/pcfich.h b/lib/include/srslte/phy/phch/pcfich.h
index 7c94d37b2..fbbb82c88 100644
--- a/lib/include/srslte/phy/phch/pcfich.h
+++ b/lib/include/srslte/phy/phch/pcfich.h
@@ -78,10 +78,8 @@ typedef struct SRSLTE_API {
   
 } srslte_pcfich_t;
 
-SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q);
-
-SRSLTE_API int srslte_pcfich_init_multi(srslte_pcfich_t *q, 
-                                        uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_pcfich_init(srslte_pcfich_t *q,
+                                  uint32_t nof_rx_antennas);
 
 SRSLTE_API int srslte_pcfich_set_cell(srslte_pcfich_t *q,
                                       srslte_regs_t *regs,
diff --git a/lib/include/srslte/phy/phch/pdcch.h b/lib/include/srslte/phy/phch/pdcch.h
index b179c21e7..9bb896b34 100644
--- a/lib/include/srslte/phy/phch/pdcch.h
+++ b/lib/include/srslte/phy/phch/pdcch.h
@@ -64,6 +64,7 @@ typedef struct SRSLTE_API {
   uint32_t nof_cce;
   uint32_t max_bits;
   uint32_t nof_rx_antennas;
+  bool     is_ue;
   
   srslte_regs_t *regs;
 
@@ -84,12 +85,12 @@ typedef struct SRSLTE_API {
   
 } srslte_pdcch_t;
 
-SRSLTE_API int srslte_pdcch_init(srslte_pdcch_t *q,
-                                 uint32_t max_prb);
+SRSLTE_API int srslte_pdcch_init_ue(srslte_pdcch_t *q,
+                                    uint32_t max_prb,
+                                    uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_pdcch_init_multi(srslte_pdcch_t *q,
-                                       uint32_t max_prb,
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_pdcch_init_enb(srslte_pdcch_t *q,
+                                     uint32_t max_prb);
 
 SRSLTE_API int srslte_pdcch_set_cell(srslte_pdcch_t *q,
                                      srslte_regs_t *regs,
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index b163f45bf..7c959f65c 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -48,7 +48,7 @@
 #include "srslte/phy/phch/pdsch_cfg.h"
 
 typedef struct {
-  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];
+  srslte_sequence_t seq[SRSLTE_MAX_CODEWORDS][SRSLTE_NSUBFRAMES_X_FRAME];
   uint32_t cell_id;
   bool sequence_generated;
 } srslte_pdsch_user_t;
@@ -65,11 +65,11 @@ typedef struct SRSLTE_API {
 
   /* buffers */
   // void buffers are shared for tx and rx
-  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  cf_t *symbols[SRSLTE_MAX_PORTS];
-  cf_t *x[SRSLTE_MAX_PORTS];
-  cf_t *d;
-  void *e;
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; /* Channel estimation (Rx only) */
+  cf_t *symbols[SRSLTE_MAX_PORTS];              /* PDSCH Encoded/Decoded Symbols */
+  cf_t *x[SRSLTE_MAX_LAYERS];                   /* Layer mapped */
+  cf_t *d[SRSLTE_MAX_CODEWORDS];                /* Modulated/Demodulated codewords */
+  void *e[SRSLTE_MAX_CODEWORDS];
 
   /* tx & rx objects */
   srslte_modem_table_t mod[4];
@@ -80,23 +80,17 @@ typedef struct SRSLTE_API {
   srslte_sequence_t tmp_seq;
 
   srslte_sch_t dl_sch;
-  
+
 } srslte_pdsch_t;
 
+
 SRSLTE_API int srslte_pdsch_init_ue(srslte_pdsch_t *q,
-                                    uint32_t max_prb);
+                                    uint32_t max_prb,
+                                    uint32_t nof_rx_antennas);
 
 SRSLTE_API int srslte_pdsch_init_enb(srslte_pdsch_t *q,
                                      uint32_t max_prb);
 
-SRSLTE_API int srslte_pdsch_init_multi_ue(srslte_pdsch_t *q,
-                                          uint32_t max_prb,
-                                          uint32_t nof_rx_antennas);
-
-SRSLTE_API int srslte_pdsch_init_multi_enb(srslte_pdsch_t *q,
-                                           uint32_t max_prb,
-                                           uint32_t nof_rx_antennas);
-
 SRSLTE_API void srslte_pdsch_free(srslte_pdsch_t *q);
 
 SRSLTE_API int srslte_pdsch_set_cell(srslte_pdsch_t *q,
@@ -113,34 +107,50 @@ SRSLTE_API int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg,
                                 srslte_ra_dl_grant_t *grant, 
                                 uint32_t cfi, 
                                 uint32_t sf_idx, 
-                                uint32_t rvidx); 
+                                int rvidx);
+
+SRSLTE_API int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg,
+                                     srslte_cell_t cell,
+                                     srslte_ra_dl_grant_t *grant,
+                                     uint32_t cfi,
+                                     uint32_t sf_idx,
+                                     int rvidx[SRSLTE_MAX_CODEWORDS],
+                                     srslte_mimo_type_t mimo_type,
+                                     uint32_t pmi);
 
 SRSLTE_API int srslte_pdsch_encode(srslte_pdsch_t *q,
-                                   srslte_pdsch_cfg_t *cfg,
-                                   srslte_softbuffer_tx_t *softbuffer,
-                                   uint8_t *data, 
-                                   uint16_t rnti,
-                                   cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
+                                         srslte_pdsch_cfg_t *cfg,
+                                         srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                                         uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                         uint16_t rnti,
+                                         cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
 
 SRSLTE_API int srslte_pdsch_decode(srslte_pdsch_t *q, 
-                                   srslte_pdsch_cfg_t *cfg, 
-                                   srslte_softbuffer_rx_t *softbuffer,
-                                   cf_t *sf_symbols, 
-                                   cf_t *ce[SRSLTE_MAX_PORTS],
-                                   float noise_estimate, 
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                                   cf_t *sf_symbols[SRSLTE_MAX_PORTS],
+                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                   float noise_estimate,
                                    uint16_t rnti,
-                                   uint8_t *data);
-
-SRSLTE_API int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
-                                         srslte_pdsch_cfg_t *cfg, 
-                                         srslte_softbuffer_rx_t *softbuffer,
-                                         cf_t *sf_symbols[SRSLTE_MAX_PORTS], 
-                                         cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                         float noise_estimate, 
-                                         uint16_t rnti,
-                                         uint8_t *data);
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                   bool acks[SRSLTE_MAX_CODEWORDS]);
+
+SRSLTE_API int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
+                                       srslte_pdsch_cfg_t *cfg,
+                                       cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                       float noise_estimate,
+                                       uint32_t nof_ce,
+                                       uint32_t pmi[SRSLTE_MAX_LAYERS],
+                                       float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]);
+
+SRSLTE_API int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
+                                       cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                       uint32_t nof_ce,
+                                       float *cn);
+
+SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter);
 
-SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q); 
+SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q);
 
 SRSLTE_API uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q); 
 
diff --git a/lib/include/srslte/phy/phch/pdsch_cfg.h b/lib/include/srslte/phy/phch/pdsch_cfg.h
index 2745470f8..eb4927fbb 100644
--- a/lib/include/srslte/phy/phch/pdsch_cfg.h
+++ b/lib/include/srslte/phy/phch/pdsch_cfg.h
@@ -40,11 +40,14 @@
 #include "srslte/phy/fec/cbsegm.h"
 
 typedef struct SRSLTE_API {
-  srslte_cbsegm_t cb_segm; 
-  srslte_ra_dl_grant_t grant; 
-  srslte_ra_nbits_t nbits; 
-  uint32_t rv; 
-  uint32_t sf_idx;  
+  srslte_cbsegm_t cb_segm[SRSLTE_MAX_CODEWORDS];
+  srslte_ra_dl_grant_t grant;
+  srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS];
+  uint32_t rv[SRSLTE_MAX_CODEWORDS];
+  uint32_t sf_idx;
+  uint32_t nof_layers;
+  uint32_t codebook_idx;
+  srslte_mimo_type_t mimo_type;
 } srslte_pdsch_cfg_t;
 
 #endif
diff --git a/lib/include/srslte/phy/phch/phich.h b/lib/include/srslte/phy/phch/phich.h
index 8c519efd6..f560fdc80 100644
--- a/lib/include/srslte/phy/phch/phich.h
+++ b/lib/include/srslte/phy/phch/phich.h
@@ -85,10 +85,8 @@ typedef struct SRSLTE_API {
   
 } srslte_phich_t;
 
-SRSLTE_API int srslte_phich_init(srslte_phich_t *q);
-
-SRSLTE_API int srslte_phich_init_multi(srslte_phich_t *q, 
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_phich_init(srslte_phich_t *q,
+                                 uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_phich_free(srslte_phich_t *q);
 
diff --git a/lib/include/srslte/phy/phch/pucch.h b/lib/include/srslte/phy/phch/pucch.h
index 3b8ca31a4..8aa1495c8 100644
--- a/lib/include/srslte/phy/phch/pucch.h
+++ b/lib/include/srslte/phy/phch/pucch.h
@@ -100,9 +100,9 @@ typedef struct SRSLTE_API {
   uint32_t f_gh[SRSLTE_NSLOTS_X_FRAME];
   float tmp_arg[SRSLTE_PUCCH_N_SEQ];
   
-  cf_t z[SRSLTE_PUCCH_MAX_SYMBOLS];
-  cf_t z_tmp[SRSLTE_PUCCH_MAX_SYMBOLS];
-  cf_t ce[SRSLTE_PUCCH_MAX_SYMBOLS];
+  cf_t *z;
+  cf_t *z_tmp;
+  cf_t *ce;
   
   bool shortened; 
   bool group_hopping_en;
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index cec2e6087..9f31a0218 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -102,11 +102,10 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   bool prb_idx[2][SRSLTE_MAX_PRB];
   uint32_t nof_prb;  
-  uint32_t Qm;
-  uint32_t Qm2; 
-  srslte_ra_mcs_t mcs;
-  srslte_ra_mcs_t mcs2;
+  uint32_t Qm[SRSLTE_MAX_CODEWORDS];
+  srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
   uint32_t nof_tb;
+  uint32_t pinfo;
 } srslte_ra_dl_grant_t;
 
 /** Unpacked DCI message for DL grant */
@@ -206,10 +205,10 @@ SRSLTE_API int srslte_ra_dl_dci_to_grant(srslte_ra_dl_dci_t *dci,
 SRSLTE_API void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, 
                                             uint32_t cfi, 
                                             srslte_cell_t cell, 
-                                            uint32_t sf_idx, 
-                                            srslte_ra_nbits_t *nbits); 
+                                            uint32_t sf_idx,
+                                            srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS]);
 
-SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, 
+SRSLTE_API uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell,
                                                uint32_t nof_prb, 
                                                uint32_t nof_ctrl_symbols); 
 
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index 4ef0b3070..a03387448 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -96,12 +96,26 @@ SRSLTE_API int srslte_dlsch_encode(srslte_sch_t *q,
                                    uint8_t *data, 
                                    uint8_t *e_bits);
 
+SRSLTE_API int srslte_dlsch_encode2(srslte_sch_t *q,
+                                    srslte_pdsch_cfg_t *cfg,
+                                    srslte_softbuffer_tx_t *softbuffer,
+                                    uint8_t *data,
+                                    uint8_t *e_bits,
+                                    int codeword_idx);
+
 SRSLTE_API int srslte_dlsch_decode(srslte_sch_t *q, 
                                    srslte_pdsch_cfg_t *cfg,
                                    srslte_softbuffer_rx_t *softbuffer,
                                    int16_t *e_bits, 
                                    uint8_t *data);
 
+SRSLTE_API int srslte_dlsch_decode2(srslte_sch_t *q,
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_rx_t *softbuffer,
+                                   int16_t *e_bits,
+                                   uint8_t *data,
+                                   int codeword_idx);
+
 SRSLTE_API int srslte_ulsch_encode(srslte_sch_t *q, 
                                    srslte_pusch_cfg_t *cfg,
                                    srslte_softbuffer_tx_t *softbuffer,
diff --git a/lib/include/srslte/phy/rf/rf.h b/lib/include/srslte/phy/rf/rf.h
index 907a5a008..860e0a257 100644
--- a/lib/include/srslte/phy/rf/rf.h
+++ b/lib/include/srslte/phy/rf/rf.h
@@ -73,9 +73,9 @@ SRSLTE_API int srslte_rf_open(srslte_rf_t *h,
 
 SRSLTE_API int srslte_rf_open_multi(srslte_rf_t *h, 
                                     char *args, 
-                                    uint32_t nof_rx_antennas); 
+                                    uint32_t nof_rx_antennas);
 
-SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h, 
+SRSLTE_API int srslte_rf_open_devname(srslte_rf_t *h,
                                       char *devname, 
                                       char *args);
 
@@ -200,5 +200,21 @@ SRSLTE_API int srslte_rf_send_timed2(srslte_rf_t *h,
                               bool is_start_of_burst,
                               bool is_end_of_burst);
 
+SRSLTE_API int srslte_rf_send_timed_multi(srslte_rf_t *rf,
+                                          void *data[4],
+                                          int nsamples,
+                                          time_t secs,
+                                          double frac_secs,
+                                          bool blocking,
+                                          bool is_start_of_burst,
+                                          bool is_end_of_burst);
+
+SRSLTE_API int srslte_rf_send_multi(srslte_rf_t *rf,
+                                    void *data[4],
+                                    int nsamples,
+                                    bool blocking,
+                                    bool is_start_of_burst,
+                                    bool is_end_of_burst);
+
 #endif
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index fb3ec585b..c4e799424 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -84,7 +84,7 @@ typedef struct SRSLTE_API {
   srslte_cfo_t sfo_correct; 
   
   srslte_pdsch_cfg_t pdsch_cfg; 
-  srslte_softbuffer_rx_t softbuffer;
+  srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
   srslte_ra_dl_dci_t dl_dci;
   srslte_cell_t cell;
 
@@ -94,7 +94,12 @@ typedef struct SRSLTE_API {
   cf_t *sf_symbols_m[SRSLTE_MAX_PORTS]; 
   cf_t *ce[SRSLTE_MAX_PORTS]; // compatibility
   cf_t *ce_m[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
+
+  /* RI, PMI and SINR for MIMO statistics */
+  float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS];
+  uint32_t pmi[SRSLTE_MAX_LAYERS];
+  uint32_t ri;
+
   srslte_dci_format_t dci_format;
   uint64_t pkt_errors; 
   uint64_t pkts_total;
@@ -113,25 +118,17 @@ typedef struct SRSLTE_API {
 }srslte_ue_dl_t;
 
 /* This function shall be called just after the initial synchronization */
-SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                                 uint32_t max_prb);
-
-SRSLTE_API int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
-                                       uint32_t max_prb,
-                                       uint32_t nof_rx_antennas);
+SRSLTE_API int srslte_ue_dl_init(srslte_ue_dl_t *q,
+                                 uint32_t max_prb,
+                                 uint32_t nof_rx_antennas);
 
 SRSLTE_API void srslte_ue_dl_free(srslte_ue_dl_t *q);
 
 SRSLTE_API int srslte_ue_dl_set_cell(srslte_ue_dl_t *q,
                                           srslte_cell_t cell);
 
-SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, 
-                                                cf_t *input, 
-                                                uint32_t sf_idx, 
-                                                uint32_t *cfi); 
-
-SRSLTE_API int srslte_ue_dl_decode_fft_estimate_multi(srslte_ue_dl_t *q, 
-                                                cf_t *input[SRSLTE_MAX_PORTS], 
+SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q,
+                                                cf_t *input[SRSLTE_MAX_PORTS],
                                                 uint32_t sf_idx, 
                                                 uint32_t *cfi); 
 
@@ -139,11 +136,12 @@ SRSLTE_API int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q,
                                             uint32_t sf_idx, 
                                             uint32_t *cfi); 
 
-SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, 
-                                      srslte_ra_dl_grant_t *grant, 
-                                      uint32_t cfi, 
-                                      uint32_t sf_idx, 
-                                      uint32_t rvidx); 
+SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q,
+                                      srslte_ra_dl_grant_t *grant,
+                                      uint32_t cfi,
+                                      uint32_t sf_idx,
+                                      int rvidx[SRSLTE_MAX_CODEWORDS],
+                                      srslte_mimo_type_t mimo_type);
 
 SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, 
                                         uint32_t cfi, 
@@ -152,12 +150,14 @@ SRSLTE_API int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q,
                                         srslte_dci_msg_t *dci_msg); 
 
 SRSLTE_API int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, 
+                                        uint32_t tm,
                                         uint32_t cfi, 
                                         uint32_t sf_idx, 
                                         uint16_t rnti, 
                                         srslte_dci_msg_t *dci_msg); 
 
 SRSLTE_API int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, 
+                                             uint32_t tm,
                                              uint32_t cfi, 
                                              uint32_t sf_idx, 
                                              uint16_t rnti, 
@@ -169,27 +169,29 @@ SRSLTE_API uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q);
 SRSLTE_API void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, 
                                                float sample_offset); 
 
-SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t * q, 
-                                   cf_t *input, 
-                                   uint8_t *data,
-                                   uint32_t tti);
-
-SRSLTE_API int srslte_ue_dl_decode_multi(srslte_ue_dl_t * q, 
-                                         cf_t *input[SRSLTE_MAX_PORTS], 
-                                         uint8_t *data,
-                                         uint32_t tti);
-
-SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t * q, 
-                                        cf_t *input, 
-                                        uint8_t *data,
+SRSLTE_API int srslte_ue_dl_decode(srslte_ue_dl_t *q,
+                                   cf_t *input[SRSLTE_MAX_PORTS],
+                                   uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                   uint32_t tm,
+                                   uint32_t tti,
+                                   bool acks[SRSLTE_MAX_CODEWORDS]);
+
+SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
+                                        cf_t *input[SRSLTE_MAX_PORTS],
+                                        uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                        uint32_t tm,
                                         uint32_t tti,
-                                        uint16_t rnti);
+                                        uint16_t rnti,
+                                        bool acks[SRSLTE_MAX_CODEWORDS]);
+
+SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
+                                          uint32_t *ri,
+                                          uint32_t *pmi,
+                                          float *current_sinr);
 
-SRSLTE_API int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t * q, 
-                                              cf_t *input[SRSLTE_MAX_PORTS], 
-                                              uint8_t *data,
-                                              uint32_t tti,
-                                              uint16_t rnti);
+SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
+                                      uint32_t *ri,
+                                      float *cn);
 
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
                                           uint32_t sf_idx, 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index 4bd8efb01..dde9fb566 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -153,6 +153,13 @@ SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q,
                                         int offset_time, 
                                         float offset_freq);
 
+SRSLTE_API int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q,
+                                              uint32_t nof_prb,
+                                              char *file_name,
+                                              int offset_time,
+                                              float offset_freq,
+                                              uint32_t nof_rx_ant);
+
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
 SRSLTE_API int srslte_ue_sync_set_cell(srslte_ue_sync_t *q,
diff --git a/lib/include/srslte/phy/utils/debug.h b/lib/include/srslte/phy/utils/debug.h
index 9233d9c61..c88e1a3ce 100644
--- a/lib/include/srslte/phy/utils/debug.h
+++ b/lib/include/srslte/phy/utils/debug.h
@@ -58,10 +58,16 @@ SRSLTE_API extern int srslte_verbose;
 #define PRINT_NONE srslte_verbose=SRSLTE_VERBOSE_NONE
 
 #define DEBUG(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_DEBUG) \
-  fprintf(stdout, "[DEBUG]: " _fmt, __VA_ARGS__)
+  fprintf(stdout, "[DEBUG]: " _fmt, ##__VA_ARGS__)
 
 #define INFO(_fmt, ...) if (SRSLTE_DEBUG_ENABLED && srslte_verbose >= SRSLTE_VERBOSE_INFO) \
-  fprintf(stdout, "[INFO]:  " _fmt, __VA_ARGS__)
+  fprintf(stdout, "[INFO]:  " _fmt, ##__VA_ARGS__)
 
+#if CMAKE_BUILD_TYPE==Debug
+/* In debug mode, it prints out the  */
+#define ERROR(_fmt, ...) fprintf(stderr, "%s.%d: " _fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)
+#else
+#define ERROR(_fmt, ...) fprintf(stderr, "[ERROR in %s]:" _fmt "\n", __FUNCTION__, ##__VA_ARGS__)
+#endif /* CMAKE_BUILD_TYPE==Debug */
 
 #endif // DEBUG_H
diff --git a/lib/include/srslte/phy/utils/mat.h b/lib/include/srslte/phy/utils/mat.h
new file mode 100644
index 000000000..d960590c4
--- /dev/null
+++ b/lib/include/srslte/phy/utils/mat.h
@@ -0,0 +1,115 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef SRSLTE_MAT_H
+#define SRSLTE_MAT_H
+
+#include "srslte/phy/utils/simd.h"
+#include "srslte/config.h"
+
+
+/*
+ * Generic Macros
+ */
+#define RANDOM_CF() (((float)rand())/((float)RAND_MAX) + _Complex_I*((float)rand())/((float)RAND_MAX))
+
+/* Generic implementation for complex reciprocal */
+SRSLTE_API cf_t srslte_mat_cf_recip_gen(cf_t a);
+
+/* Generic implementation for 2x2 determinant */
+SRSLTE_API cf_t srslte_mat_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11);
+
+/* Generic implementation for 2x2 Matrix Inversion */
+SRSLTE_API void srslte_mat_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+                                       cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11);
+
+/* Generic implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_gen(cf_t y0, cf_t y1,
+                                      cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                      cf_t *x0, cf_t *x1,
+                                      float norm);
+
+/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_gen(cf_t y0, cf_t y1,
+                                        cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                        cf_t *x0, cf_t *x1,
+                                        float noise_estimate,
+                                        float norm);
+
+SRSLTE_API float srslte_mat_2x2_cn(cf_t h00,
+                                   cf_t h01,
+                                   cf_t h10,
+                                   cf_t h11);
+
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+
+/* SSE implementation for complex reciprocal */
+SRSLTE_API __m128 srslte_mat_cf_recip_sse(__m128 a);
+
+/* SSE implementation for 2x2 determinant */
+SRSLTE_API __m128 srslte_mat_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11);
+
+/* SSE implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_sse(__m128 y0, __m128 y1,
+                                      __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                      __m128 *x0, __m128 *x1,
+                                      float norm);
+
+/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1,
+                                        __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                        __m128 *x0, __m128 *x1,
+                                        float noise_estimate, float norm);
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+#include <immintrin.h>
+
+/* AVX implementation for complex reciprocal */
+SRSLTE_API __m256 srslte_mat_cf_recip_avx(__m256 a);
+
+/* AVX implementation for 2x2 determinant */
+SRSLTE_API __m256 srslte_mat_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11);
+
+/* AVX implementation for Zero Forcing (ZF) solver */
+SRSLTE_API void srslte_mat_2x2_zf_avx(__m256 y0, __m256 y1,
+                                      __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                      __m256 *x0, __m256 *x1,
+                                      float norm);
+
+/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
+SRSLTE_API void srslte_mat_2x2_mmse_avx(__m256 y0, __m256 y1,
+                                        __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                        __m256 *x0, __m256 *x1,
+                                        float noise_estimate, float norm);
+
+#endif /* LV_HAVE_AVX */
+
+#endif /* SRSLTE_MAT_H */
diff --git a/lib/include/srslte/phy/utils/simd.h b/lib/include/srslte/phy/utils/simd.h
new file mode 100644
index 000000000..420d07213
--- /dev/null
+++ b/lib/include/srslte/phy/utils/simd.h
@@ -0,0 +1,81 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef SRSLTE_SIMD_H_H
+#define SRSLTE_SIMD_H_H
+
+/*
+ * SSE Macros
+ */
+#ifdef LV_HAVE_SSE
+#define _MM_SWAP(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,3,0,1)))
+#define _MM_PERM(X) ((__m128)_mm_shuffle_ps(X, X, _MM_SHUFFLE(2,1,3,0)))
+#define _MM_MULJ_PS(X) _MM_SWAP(_MM_CONJ_PS(X))
+#define _MM_CONJ_PS(X) (_mm_xor_ps(X, _mm_set_ps(-0.0f, 0.0f, -0.0f, 0.0f)))
+#define _MM_SQMOD_PS(X) _MM_PERM(_mm_hadd_ps(_mm_mul_ps(X,X), _mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f)))
+#define _MM_PROD_PS(a, b) _mm_addsub_ps(_mm_mul_ps(a,_mm_moveldup_ps(b)),_mm_mul_ps(\
+                            _mm_shuffle_ps(a,a,0xB1),_mm_movehdup_ps(b)))
+
+#endif /* LV_HAVE_SSE */
+
+/*
+ * AVX Macros
+ */
+#ifdef LV_HAVE_AVX
+
+#define _MM256_MULJ_PS(X) _mm256_permute_ps(_MM256_CONJ_PS(X), 0b10110001)
+#define _MM256_CONJ_PS(X) (_mm256_xor_ps(X, _mm256_set_ps(-0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f, -0.0f, 0.0f)))
+
+#ifdef LV_HAVE_FMA
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+#define _MM256_PROD_PS(a, b) _mm256_fmaddsub_ps(a,_mm256_moveldup_ps(b),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#else
+#define _MM256_SQMOD_PS(A, B) _mm256_permute_ps(_mm256_hadd_ps(_mm256_add_ps(_mm256_mul_ps(A,A), _mm256_mul_ps(B,B)), \
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+#define _MM256_PROD_PS(a, b) _mm256_addsub_ps(_mm256_mul_ps(a,_mm256_moveldup_ps(b)),\
+                              _mm256_mul_ps(_mm256_shuffle_ps(a,a,0xB1),_mm256_movehdup_ps(b)))
+#endif /* LV_HAVE_FMA */
+#endif /* LV_HAVE_AVX */
+
+
+/*
+ * AVX extension with FMA Macros
+ */
+#ifdef LV_HAVE_FMA
+
+#define _MM256_SQMOD_ADD_PS(A, B, C) _mm256_permute_ps(_mm256_hadd_ps(_mm256_fmadd_ps(A, A, _mm256_fmadd_ps(B, B, C)),\
+                                           _mm256_set_ps(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)), 0b11011100)
+
+#define _MM256_PROD_ADD_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmaddsub_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+
+#define _MM256_PROD_SUB_PS(A, B, C) _mm256_fmaddsub_ps(A,_mm256_moveldup_ps(B),\
+                              _mm256_fmsubadd_ps(_mm256_shuffle_ps(A,A,0xB1),_mm256_movehdup_ps(B), C))
+#endif /* LV_HAVE_FMA */
+
+#endif //SRSLTE_SIMD_H_H
diff --git a/lib/include/srslte/phy/utils/vector.h b/lib/include/srslte/phy/utils/vector.h
index d203ff390..4a55d18b6 100644
--- a/lib/include/srslte/phy/utils/vector.h
+++ b/lib/include/srslte/phy/utils/vector.h
@@ -175,6 +175,9 @@ SRSLTE_API void srslte_vec_abs_square_cf(cf_t *x, float *abs_square, uint32_t le
 /* argument of each vector element */
 SRSLTE_API void srslte_vec_arg_cf(cf_t *x, float *arg, uint32_t len);
 
+/* Copy 256 bit aligned vector */
+SRSLTE_API void srs_vec_cf_cpy(cf_t *src, cf_t *dst, int len);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index 5adaa08c5..edb8c5ca2 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -448,12 +448,15 @@ float srslte_chest_dl_get_rsrq(srslte_chest_dl_t *q) {
   
 }
 
-float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {  
-  // Note: use only port 0 but average across antennas
-  float n = 0; 
-  for (int i=0;i<q->last_nof_antennas;i++) {
-    n += q->rsrp[i][0]; 
+float srslte_chest_dl_get_rsrp_port(srslte_chest_dl_t *q, uint32_t port) {
+  float n = 0;
+  for (int i = 0; i < q->last_nof_antennas; i++) {
+      n += q->rsrp[i][port];
   }
-  return n/q->last_nof_antennas;
+  return n / q->last_nof_antennas;
 }
 
+float srslte_chest_dl_get_rsrp(srslte_chest_dl_t *q) {
+  // Note: use only port 0 but average across antennas
+  return srslte_chest_dl_get_rsrp_port(q, 0);
+}
diff --git a/lib/src/phy/ch_estimation/test/CMakeLists.txt b/lib/src/phy/ch_estimation/test/CMakeLists.txt
index fd293c273..8e1208ef5 100644
--- a/lib/src/phy/ch_estimation/test/CMakeLists.txt
+++ b/lib/src/phy/ch_estimation/test/CMakeLists.txt
@@ -52,4 +52,3 @@ add_test(chest_test_ul_cellid1 chest_test_ul -c 2 -r 50)
 
 
 
-
diff --git a/lib/src/phy/common/phy_common.c b/lib/src/phy/common/phy_common.c
index d6fe8e6df..b502fbd75 100644
--- a/lib/src/phy/common/phy_common.c
+++ b/lib/src/phy/common/phy_common.c
@@ -124,6 +124,24 @@ bool srslte_N_id_1_isvalid(uint32_t N_id_1) {
   }
 }
 
+srslte_mod_t srslte_str2mod (char * mod_str) {
+  int i = 0;
+
+  /* Upper case */
+  while (mod_str[i] &= (~' '), mod_str[++i]);
+
+  if (!strcmp(mod_str, "QPSK")) {
+    return SRSLTE_MOD_QPSK;
+  } else if (!strcmp(mod_str, "16QAM")) {
+    return SRSLTE_MOD_16QAM;
+  } else if (!strcmp(mod_str, "64QAM")) {
+    return SRSLTE_MOD_64QAM;
+  } else {
+    return (srslte_mod_t) SRSLTE_ERROR_INVALID_INPUTS;
+  }
+};
+
+
 char *srslte_mod_string(srslte_mod_t mod) {
   switch (mod) {
   case SRSLTE_MOD_BPSK:
@@ -424,18 +442,40 @@ struct lte_band lte_bands[SRSLTE_NOF_LTE_BANDS] = {
 
 
 int srslte_str2mimotype(char *mimo_type_str, srslte_mimo_type_t *type) {
-  if (!strcmp(mimo_type_str, "single")) {
+  int i = 0;
+
+  /* Low case */
+  while (mimo_type_str[i] |= ' ', mimo_type_str[++i]);
+
+  if (!strcmp(mimo_type_str, "single") || !strcmp(mimo_type_str, "port0")) {
     *type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-  } else if (!strcmp(mimo_type_str, "diversity")) {
+  } else if (!strcmp(mimo_type_str, "diversity") || !strcmp(mimo_type_str, "txdiversity")) {
     *type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-  } else if (!strcmp(mimo_type_str, "multiplex")) {
+  } else if (!strcmp(mimo_type_str, "multiplex") || !strcmp(mimo_type_str, "spatialmux")) {
     *type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+  } else if (!strcmp(mimo_type_str, "cdd")) {
+    *type = SRSLTE_MIMO_TYPE_CDD;
   } else {
     return SRSLTE_ERROR;
   }
   return SRSLTE_SUCCESS;
 }
 
+char *srslte_mimotype2str(srslte_mimo_type_t mimo_type) {
+  switch (mimo_type) {
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      return "Single";
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      return "Diversity";
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      return "Multiplex";
+    case SRSLTE_MIMO_TYPE_CDD:
+      return "CDD";
+    default:
+      return "N/A";
+  }
+}
+
 float get_fd(struct lte_band *band, uint32_t dl_earfcn) {
   if (dl_earfcn >= band->dl_earfcn_offset) {
     return band->fd_low_mhz + 0.1*(dl_earfcn - band->dl_earfcn_offset);    
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index 0f77c21d9..db0f38f33 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -131,7 +131,7 @@ int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
-  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
   if (!plan->p) {
     return -1;
   }
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index 5648490bb..de9ef3e22 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -65,16 +65,16 @@ int srslte_enb_dl_init(srslte_enb_dl_t *q, uint32_t max_prb)
       fprintf(stderr, "Error creating PBCH object\n");
       goto clean_exit;
     }
-    if (srslte_pcfich_init(&q->pcfich)) {
+    if (srslte_pcfich_init(&q->pcfich, 0)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich)) {
+    if (srslte_phich_init(&q->phich, 0)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init(&q->pdcch, max_prb)) {
+    if (srslte_pdcch_init_enb(&q->pdcch, max_prb)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
@@ -319,9 +319,9 @@ int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, srslte_ra_ul_dci_t *grant,
   return SRSLTE_SUCCESS;
 }
 
-int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer,
+int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                             uint16_t rnti, uint32_t rv_idx, uint32_t sf_idx, 
-                            uint8_t *data) 
+                            uint8_t *data[SRSLTE_MAX_CODEWORDS])
 {  
   /* Configure pdsch_cfg parameters */
   if (srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx)) {
diff --git a/lib/src/phy/io/filesink.c b/lib/src/phy/io/filesink.c
index 60a8c386e..a61c417fd 100644
--- a/lib/src/phy/io/filesink.c
+++ b/lib/src/phy/io/filesink.c
@@ -99,3 +99,60 @@ int srslte_filesink_write(srslte_filesink_t *q, void *buffer, int nsamples) {
   return i;
 }
 
+int srslte_filesink_write_multi(srslte_filesink_t *q, void **buffer, int nsamples, int nchannels) {
+  int i, j;
+  float **fbuf = (float**) buffer;
+  _Complex float **cbuf = (_Complex float**) buffer;
+  _Complex short **sbuf = (_Complex short**) buffer;
+  int size;
+
+  switch(q->type) {
+    case SRSLTE_FLOAT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%g%c", fbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_COMPLEX_FLOAT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%g%+gi%c", __real__ cbuf[j][i], __imag__ cbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_COMPLEX_SHORT:
+      for (i=0;i<nsamples;i++) {
+        for (j=0;j<nchannels;j++) {
+          fprintf(q->f, "%hd%+hdi%c", __real__ sbuf[j][i], __imag__ sbuf[j][i], (j!=(nchannels-1))?'\t':'\n');
+        }
+      }
+      break;
+    case SRSLTE_FLOAT_BIN:
+    case SRSLTE_COMPLEX_FLOAT_BIN:
+    case SRSLTE_COMPLEX_SHORT_BIN:
+      if (q->type == SRSLTE_FLOAT_BIN) {
+        size = sizeof(float);
+      } else if (q->type == SRSLTE_COMPLEX_FLOAT_BIN) {
+        size = sizeof(_Complex float);
+      } else if (q->type == SRSLTE_COMPLEX_SHORT_BIN) {
+        size = sizeof(_Complex short);
+      }
+      if (nchannels > 1) {
+        uint32_t count = 0;
+        for (i = 0; i < nsamples; i++) {
+          for (j = 0; j < nchannels; j++) {
+            count += fwrite(&cbuf[j][i], size, 1, q->f);
+          }
+        }
+        return count;
+      } else {
+        return fwrite(buffer[0], size, nsamples, q->f);
+      }
+      break;
+    default:
+      i = -1;
+      break;
+  }
+  return i;
+}
\ No newline at end of file
diff --git a/lib/src/phy/io/filesource.c b/lib/src/phy/io/filesource.c
index 4010f8da4..048ecb584 100644
--- a/lib/src/phy/io/filesource.c
+++ b/lib/src/phy/io/filesource.c
@@ -116,3 +116,29 @@ int srslte_filesource_read(srslte_filesource_t *q, void *buffer, int nsamples) {
   return i;
 }
 
+int srslte_filesource_read_multi(srslte_filesource_t *q, void **buffer, int nsamples, int nof_channels) {
+  int i, j, count = 0;
+  _Complex float **cbuf = (_Complex float **) buffer;
+
+  switch (q->type) {
+    case SRSLTE_FLOAT:
+    case SRSLTE_COMPLEX_FLOAT:
+    case SRSLTE_COMPLEX_SHORT:
+    case SRSLTE_FLOAT_BIN:
+    case SRSLTE_COMPLEX_SHORT_BIN:
+      fprintf(stderr, "%s.%d:Read Mode not implemented\n", __FILE__, __LINE__);
+      count = SRSLTE_ERROR;
+      break;
+    case SRSLTE_COMPLEX_FLOAT_BIN:
+      for (i = 0; i < nsamples; i++) {
+        for (j = 0; j < nof_channels; j++) {
+          count += fread(&cbuf[j][i], sizeof(cf_t), (size_t) 1, q->f);
+        }
+      }
+      break;
+    default:
+      count = SRSLTE_ERROR;
+      break;
+  }
+  return count;
+}
\ No newline at end of file
diff --git a/lib/src/phy/mimo/layermap.c b/lib/src/phy/mimo/layermap.c
index 868bf2f6a..a5bfc8b92 100644
--- a/lib/src/phy/mimo/layermap.c
+++ b/lib/src/phy/mimo/layermap.c
@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <string.h>
+#include <srslte/phy/utils/vector.h>
 
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/layermap.h"
@@ -51,7 +52,12 @@ int srslte_layermap_diversity(cf_t *d, cf_t *x[SRSLTE_MAX_LAYERS], int nof_layer
 
 int srslte_layermap_multiplex(cf_t *d[SRSLTE_MAX_CODEWORDS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_cw, int nof_layers,
     int nof_symbols[SRSLTE_MAX_CODEWORDS]) {
-  if (nof_cw == 1) {
+  if (nof_cw == nof_layers) {
+    for (int i = 0; i < nof_cw; i++) {
+      srs_vec_cf_cpy(x[i], d[i], (uint32_t) nof_symbols[0]);
+    }
+    return nof_symbols[0];
+  } else if (nof_cw == 1) {
     return srslte_layermap_diversity(d[0], x, nof_layers, nof_symbols[0]);
   } else {
     int n[2];
diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index 8781dbad2..f1aab3b5d 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -25,7 +25,6 @@
  */
 
 #include <stdlib.h>
-#include <assert.h>
 #include <complex.h>
 #include <string.h>
 #include <math.h>
@@ -33,20 +32,23 @@
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/mimo/precoding.h"
 #include "srslte/phy/utils/vector.h"
+#include "srslte/phy/utils/debug.h"
 
 #ifdef LV_HAVE_SSE
-#include <xmmintrin.h>
-#include <pmmintrin.h>
+#include <immintrin.h>
+#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 int srslte_predecoding_diversity2_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_symbols);
 #endif
 
 #ifdef LV_HAVE_AVX
 #include <immintrin.h>
+#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 #endif
 
 
+static srslte_mimo_decoder_t mimo_decoder = SRSLTE_MIMO_DECODER_MMSE;
 
 /************************************************
  * 
@@ -527,12 +529,898 @@ int srslte_predecoding_type(cf_t *y_, cf_t *h_[SRSLTE_MAX_PORTS], cf_t *x[SRSLTE
     h[i][0] = h_[i];
   }
   y[0] = y_; 
-  return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, type, noise_estimate);  
+  return srslte_predecoding_type_multi(y, h, x, nof_rxant, nof_ports, nof_layers, 0, nof_symbols, type, noise_estimate);
+}
+
+
+int srslte_precoding_mimo_2x2_gen(cf_t W[2][2], cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], 
+                              int nof_symbols, float noise_estimate)
+{
+ 
+  cf_t G[2][2], Gx[2][2]; 
+  
+  for (int i=0; i<nof_symbols; i++) {
+  
+    // G=H*W
+    G[0][0] = h[0][0][i]*W[0][0]+h[0][1][i]*W[1][0];        
+    G[0][1] = h[0][0][i]*W[1][0]+h[0][1][i]*W[1][1]; 
+    G[1][0] = h[1][0][i]*W[0][0]+h[1][1][i]*W[1][0];
+    G[1][1] = h[1][0][i]*W[1][0]+h[1][1][i]*W[1][1]; 
+    
+    if (noise_estimate == 0) {
+      // MF equalizer: Gx = G'
+      Gx[0][0] = conjf(G[0][0]); 
+      Gx[0][1] = conjf(G[1][0]); 
+      Gx[1][0] = conjf(G[0][1]); 
+      Gx[1][1] = conjf(G[1][1]); 
+    } else {
+      // MMSE equalizer: Gx = (G'G+I)      
+      fprintf(stderr, "MMSE MIMO decoder not implemented\n");
+      return -1; 
+    }
+    
+    // x=G*y
+    x[0][i] = Gx[0][0]*y[0][i] + Gx[0][1]*y[1][i];
+    x[1][i] = Gx[1][0]*y[0][i] + Gx[1][1]*y[1][i];
+  }
+  
+  return SRSLTE_SUCCESS; 
+}
+
+// AVX implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_AVX
+
+int srslte_predecoding_ccd_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols) {
+  uint32_t i = 0;
+
+  __m256 mask0 = _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f);
+  __m256 mask1 = _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f);
+
+  for (i = 0; i < nof_symbols - 3; i += 4) {
+    /* Load channel */
+    __m256 h00i = _mm256_load_ps((float *) &h[0][0][i]);
+    __m256 h01i = _mm256_load_ps((float *) &h[0][1][i]);
+    __m256 h10i = _mm256_load_ps((float *) &h[1][0][i]);
+    __m256 h11i = _mm256_load_ps((float *) &h[1][1][i]);
+
+    /* Apply precoding */
+    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, mask0));
+    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, mask0));
+    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, mask1));
+    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, mask1));
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_mat_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif /* LV_HAVE_AVX */
+
+// SSE implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_SSE
+
+int srslte_predecoding_ccd_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols) {
+  uint32_t i = 0;
+
+  for (i = 0; i < nof_symbols - 1; i += 2) {
+    /* Load channel */
+    __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
+    __m128 h01i = _mm_load_ps((float *) &h[0][1][i]);
+    __m128 h10i = _mm_load_ps((float *) &h[1][0][i]);
+    __m128 h11i = _mm_load_ps((float *) &h[1][1][i]);
+
+    /* Apply precoding */
+    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_mat_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, 2.0f);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif
+
+// Generic implementation of ZF 2x2 CCD equalizer
+int srslte_predecoding_ccd_2x2_zf_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                      int nof_symbols) {
+  cf_t h00, h01, h10, h11, det;
+  for (int i = 0; i < nof_symbols; i++) {
+
+    // Even precoder
+    h00 = +h[0][0][i] + h[1][0][i];
+    h10 = +h[0][1][i] + h[1][1][i];
+    h01 = +h[0][0][i] - h[1][0][i];
+    h11 = +h[0][1][i] - h[1][1][i];
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * ((float) 2.0 / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+
+    i++;
+
+    // Odd precoder
+    h00 = h[0][0][i] - h[1][0][i];
+    h10 = h[0][1][i] - h[1][1][i];
+    h01 = h[0][0][i] + h[1][0][i];
+    h11 = h[0][1][i] + h[1][1][i];
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * ((float) 2.0 / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+
+  }
+  return SRSLTE_SUCCESS;
+}
+
+int srslte_predecoding_ccd_zf(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                              int nof_rxant, int nof_ports, int nof_layers, int nof_symbols) 
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_ccd_2x2_zf_avx(y, h, x, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+      return srslte_predecoding_ccd_2x2_zf_sse(y, h, x, nof_symbols);
+#else
+      return srslte_predecoding_ccd_2x2_zf_gen(y, h, x, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+    } else {
+      fprintf(stderr, "Error predecoding CCD: Invalid number of layers %d\n", nof_layers);
+      return -1;       
+    }          
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
+  } else {
+    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+  }
+  return SRSLTE_ERROR;
+}
+
+// AVX implementation of MMSE 2x2 CCD equalizer
+#ifdef LV_HAVE_AVX
+
+int srslte_predecoding_ccd_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS],
+                                        cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS],
+                                        uint32_t nof_symbols, float noise_estimate) {
+  uint32_t i = 0;
+
+  for (i = 0; i < nof_symbols - 3; i += 4) {
+    /* Load channel */
+    __m256 h00i = _mm256_load_ps((float *) &h[0][0][i]);
+    __m256 h01i = _mm256_load_ps((float *) &h[0][1][i]);
+    __m256 h10i = _mm256_load_ps((float *) &h[1][0][i]);
+    __m256 h11i = _mm256_load_ps((float *) &h[1][1][i]);
+
+    /* Apply precoding */
+    __m256 h00 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f)));
+    __m256 h10 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f)));
+    __m256 h01 = _mm256_add_ps(h00i, _mm256_xor_ps(h10i, _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f)));
+    __m256 h11 = _mm256_add_ps(h01i, _mm256_xor_ps(h11i, _mm256_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f)));
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_mat_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif /* LV_HAVE_AVX */
+
+// SSE implementation of ZF 2x2 CCD equalizer
+#ifdef LV_HAVE_SSE
+
+int srslte_predecoding_ccd_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS],
+                                      cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                      cf_t *x[SRSLTE_MAX_LAYERS],
+                                      uint32_t nof_symbols, float noise_estimate) {
+  uint32_t i = 0;
+
+  for (i = 0; i < nof_symbols - 1; i += 2) {
+    /* Load channel */
+    __m128 h00i = _mm_load_ps((float *) &h[0][0][i]);
+    __m128 h01i = _mm_load_ps((float *) &h[0][1][i]);
+    __m128 h10i = _mm_load_ps((float *) &h[1][0][i]);
+    __m128 h11i = _mm_load_ps((float *) &h[1][1][i]);
+
+    /* Apply precoding */
+    __m128 h00 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h10 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f)));
+    __m128 h01 = _mm_add_ps(h00i, _mm_xor_ps(h10i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+    __m128 h11 = _mm_add_ps(h01i, _mm_xor_ps(h11i, _mm_setr_ps(-0.0f, -0.0f, +0.0f, +0.0f)));
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_mat_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, 2.0f);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+  }
+
+  return nof_symbols;
+}
+#endif /* LV_HAVE_SSE */
+
+// Generic implementation of ZF 2x2 CCD equalizer
+int srslte_predecoding_ccd_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                      int nof_symbols, float noise_estimate) {
+  cf_t h00, h01, h10, h11;
+  for (int i = 0; i < nof_symbols; i++) {
+
+    // Even precoder
+    h00 = +h[0][0][i] + h[1][0][i];
+    h10 = +h[0][1][i] + h[1][1][i];
+    h01 = +h[0][0][i] - h[1][0][i];
+    h11 = +h[0][1][i] - h[1][1][i];
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
+
+    i++;
+
+    // Odd precoder
+    h00 = h[0][0][i] - h[1][0][i];
+    h10 = h[0][1][i] - h[1][1][i];
+    h01 = h[0][0][i] + h[1][0][i];
+    h11 = h[0][1][i] + h[1][1][i];
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, 2.0f);
+  }
+  return SRSLTE_SUCCESS;
+}
+
+
+int srslte_predecoding_ccd_mmse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                              int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, float noise_estimate)
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_ccd_2x2_mmse_avx(y, h, x, nof_symbols, noise_estimate);
+#else
+      #ifdef LV_HAVE_SSE
+      return srslte_predecoding_ccd_2x2_mmse_sse(y, h, x, nof_symbols, noise_estimate);
+#else
+      return srslte_predecoding_ccd_2x2_mmse_gen(y, h, x, nof_symbols, noise_estimate);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+    } else {
+      fprintf(stderr, "Error predecoding CCD: Invalid number of layers %d\n", nof_layers);
+      return -1;
+    }
+  } else if (nof_ports == 4) {
+    fprintf(stderr, "Error predecoding CCD: Only 2 ports supported\n");
+  } else {
+    fprintf(stderr, "Error predecoding CCD: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+  }
+  return SRSLTE_ERROR;
+}
+
+#ifdef LV_HAVE_AVX
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm256_add_ps(_h00, _h10);
+        h01 = _mm256_sub_ps(_h00, _h10);
+        h10 = _mm256_add_ps(_h01, _h11);
+        h11 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h01 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h10 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
+        h11 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_mat_2x2_zf_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+
+// SSE implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm_add_ps(_h00, _h10);
+        h01 = _mm_sub_ps(_h00, _h10);
+        h10 = _mm_add_ps(_h01, _h11);
+        h11 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h01 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h10 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
+        h11 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_mat_2x2_zf_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, norm);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_SSE */
+
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_zf_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i++) {
+    cf_t h00, h01, h10, h11, det;
+
+    switch(codebook_idx) {
+      case 0:
+        h00 = h[0][0][i];
+        h01 = h[1][0][i];
+        h10 = h[0][1][i];
+        h11 = h[1][1][i];
+        break;
+      case 1:
+        h00 = h[0][0][i] + h[1][0][i];
+        h01 = h[0][0][i] - h[1][0][i];
+        h10 = h[0][1][i] + h[1][1][i];
+        h11 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h00 = h[0][0][i] + _Complex_I*h[1][0][i];
+        h01 = h[0][0][i] - _Complex_I*h[1][0][i];
+        h10 = h[0][1][i] + _Complex_I*h[1][1][i];
+        h11 = h[0][1][i] - _Complex_I*h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    det = (h00 * h11 - h01 * h10);
+    det = conjf(det) * (norm / (crealf(det) * crealf(det) + cimagf(det) * cimagf(det)));
+
+    x[0][i] = (+h11 * y[0][i] - h01 * y[1][i]) * det;
+    x[1][i] = (-h10 * y[0][i] + h00 * y[1][i]) * det;
+  }
+  return SRSLTE_SUCCESS;
+}
+
+#ifdef LV_HAVE_AVX
+
+// AVX implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm256_add_ps(_h00, _h10);
+        h01 = _mm256_sub_ps(_h00, _h10);
+        h10 = _mm256_add_ps(_h01, _h11);
+        h11 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h01 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h10 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
+        h11 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 y0 = _mm256_load_ps((float *) &y[0][i]);
+    __m256 y1 = _mm256_load_ps((float *) &y[1][i]);
+
+    __m256 x0, x1;
+
+    srslte_mat_2x2_mmse_avx(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+
+    _mm256_store_ps((float *) &x[0][i], x0);
+    _mm256_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_AVX */
+
+
+#ifdef LV_HAVE_SSE
+
+// SSE implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h00, h01, h10, h11;
+    switch (codebook_idx) {
+      case 0:
+        h00 = _h00;
+        h01 = _h10;
+        h10 = _h01;
+        h11 = _h11;
+        break;
+      case 1:
+        h00 = _mm_add_ps(_h00, _h10);
+        h01 = _mm_sub_ps(_h00, _h10);
+        h10 = _mm_add_ps(_h01, _h11);
+        h11 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h00 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h01 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h10 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
+        h11 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 y0 = _mm_load_ps((float *) &y[0][i]);
+    __m128 y1 = _mm_load_ps((float *) &y[1][i]);
+
+    __m128 x0, x1;
+
+    srslte_mat_2x2_mmse_sse(y0, y1, h00, h01, h10, h11, &x0, &x1, noise_estimate, norm);
+
+    _mm_store_ps((float *) &x[0][i], x0);
+    _mm_store_ps((float *) &x[1][i], x1);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+#endif /* LV_HAVE_SSE */
+
+// Generic implementation of ZF 2x2 Spatial Multiplexity equalizer
+int srslte_predecoding_multiplex_2x2_mmse_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols,
+                                        float noise_estimate) {
+  float norm = 1.0;
+
+  switch(codebook_idx) {
+    case 0:
+      norm = (float) M_SQRT2;
+      break;
+    case 1:
+    case 2:
+      norm = 2.0f;
+      break;
+    default:
+      ERROR("Wrong codebook_idx=%d", codebook_idx);
+      return SRSLTE_ERROR;
+  }
+
+  for (int i = 0; i < nof_symbols; i++) {
+    cf_t h00, h01, h10, h11;
+
+    switch(codebook_idx) {
+      case 0:
+        h00 = h[0][0][i];
+        h01 = h[1][0][i];
+        h10 = h[0][1][i];
+        h11 = h[1][1][i];
+        break;
+      case 1:
+        h00 = h[0][0][i] + h[1][0][i];
+        h01 = h[0][0][i] - h[1][0][i];
+        h10 = h[0][1][i] + h[1][1][i];
+        h11 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h00 = h[0][0][i] + _Complex_I*h[1][0][i];
+        h01 = h[0][0][i] - _Complex_I*h[1][0][i];
+        h10 = h[0][1][i] + _Complex_I*h[1][1][i];
+        h11 = h[0][1][i] - _Complex_I*h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    srslte_mat_2x2_mmse_gen(y[0][i], y[1][i], h00, h01, h10, h11, &x[0][i], &x[1][i], noise_estimate, norm);
+  }
+  return SRSLTE_SUCCESS;
+}
+
+#ifdef LV_HAVE_AVX
+
+// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+int srslte_predecoding_multiplex_2x1_mrc_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                             cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
+    __m256 _h00 = _mm256_load_ps((float*)&(h[0][0][i]));
+    __m256 _h01 = _mm256_load_ps((float*)&(h[0][1][i]));
+    __m256 _h10 = _mm256_load_ps((float*)&(h[1][0][i]));
+    __m256 _h11 = _mm256_load_ps((float*)&(h[1][1][i]));
+
+    __m256 h0, h1;
+    switch (codebook_idx) {
+      case 0:
+        h0 = _mm256_add_ps(_h00, _h10);
+        h1 = _mm256_add_ps(_h01, _h11);
+        break;
+      case 1:
+        h0 = _mm256_sub_ps(_h00, _h10);
+        h1 = _mm256_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h0 = _mm256_add_ps(_h00, _MM256_MULJ_PS(_h10));
+        h1 = _mm256_add_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      case 3:
+        h0 = _mm256_sub_ps(_h00, _MM256_MULJ_PS(_h10));
+        h1 = _mm256_sub_ps(_h01, _MM256_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m256 h0_2 = _mm256_mul_ps(h0, h0);
+    __m256 h1_2 = _mm256_mul_ps(h1, h1);
+    __m256 hh0 = _mm256_add_ps(_mm256_movehdup_ps(h0_2), _mm256_moveldup_ps(h0_2));
+    __m256 hh1 = _mm256_add_ps(_mm256_movehdup_ps(h1_2), _mm256_moveldup_ps(h1_2));
+    __m256 hh = _mm256_add_ps(hh0, hh1);
+    __m256 hhrec = _mm256_rcp_ps(hh);
+
+    hhrec = _mm256_mul_ps(hhrec, _mm256_set1_ps((float) M_SQRT2));
+    __m256 y0 = _mm256_load_ps((float*)&y[0][i]);
+    __m256 y1 = _mm256_load_ps((float*)&y[1][i]);
+
+    __m256 x0 = _mm256_add_ps(_MM256_PROD_PS(_MM256_CONJ_PS(h0), y0), _MM256_PROD_PS(_MM256_CONJ_PS(h1), y1));
+    x0 = _mm256_mul_ps(hhrec, x0);
+
+    _mm256_store_ps((float*)&x[0][i], x0);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_AVX */
+
+
+// SSE  implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+#ifdef LV_HAVE_SSE
+
+int srslte_predecoding_multiplex_2x1_mrc_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                             cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 _h00 = _mm_load_ps((float*)&(h[0][0][i]));
+    __m128 _h01 = _mm_load_ps((float*)&(h[0][1][i]));
+    __m128 _h10 = _mm_load_ps((float*)&(h[1][0][i]));
+    __m128 _h11 = _mm_load_ps((float*)&(h[1][1][i]));
+
+    __m128 h0, h1;
+    switch (codebook_idx) {
+      case 0:
+        h0 = _mm_add_ps(_h00, _h10);
+        h1 = _mm_add_ps(_h01, _h11);
+        break;
+      case 1:
+        h0 = _mm_sub_ps(_h00, _h10);
+        h1 = _mm_sub_ps(_h01, _h11);
+        break;
+      case 2:
+        h0 = _mm_add_ps(_h00, _MM_MULJ_PS(_h10));
+        h1 = _mm_add_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      case 3:
+        h0 = _mm_sub_ps(_h00, _MM_MULJ_PS(_h10));
+        h1 = _mm_sub_ps(_h01, _MM_MULJ_PS(_h11));
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    __m128 h0_2 = _mm_mul_ps(h0, h0);
+    __m128 h1_2 = _mm_mul_ps(h1, h1);
+    __m128 hh0 = _mm_add_ps(_mm_movehdup_ps(h0_2), _mm_moveldup_ps(h0_2));
+    __m128 hh1 = _mm_add_ps(_mm_movehdup_ps(h1_2), _mm_moveldup_ps(h1_2));
+    __m128 hh = _mm_add_ps(hh0, hh1);
+    __m128 hhrec = _mm_rcp_ps(hh);
+
+    hhrec = _mm_mul_ps(hhrec, _mm_set1_ps((float) M_SQRT2));
+
+    __m128 y0 = _mm_load_ps((float*)&y[0][i]);
+    __m128 y1 = _mm_load_ps((float*)&y[1][i]);
+
+    __m128 x0 = _mm_add_ps(_MM_PROD_PS(_MM_CONJ_PS(h0), y0), _MM_PROD_PS(_MM_CONJ_PS(h1), y1));
+    x0 = _mm_mul_ps(hhrec, x0);
+
+    _mm_store_ps((float*)&x[0][i], x0);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+#endif /* LV_HAVE_SSE */
+
+// Generic implementation of MRC 2x1 (two antennas into one layer) Spatial Multiplexing equalizer
+int srslte_predecoding_multiplex_2x1_mrc_gen(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                        cf_t *x[SRSLTE_MAX_LAYERS], int codebook_idx, int nof_symbols) {
+  for (int i = 0; i < nof_symbols; i += 1) {
+    cf_t h0, h1;
+    float hh;
+
+    switch(codebook_idx) {
+      case 0:
+        h0 = h[0][0][i] + h[1][0][i];
+        h1 = h[0][1][i] + h[1][1][i];
+        break;
+      case 1:
+        h0 = h[0][0][i] - h[1][0][i];
+        h1 = h[0][1][i] - h[1][1][i];
+        break;
+      case 2:
+        h0 = h[0][0][i] + _Complex_I * h[1][0][i];
+        h1 = h[0][1][i] + _Complex_I * h[1][1][i];
+        break;
+      case 3:
+        h0 = h[0][0][i] - _Complex_I * h[1][0][i];
+        h1 = h[0][1][i] - _Complex_I * h[1][1][i];
+        break;
+      default:
+        fprintf(stderr, "Wrong codebook_idx=%d\n", codebook_idx);
+        return SRSLTE_ERROR;
+    }
+
+    hh = (float) M_SQRT2/(crealf(h0)*crealf(h0) + cimagf(h0)*cimagf(h0) + crealf(h1)*crealf(h1) + cimagf(h1)*cimagf(h1));
+
+    x[0][i] = (conjf(h0) * y[0][i] + conjf(h1) * y[1][i]) * hh;
+  }
+  return SRSLTE_SUCCESS;
+}
+
+int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
+                                 int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols,
+                                 float noise_estimate)
+{
+  if (nof_ports == 2 && nof_rxant == 2) {
+    if (nof_layers == 2) {
+      switch (mimo_decoder) {
+        case SRSLTE_MIMO_DECODER_ZF:
+#ifdef LV_HAVE_AVX
+          return srslte_predecoding_multiplex_2x2_zf_avx(y, h, x, codebook_idx, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+          return srslte_predecoding_multiplex_2x2_zf_sse(y, h, x, codebook_idx, nof_symbols);
+#else
+          return srslte_predecoding_multiplex_2x2_zf_gen(y, h, x, codebook_idx, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+          break;
+        case SRSLTE_MIMO_DECODER_MMSE:
+#ifdef LV_HAVE_AVX
+          return srslte_predecoding_multiplex_2x2_mmse_avx(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#else
+#ifdef LV_HAVE_SSE
+          return srslte_predecoding_multiplex_2x2_mmse_sse(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#else
+          return srslte_predecoding_multiplex_2x2_mmse_gen(y, h, x, codebook_idx, nof_symbols, noise_estimate);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+          break;
+      }
+    } else {
+#ifdef LV_HAVE_AVX
+      return srslte_predecoding_multiplex_2x1_mrc_avx(y, h, x, codebook_idx, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+      return srslte_predecoding_multiplex_2x1_mrc_sse(y, h, x, codebook_idx, nof_symbols);
+#else
+      return srslte_predecoding_multiplex_2x1_mrc_gen(y, h, x, codebook_idx, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+    }
+  } else if (nof_ports == 4) {
+    ERROR("Error predecoding multiplex: not implemented for %d Tx ports", nof_ports);
+  } else {
+    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+  }
+  return SRSLTE_ERROR;
+}
+
+void srslte_predecoding_set_mimo_decoder (srslte_mimo_decoder_t _mimo_decoder) {
+  mimo_decoder = _mimo_decoder;
 }
 
 /* 36.211 v10.3.0 Section 6.3.4 */
-int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS],
-    int nof_rxant, int nof_ports, int nof_layers, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
+int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                  cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_ports, int nof_layers,
+                                  int codebook_idx, int nof_symbols, srslte_mimo_type_t type, float noise_estimate) {
 
   if (nof_ports > SRSLTE_MAX_PORTS) {
     fprintf(stderr, "Maximum number of ports is %d (nof_ports=%d)\n", SRSLTE_MAX_PORTS,
@@ -547,7 +1435,20 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
 
   switch (type) {
   case SRSLTE_MIMO_TYPE_CDD:
-    fprintf(stderr, "CCD not supported\n");
+    if (nof_layers >= 2 && nof_layers <= 4) {
+      switch (mimo_decoder) {
+        case SRSLTE_MIMO_DECODER_ZF:
+          return srslte_predecoding_ccd_zf(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols);
+          break;
+        case SRSLTE_MIMO_DECODER_MMSE:
+          return srslte_predecoding_ccd_mmse(y, h, x, nof_rxant, nof_ports, nof_layers, nof_symbols, noise_estimate);
+          break;
+      }
+    } else {
+      fprintf(stderr,
+          "Invalid number of layers %d\n", nof_layers);
+      return -1;
+    }
     return -1; 
   case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
     if (nof_ports == 1 && nof_layers == 1) {
@@ -568,10 +1469,12 @@ int srslte_predecoding_type_multi(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_
     }
     break;
   case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-    fprintf(stderr, "Spatial multiplexing not supported\n");
-    return -1;
+    return srslte_predecoding_multiplex(y, h, x, nof_rxant, nof_ports, nof_layers, codebook_idx, nof_symbols,
+                                        noise_estimate);
+    default:
+      return SRSLTE_ERROR;
   }
-  return 0;
+  return SRSLTE_ERROR;
 }
 
 
@@ -634,22 +1537,85 @@ int srslte_precoding_diversity(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PO
   }
 }
 
-int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports, int nof_symbols) 
+#ifdef LV_HAVE_AVX
+
+int srslte_precoding_cdd_2x2_avx(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
+{
+  __m256 norm_avx = _mm256_set1_ps(0.5f);
+  for (int i = 0; i < nof_symbols - 3; i += 4) {
+    __m256 x0 = _mm256_load_ps((float*) &x[0][i]);
+    __m256 x1 = _mm256_load_ps((float*) &x[1][i]);
+
+    __m256 y0 = _mm256_mul_ps(norm_avx, _mm256_add_ps(x0, x1));
+
+    x0 = _mm256_xor_ps(x0, _mm256_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f));
+    x1 = _mm256_xor_ps(x1, _mm256_set_ps(+0.0f, +0.0f, -0.0f, -0.0f, +0.0f, +0.0f, -0.0f, -0.0f));
+
+    __m256 y1 = _mm256_mul_ps(norm_avx, _mm256_add_ps(x0, x1));
+
+    _mm256_store_ps((float*)&y[0][i], y0);
+    _mm256_store_ps((float*)&y[1][i], y1);
+  }
+
+  return 2*nof_symbols;
+}
+
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+
+int srslte_precoding_cdd_2x2_sse(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
+{
+  __m128 norm_sse = _mm_set1_ps(0.5f);
+  for (int i = 0; i < nof_symbols - 1; i += 2) {
+    __m128 x0 = _mm_load_ps((float*) &x[0][i]);
+    __m128 x1 = _mm_load_ps((float*) &x[1][i]);
+
+    __m128 y0 = _mm_mul_ps(norm_sse, _mm_add_ps(x0, x1));
+
+    x0 = _mm_xor_ps(x0, _mm_setr_ps(+0.0f, +0.0f, -0.0f, -0.0f));
+    x1 = _mm_xor_ps(x1, _mm_set_ps(+0.0f, +0.0f, -0.0f, -0.0f));
+
+    __m128 y1 = _mm_mul_ps(norm_sse, _mm_add_ps(x0, x1));
+
+    _mm_store_ps((float*)&y[0][i], y0);
+    _mm_store_ps((float*)&y[1][i], y1);
+  }
+
+  return 2 * nof_symbols;
+}
+
+#endif /* LV_HAVE_SSE */
+
+
+int srslte_precoding_cdd_2x2_gen(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_symbols)
+{
+  for (int i = 0; i < nof_symbols; i++) {
+    y[0][i] =  (x[0][i]+x[1][i])/2.0f;
+    y[1][i] =  (x[0][i]-x[1][i])/2.0f;
+    i++;
+    y[0][i] =  (x[0][i]+x[1][i])/2.0f;
+    y[1][i] = (-x[0][i]+x[1][i])/2.0f;
+  }
+  return 2 * nof_symbols;
+}
+
+int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports, int nof_symbols)
 {
-  int i;
   if (nof_ports == 2) {
     if (nof_layers != 2) {
       fprintf(stderr, "Invalid number of layers %d for 2 ports\n", nof_layers);
-      return -1; 
-    }
-    for (i = 0; i < nof_symbols; i++) {
-      y[0][i] =  (x[0][i]+x[1][i])/2;
-      y[1][i] =  (x[0][i]-x[1][i])/2;
-      i++;
-      y[0][i] =  (x[0][i]+x[1][i])/2;
-      y[1][i] = (-x[0][i]+x[1][i])/2;
+      return -1;
     }
-    return 2 * i;
+#ifdef LV_HAVE_AVX
+    return srslte_precoding_cdd_2x2_avx(x, y, nof_symbols);
+#else
+#ifdef LV_HAVE_SSE
+    return srslte_precoding_cdd_2x2_sse(x, y, nof_symbols);
+#else
+    return srslte_precoding_cdd_2x2_gen(x, y, nof_symbols);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
   } else if (nof_ports == 4) {
     fprintf(stderr, "Not implemented\n");
     return -1;
@@ -659,9 +1625,122 @@ int srslte_precoding_cdd(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS],
   }
 }
 
+int srslte_precoding_multiplex(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers, int nof_ports,
+                               int codebook_idx, uint32_t nof_symbols)
+{
+  int i = 0;
+  if (nof_ports == 2) {
+    if (nof_layers == 1) {
+      switch(codebook_idx) {
+        case 0:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 1:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[0], -1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 2:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_ccc(x[0], _Complex_I/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 3:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_ccc(x[0], -_Complex_I/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        default:
+          fprintf(stderr, "Invalid multiplex combination: codebook_idx=%d, nof_layers=%d, nof_ports=%d\n",
+                  codebook_idx, nof_layers, nof_ports);
+          return SRSLTE_ERROR;
+      }
+    } else if (nof_layers == 2) {
+      switch(codebook_idx) {
+        case 0:
+          srslte_vec_sc_prod_cfc(x[0], 1.0f/sqrtf(2.0f), y[0], nof_symbols);
+          srslte_vec_sc_prod_cfc(x[1], 1.0f/sqrtf(2.0f), y[1], nof_symbols);
+          break;
+        case 1:
+#ifdef LV_HAVE_AVX
+          for (; i < nof_symbols - 3; i += 4) {
+            __m256 x0 = _mm256_load_ps((float*)&x[0][i]);
+            __m256 x1 = _mm256_load_ps((float*)&x[1][i]);
+
+            __m256 y0 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_add_ps(x0, x1));
+            __m256 y1 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_sub_ps(x0, x1));
+
+            _mm256_store_ps((float*)&y[0][i], y0);
+            _mm256_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+          for (; i < nof_symbols - 1; i += 2) {
+            __m128 x0 = _mm_load_ps((float*)&x[0][i]);
+            __m128 x1 = _mm_load_ps((float*)&x[1][i]);
+
+            __m128 y0 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_add_ps(x0, x1));
+            __m128 y1 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_sub_ps(x0, x1));
+
+            _mm_store_ps((float*)&y[0][i], y0);
+            _mm_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_SSE */
+
+          for (; i < nof_symbols; i++) {
+            y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
+            y[1][i] = 0.5f*x[0][i] - 0.5f*x[1][i];
+          }
+          break;
+        case 2:
+#ifdef LV_HAVE_AVX
+          for (; i < nof_symbols - 3; i += 4) {
+            __m256 x0 = _mm256_load_ps((float*)&x[0][i]);
+            __m256 x1 = _mm256_load_ps((float*)&x[1][i]);
+
+            __m256 y0 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _mm256_add_ps(x0, x1));
+            __m256 y1 = _mm256_mul_ps(_mm256_set1_ps(0.5f), _MM256_MULJ_PS(_mm256_sub_ps(x0, x1)));
+
+            _mm256_store_ps((float*)&y[0][i], y0);
+            _mm256_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_AVX */
+
+#ifdef LV_HAVE_SSE
+          for (; i < nof_symbols - 1; i += 2) {
+            __m128 x0 = _mm_load_ps((float*)&x[0][i]);
+            __m128 x1 = _mm_load_ps((float*)&x[1][i]);
+
+            __m128 y0 = _mm_mul_ps(_mm_set1_ps(0.5f), _mm_add_ps(x0, x1));
+            __m128 y1 = _mm_mul_ps(_mm_set1_ps(0.5f), _MM_MULJ_PS(_mm_sub_ps(x0, x1)));
+
+            _mm_store_ps((float*)&y[0][i], y0);
+            _mm_store_ps((float*)&y[1][i], y1);
+          }
+#endif /* LV_HAVE_SSE */
+
+          for (; i < nof_symbols; i++) {
+            y[0][i] = 0.5f*x[0][i] + 0.5f*x[1][i];
+            y[1][i] = 0.5f*_Complex_I*x[0][i] - 0.5f*_Complex_I*x[1][i];
+          }
+          break;
+        case 3:
+        default:
+          fprintf(stderr, "Invalid multiplex combination: codebook_idx=%d, nof_layers=%d, nof_ports=%d\n",
+                  codebook_idx, nof_layers, nof_ports);
+          return SRSLTE_ERROR;
+      }
+    } else {
+      ERROR("Not implemented");
+    }
+  } else {
+    ERROR("Not implemented");
+  }
+  return SRSLTE_SUCCESS;
+}
+
 /* 36.211 v10.3.0 Section 6.3.4 */
 int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS], int nof_layers,
-    int nof_ports, int nof_symbols, srslte_mimo_type_t type) {
+    int nof_ports, int codebook_idx, int nof_symbols, srslte_mimo_type_t type) {
 
   if (nof_ports > SRSLTE_MAX_PORTS) {
     fprintf(stderr, "Maximum number of ports is %d (nof_ports=%d)\n", SRSLTE_MAX_PORTS,
@@ -675,29 +1754,800 @@ int srslte_precoding_type(cf_t *x[SRSLTE_MAX_LAYERS], cf_t *y[SRSLTE_MAX_PORTS],
   }
 
   switch (type) {
-  case SRSLTE_MIMO_TYPE_CDD:
-    return srslte_precoding_cdd(x, y, nof_layers, nof_ports, nof_symbols); 
-  case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-    if (nof_ports == 1 && nof_layers == 1) {
-      return srslte_precoding_single(x[0], y[0], nof_symbols);
-    } else {
-      fprintf(stderr,
-          "Number of ports and layers must be 1 for transmission on single antenna ports\n");
-      return -1;
+    case SRSLTE_MIMO_TYPE_CDD:
+      return srslte_precoding_cdd(x, y, nof_layers, nof_ports, nof_symbols);
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      if (nof_ports == 1 && nof_layers == 1) {
+        return srslte_precoding_single(x[0], y[0], nof_symbols);
+      } else {
+        fprintf(stderr,
+                "Number of ports and layers must be 1 for transmission on single antenna ports\n");
+        return -1;
+      }
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      if (nof_ports == nof_layers) {
+        return srslte_precoding_diversity(x, y, nof_ports, nof_symbols);
+      } else {
+        fprintf(stderr,
+                "Error number of layers must equal number of ports in transmit diversity\n");
+        return -1;
+      }
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      return srslte_precoding_multiplex(x, y, nof_layers, nof_ports, codebook_idx, nof_symbols);
+    default:
+      return SRSLTE_ERROR;
+  }
+  return SRSLTE_ERROR;
+}
+
+#define PMI_SEL_PRECISION 24
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+#define SQRT1_2 ((float)M_SQRT1_2)
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += PMI_SEL_PRECISION) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a0, a1;
+      switch (i) {
+        case 0:
+          a0 = conjf(h00) + conjf(h01);
+          a1 = conjf(h10) + conjf(h11);
+          break;
+        case 1:
+          a0 = conjf(h00) - conjf(h01);
+          a1 = conjf(h10) - conjf(h11);
+          break;
+        case 2:
+          a0 = conjf(h00) - _Complex_I * conjf(h01);
+          a1 = conjf(h10) - _Complex_I * conjf(h11);
+          break;
+        case 3:
+          a0 = conjf(h00) + _Complex_I * conjf(h01);
+          a1 = conjf(h10) + _Complex_I * conjf(h11);
+          break;
+      }
+      a0 *= SQRT1_2;
+      a1 *= SQRT1_2;
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b0 = a0 * h00 + a1 * h10;
+      cf_t b1 = a0 * h01 + a1 * h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c;
+      switch (i) {
+        case 0:
+          c = b0 + b1;
+          break;
+        case 1:
+          c = b0 - b1;
+          break;
+        case 2:
+          c = b0 + _Complex_I * b1;
+          break;
+        case 3:
+          c = b0 - _Complex_I * b1;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c *= SQRT1_2;
+
+      /* Add for averaging */
+      sinr_list[i] += crealf(c);
+
+      count++;
     }
-    break;
-  case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-    if (nof_ports == nof_layers) {
-      return srslte_precoding_diversity(x, y, nof_ports, nof_symbols);
-    } else {
-      fprintf(stderr,
-          "Error number of layers must equal number of ports in transmit diversity\n");
-      return -1;
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
     }
-  case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-    fprintf(stderr, "Spatial multiplexing not supported\n");
-    return -1;
   }
-  return 0;
+
+  return i;
+}
+
+#ifdef LV_HAVE_SSE
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  float max_sinr = 0.0;
+  uint32_t i, count;
+  __m128 sse_norm = _mm_set1_ps(0.5f);
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 2 + 1; j += PMI_SEL_PRECISION * 2) {
+      /* 0. Load channel matrix */
+      __m128 h00 = _mm_set_ps(crealf(h[0][0][j]),
+                              cimagf(h[0][0][j]),
+                              crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][0][j + PMI_SEL_PRECISION]));
+      __m128 h01 = _mm_set_ps(crealf(h[1][0][j]),
+                              cimagf(h[1][0][j]),
+                              crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][0][j + PMI_SEL_PRECISION]));
+      __m128 h10 = _mm_set_ps(crealf(h[0][1][j]),
+                              cimagf(h[0][1][j]),
+                              crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[0][1][j + PMI_SEL_PRECISION]));
+      __m128 h11 = _mm_set_ps(crealf(h[1][1][j]),
+                              cimagf(h[1][1][j]),
+                              crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                              cimagf(h[1][1][j + PMI_SEL_PRECISION]));
+
+      /* 1. B = W'* H' */
+      __m128 a0, a1;
+      switch (i) {
+        case 0:
+          a0 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a1 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 1:
+          a0 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a1 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 2:
+          a0 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a1 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+        case 3:
+          a0 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a1 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      __m128 b0 = _mm_add_ps(_MM_PROD_PS(a0, h00), _MM_PROD_PS(a1, h10));
+      __m128 b1 = _mm_add_ps(_MM_PROD_PS(a0, h01), _MM_PROD_PS(a1, h11));
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m128 c;
+      switch (i) {
+        case 0:
+          c = _mm_add_ps(b0, b1);
+          break;
+        case 1:
+          c = _mm_sub_ps(b0, b1);
+          break;
+        case 2:
+          c = _mm_sub_ps(b0, _MM_MULJ_PS(b1));
+          break;
+        case 3:
+          c = _mm_add_ps(b0, _MM_MULJ_PS(b1));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c = _mm_mul_ps(c, sse_norm);
+
+      /* Add for averaging */
+      __attribute__((aligned(128))) float gamma[4];
+      _mm_store_ps(gamma, c);
+      sinr_list[i] += gamma[0] + gamma[2];
+
+      count += 2;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+/* PMI Select for 1 layer */
+int srslte_precoding_pmi_select_1l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  float max_sinr = 0.0;
+  uint32_t i, count;
+  __m256 avx_norm = _mm256_set1_ps(0.5f);
+
+  for (i = 0; i < 4; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
+      /* 0. Load channel matrix */
+      __m256 h00 = _mm256_setr_ps(crealf(h[0][0][j]),
+                                 cimagf(h[0][0][j]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h01 = _mm256_setr_ps(crealf(h[1][0][j]),
+                                 cimagf(h[1][0][j]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h10 = _mm256_setr_ps(crealf(h[0][1][j]),
+                                 cimagf(h[0][1][j]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
+      __m256 h11 = _mm256_setr_ps(crealf(h[1][1][j]),
+                                 cimagf(h[1][1][j]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 3]));
+
+      /* 1. B = W'* H' */
+      __m256 a0, a1;
+      switch (i) {
+        case 0:
+          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 1:
+          a0 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 2:
+          a0 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a1 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+        default:
+          a0 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a1 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+#ifdef  LV_HAVE_FMA
+      __m256 b0 = _MM256_PROD_ADD_PS(a0, h00, _MM256_PROD_PS(a1, h10));
+      __m256 b1 = _MM256_PROD_ADD_PS(a0, h01, _MM256_PROD_PS(a1, h11));
+#else
+      __m256 b0 = _mm256_add_ps(_MM256_PROD_PS(a0, h00), _MM256_PROD_PS(a1, h10));
+      __m256 b1 = _mm256_add_ps(_MM256_PROD_PS(a0, h01), _MM256_PROD_PS(a1, h11));
+#endif /* LV_HAVE_FMA */
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m256 c;
+      switch (i) {
+        case 0:
+          c = _mm256_add_ps(b0, b1);
+          break;
+        case 1:
+          c = _mm256_sub_ps(b0, b1);
+          break;
+        case 2:
+          c = _mm256_add_ps(b0, _MM256_MULJ_PS(b1));
+          break;
+        case 3:
+          c = _mm256_sub_ps(b0, _MM256_MULJ_PS(b1));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c = _mm256_mul_ps(c, avx_norm);
+
+      /* Add for averaging */
+      __attribute__((aligned(256))) float gamma[8];
+      _mm256_store_ps(gamma, c);
+      sinr_list[i] += gamma[0] + gamma[2] + gamma[4] + gamma[6];
+
+      count += 4;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= noise_estimate * count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_AVX */
+
+int srslte_precoding_pmi_select_1l(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                   float noise_estimate, uint32_t *pmi,
+                                   float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+  int ret;
+#ifdef LV_HAVE_AVX
+  ret = srslte_precoding_pmi_select_1l_avx(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  #ifdef LV_HAVE_SSE
+  ret = srslte_precoding_pmi_select_1l_sse(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  ret = srslte_precoding_pmi_select_1l_gen(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#endif
+#endif
+  INFO("Precoder PMI Select for 1 layer SINR=[%.1fdB; %.1fdB; %.1fdB; %.1fdB] PMI=%d\n", 10 * log10(sinr_list[0]),
+       10 * log10(sinr_list[1]), 10 * log10(sinr_list[2]), 10 * log10(sinr_list[3]), *pmi);
+
+  return ret;
+}
+
+int srslte_precoding_pmi_select_2l_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols; j += PMI_SEL_PRECISION) {
+      /* 0. Load channel matrix */
+      cf_t h00 = h[0][0][j];
+      cf_t h01 = h[1][0][j];
+      cf_t h10 = h[0][1][j];
+      cf_t h11 = h[1][1][j];
+
+      /* 1. B = W'* H' */
+      cf_t a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = conjf(h00) + conjf(h01);
+          a01 = conjf(h10) + conjf(h11);
+          a10 = conjf(h00) - conjf(h01);
+          a11 = conjf(h10) - conjf(h11);
+          break;
+        case 1:
+          a00 = conjf(h00) - _Complex_I * conjf(h01);
+          a01 = conjf(h10) - _Complex_I * conjf(h11);
+          a10 = conjf(h00) + _Complex_I * conjf(h01);
+          a11 = conjf(h10) + _Complex_I * conjf(h11);
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      cf_t b00 = a00 * h00 + a01 * h10;
+      cf_t b01 = a00 * h01 + a01 * h11;
+      cf_t b10 = a10 * h00 + a11 * h10;
+      cf_t b11 = a10 * h01 + a11 * h11;
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      cf_t c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = b00 + b01;
+          c01 = b00 - b01;
+          c10 = b10 + b11;
+          c11 = b10 - b11;
+          break;
+        case 1:
+          c00 = b00 + _Complex_I * b01;
+          c01 = b00 - _Complex_I * b01;
+          c10 = b10 + _Complex_I * b11;
+          c11 = b10 - _Complex_I * b11;
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 *= 0.25;
+      c01 *= 0.25;
+      c10 *= 0.25;
+      c11 *= 0.25;
+
+      /* 4. C += noise * I */
+      c00 += noise_estimate;
+      c11 += noise_estimate;
+
+      /* 5. detC */
+      cf_t detC = c00 * c11 - c01 * c10;
+      cf_t inv_detC = conjf(detC) / (crealf(detC) * crealf(detC) + cimagf(detC) * cimagf(detC));
+
+      cf_t den0 = noise_estimate * c00 * inv_detC;
+      cf_t den1 = noise_estimate * c11 * inv_detC;
+
+      float gamma0 = crealf((conjf(den0) / (crealf(den0) * crealf(den0) + cimagf(den0) * cimagf(den0))) - 1);
+      float gamma1 = crealf((conjf(den1) / (crealf(den1) * crealf(den1) + cimagf(den1) * cimagf(den1))) - 1);
+
+      /* Add for averaging */
+      sinr_list[i] += (gamma0 + gamma1);
+
+      count++;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#ifdef LV_HAVE_SSE
+
+int srslte_precoding_pmi_select_2l_sse(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  __m128 sse_noise_estimate = _mm_setr_ps(noise_estimate, 0.0f, noise_estimate, 0.0f);
+  __m128 sse_norm = _mm_set1_ps(0.25f);
+  __m128 sse_ones = _mm_set1_ps(1.0f);
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 2 + 1; j += PMI_SEL_PRECISION * 2) {
+      /* 0. Load channel matrix */
+      __m128 h00 = _mm_setr_ps(crealf(h[0][0][j]),
+                               cimagf(h[0][0][j]),
+                               crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                               cimagf(h[0][0][j + PMI_SEL_PRECISION]));
+      __m128 h01 = _mm_setr_ps(crealf(h[1][0][j]),
+                               cimagf(h[1][0][j]),
+                               crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                               cimagf(h[1][0][j + PMI_SEL_PRECISION]));
+      __m128 h10 = _mm_setr_ps(crealf(h[0][1][j]),
+                               cimagf(h[0][1][j]),
+                               crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                               cimagf(h[0][1][j + PMI_SEL_PRECISION]));
+      __m128 h11 = _mm_setr_ps(crealf(h[1][1][j]),
+                               cimagf(h[1][1][j]),
+                               crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                               cimagf(h[1][1][j + PMI_SEL_PRECISION]));
+
+      /* 1. B = W'* H' */
+      __m128 a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a01 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          a10 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_CONJ_PS(h01));
+          a11 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_CONJ_PS(h11));
+          break;
+        case 1:
+          a00 = _mm_sub_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a01 = _mm_sub_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          a10 = _mm_add_ps(_MM_CONJ_PS(h00), _MM_MULJ_PS(_MM_CONJ_PS(h01)));
+          a11 = _mm_add_ps(_MM_CONJ_PS(h10), _MM_MULJ_PS(_MM_CONJ_PS(h11)));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+      __m128 b00 = _mm_add_ps(_MM_PROD_PS(a00, h00), _MM_PROD_PS(a01, h10));
+      __m128 b01 = _mm_add_ps(_MM_PROD_PS(a00, h01), _MM_PROD_PS(a01, h11));
+      __m128 b10 = _mm_add_ps(_MM_PROD_PS(a10, h00), _MM_PROD_PS(a11, h10));
+      __m128 b11 = _mm_add_ps(_MM_PROD_PS(a10, h01), _MM_PROD_PS(a11, h11));
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m128 c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = _mm_add_ps(b00, b01);
+          c01 = _mm_sub_ps(b00, b01);
+          c10 = _mm_add_ps(b10, b11);
+          c11 = _mm_sub_ps(b10, b11);
+          break;
+        case 1:
+          c00 = _mm_add_ps(b00, _MM_MULJ_PS(b01));
+          c01 = _mm_sub_ps(b00, _MM_MULJ_PS(b01));
+          c10 = _mm_add_ps(b10, _MM_MULJ_PS(b11));
+          c11 = _mm_sub_ps(b10, _MM_MULJ_PS(b11));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 = _mm_mul_ps(c00, sse_norm);
+      c01 = _mm_mul_ps(c01, sse_norm);
+      c10 = _mm_mul_ps(c10, sse_norm);
+      c11 = _mm_mul_ps(c11, sse_norm);
+
+      /* 4. C += noise * I */
+      c00 = _mm_add_ps(c00, sse_noise_estimate);
+      c11 = _mm_add_ps(c11, sse_noise_estimate);
+
+      /* 5. detC */
+      __m128 detC = srslte_mat_2x2_det_sse(c00, c01, c10, c11);
+      __m128 inv_detC = srslte_mat_cf_recip_sse(detC);
+      inv_detC = _mm_mul_ps(sse_noise_estimate, inv_detC);
+
+      __m128 den0 = _MM_PROD_PS(c00, inv_detC);
+      __m128 den1 = _MM_PROD_PS(c11, inv_detC);
+
+      __m128 gamma0 = _mm_sub_ps(_mm_rcp_ps(den0), sse_ones);
+      __m128 gamma1 = _mm_sub_ps(_mm_rcp_ps(den1), sse_ones);
+
+      /* Add for averaging */
+      __m128 sinr_sse = _mm_add_ps(gamma0, gamma1);
+      __attribute__((aligned(128))) float sinr[4];
+      _mm_store_ps(sinr, sinr_sse);
+
+      sinr_list[i] += sinr[0] + sinr[2];
+
+      count += 2;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+int srslte_precoding_pmi_select_2l_avx(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                       float noise_estimate, uint32_t *pmi,
+                                       float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  float max_sinr = 0.0;
+  uint32_t i, count;
+
+  __m256 avx_noise_estimate = _mm256_setr_ps(noise_estimate, 0.0f, noise_estimate, 0.0f,
+                                        noise_estimate, 0.0f, noise_estimate, 0.0f);
+  __m256 avx_norm = _mm256_set1_ps(0.25f);
+  __m256 avx_ones = _mm256_set1_ps(1.0f);
+
+  for (i = 0; i < 2; i++) {
+    sinr_list[i] = 0;
+    count = 0;
+
+    for (uint32_t j = 0; j < nof_symbols - PMI_SEL_PRECISION * 4 + 1; j += PMI_SEL_PRECISION * 4) {
+      /* 0. Load channel matrix */
+      __m256 h00 = _mm256_setr_ps(crealf(h[0][0][j]),
+                                 cimagf(h[0][0][j]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h01 = _mm256_setr_ps(crealf(h[1][0][j]),
+                                 cimagf(h[1][0][j]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][0][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][0][j + PMI_SEL_PRECISION * 3]));
+      __m256 h10 = _mm256_setr_ps(crealf(h[0][1][j]),
+                                 cimagf(h[0][1][j]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[0][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[0][1][j + PMI_SEL_PRECISION * 3]));
+      __m256 h11 = _mm256_setr_ps(crealf(h[1][1][j]),
+                                 cimagf(h[1][1][j]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 2]),
+                                 crealf(h[1][1][j + PMI_SEL_PRECISION * 3]),
+                                 cimagf(h[1][1][j + PMI_SEL_PRECISION * 3]));
+
+      /* 1. B = W'* H' */
+      __m256 a00, a01, a10, a11;
+      switch (i) {
+        case 0:
+          a00 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a01 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          a10 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_CONJ_PS(h01));
+          a11 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_CONJ_PS(h11));
+          break;
+        case 1:
+          a00 = _mm256_sub_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a01 = _mm256_sub_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          a10 = _mm256_add_ps(_MM256_CONJ_PS(h00), _MM256_MULJ_PS(_MM256_CONJ_PS(h01)));
+          a11 = _mm256_add_ps(_MM256_CONJ_PS(h10), _MM256_MULJ_PS(_MM256_CONJ_PS(h11)));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+
+      /* 2. B = W' * H' * H = A * H */
+#ifdef LV_HAVE_FMA
+      __m256 b00 = _MM256_PROD_ADD_PS(a00, h00, _MM256_PROD_PS(a01, h10));
+      __m256 b01 = _MM256_PROD_ADD_PS(a00, h01, _MM256_PROD_PS(a01, h11));
+      __m256 b10 = _MM256_PROD_ADD_PS(a10, h00, _MM256_PROD_PS(a11, h10));
+      __m256 b11 = _MM256_PROD_ADD_PS(a10, h01, _MM256_PROD_PS(a11, h11));
+#else
+      __m256 b00 = _mm256_add_ps(_MM256_PROD_PS(a00, h00), _MM256_PROD_PS(a01, h10));
+      __m256 b01 = _mm256_add_ps(_MM256_PROD_PS(a00, h01), _MM256_PROD_PS(a01, h11));
+      __m256 b10 = _mm256_add_ps(_MM256_PROD_PS(a10, h00), _MM256_PROD_PS(a11, h10));
+      __m256 b11 = _mm256_add_ps(_MM256_PROD_PS(a10, h01), _MM256_PROD_PS(a11, h11));
+#endif /* LV_HAVE_FMA */
+
+      /* 3. C = W' * H' * H * W' = B * W */
+      __m256 c00, c01, c10, c11;
+      switch (i) {
+        case 0:
+          c00 = _mm256_add_ps(b00, b01);
+          c01 = _mm256_sub_ps(b00, b01);
+          c10 = _mm256_add_ps(b10, b11);
+          c11 = _mm256_sub_ps(b10, b11);
+          break;
+        case 1:
+          c00 = _mm256_add_ps(b00, _MM256_MULJ_PS(b01));
+          c01 = _mm256_sub_ps(b00, _MM256_MULJ_PS(b01));
+          c10 = _mm256_add_ps(b10, _MM256_MULJ_PS(b11));
+          c11 = _mm256_sub_ps(b10, _MM256_MULJ_PS(b11));
+          break;
+        default:
+          return SRSLTE_ERROR;
+      }
+      c00 = _mm256_mul_ps(c00, avx_norm);
+      c01 = _mm256_mul_ps(c01, avx_norm);
+      c10 = _mm256_mul_ps(c10, avx_norm);
+      c11 = _mm256_mul_ps(c11, avx_norm);
+
+      /* 4. C += noise * I */
+      c00 = _mm256_add_ps(c00, avx_noise_estimate);
+      c11 = _mm256_add_ps(c11, avx_noise_estimate);
+
+      /* 5. detC */
+      __m256 detC = srslte_mat_2x2_det_avx(c00, c01, c10, c11);
+      __m256 inv_detC = srslte_mat_cf_recip_avx(detC);
+      inv_detC = _mm256_mul_ps(avx_noise_estimate, inv_detC);
+
+      __m256 den0 = _MM256_PROD_PS(c00, inv_detC);
+      __m256 den1 = _MM256_PROD_PS(c11, inv_detC);
+
+      __m256 gamma0 = _mm256_sub_ps(_mm256_rcp_ps(den0), avx_ones);
+      __m256 gamma1 = _mm256_sub_ps(_mm256_rcp_ps(den1), avx_ones);
+
+      /* Add for averaging */
+      __m256 sinr_avx = _mm256_permute_ps(_mm256_add_ps(gamma0, gamma1), 0b00101000);
+      __attribute__((aligned(256))) float sinr[8];
+      _mm256_store_ps(sinr, sinr_avx);
+
+      sinr_list[i] += sinr[0] + sinr[2] + sinr[4] + sinr[6];
+
+      count += 4;
+    }
+
+    /* Divide average by noise */
+    sinr_list[i] /= count;
+
+    if (sinr_list[i] > max_sinr) {
+      max_sinr = sinr_list[i];
+      *pmi = i;
+    }
+  }
+
+  return i;
+}
+
+#endif /* LV_HAVE_AVX */
+
+/* PMI Select for 2 layers */
+int srslte_precoding_pmi_select_2l(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                   float noise_estimate, uint32_t *pmi,
+                                   float sinr_list[SRSLTE_MAX_CODEBOOKS]) {
+
+  int ret;
+#ifdef LV_HAVE_AVX
+  ret = srslte_precoding_pmi_select_2l_avx(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  #ifdef LV_HAVE_SSE
+  ret = srslte_precoding_pmi_select_2l_sse(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#else
+  ret = srslte_precoding_pmi_select_2l_gen(h, nof_symbols, noise_estimate, pmi, sinr_list);
+#endif /* LV_HAVE_SSE */
+#endif /* LV_HAVE_AVX */
+
+  INFO("Precoder PMI Select for 2 layers SINR=[%.1fdB; %.1fdB] PMI=%d\n", 10 * log10(sinr_list[0]),
+       10 * log10(sinr_list[1]), *pmi);
+
+  return ret;
+}
+
+int srslte_precoding_pmi_select(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols,
+                                float noise_estimate, int nof_layers, uint32_t *pmi,
+                                float sinr[SRSLTE_MAX_CODEBOOKS]) {
+  int ret;
+
+  if (sinr == NULL || pmi == NULL) {
+    ERROR("Null pointer");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  } else if (nof_layers == 1) {
+    ret = srslte_precoding_pmi_select_1l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else if (nof_layers == 2) {
+    ret = srslte_precoding_pmi_select_2l(h, nof_symbols, noise_estimate, pmi, sinr);
+  } else {
+    ERROR("Wrong number of layers");
+    ret = SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return ret;
+}
+
+/* PMI Select for 1 layer */
+float srslte_precoding_2x2_cn_gen(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_symbols) {
+  uint32_t count = 0;
+  float cn_avg = 0.0f;
+
+  for (uint32_t i = 0; i < nof_symbols; i += PMI_SEL_PRECISION) {
+    /* 0. Load channel matrix */
+    cf_t h00 = h[0][0][i];
+    cf_t h01 = h[1][0][i];
+    cf_t h10 = h[0][1][i];
+    cf_t h11 = h[1][1][i];
+
+    cn_avg += srslte_mat_2x2_cn(h00, h01, h10, h11);
+
+    count++;
+  }
+
+  return cn_avg/count;
+}
+
+/* Computes the condition number for a given number of antennas,
+ * stores in the parameter *cn the Condition Number in dB */
+int srslte_precoding_cn(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_tx_antennas,
+                          uint32_t nof_rx_antennas, uint32_t nof_symbols, float *cn) {
+  if (nof_tx_antennas == 2 && nof_rx_antennas == 2) {
+    *cn = srslte_precoding_2x2_cn_gen(h, nof_symbols);
+    return SRSLTE_SUCCESS;
+  } else {
+    ERROR("MIMO Condition Number calculation not implemented for %d×%d", nof_tx_antennas, nof_rx_antennas);
+    return SRSLTE_ERROR;
+  }
 }
 
diff --git a/lib/src/phy/mimo/test/CMakeLists.txt b/lib/src/phy/mimo/test/CMakeLists.txt
index e0e5578d5..cae668d9e 100644
--- a/lib/src/phy/mimo/test/CMakeLists.txt
+++ b/lib/src/phy/mimo/test/CMakeLists.txt
@@ -42,7 +42,7 @@ add_test(layermap_multiplex_24 layermap_test -n 1000 -m multiplex -c 2 -l 4)
  
  
 ########################################################################
-# LAYER MAPPING TEST  
+# PRECODING MAPPING TEST
 ########################################################################
  
 add_executable(precoding_test precoder_test.c)
@@ -52,6 +52,30 @@ add_test(precoding_single precoding_test -n 1000 -m single)
 add_test(precoding_diversity2 precoding_test -n 1000 -m diversity -l 2 -p 2) 
 add_test(precoding_diversity4 precoding_test -n 1024 -m diversity -l 4 -p 4) 
 
- 
+add_test(precoding_cdd_2x2_zf precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000 -d zf)
+add_test(precoding_cdd_2x2_mmse precoding_test -m cdd -l 2 -p 2 -r 2 -n 14000 -d mmse)
+
+add_test(precoding_multiplex_1l_cb0 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 0)
+add_test(precoding_multiplex_1l_cb1 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 1)
+add_test(precoding_multiplex_1l_cb2 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 2)
+add_test(precoding_multiplex_1l_cb3 precoding_test -m multiplex -l 1 -p 2 -r 2 -n 14000 -c 3)
+
+add_test(precoding_multiplex_2l_cb0_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0 -d zf)
+add_test(precoding_multiplex_2l_cb1_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1 -d zf)
+add_test(precoding_multiplex_2l_cb2_zf precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2 -d zf)
+
+
+add_test(precoding_multiplex_2l_cb0_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 0 -d mmse)
+add_test(precoding_multiplex_2l_cb1_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 1 -d mmse)
+add_test(precoding_multiplex_2l_cb2_mmse precoding_test -m multiplex -l 2 -p 2 -r 2 -n 14000 -c 2 -d mmse)
+
+########################################################################
+# PMI SELECT TEST
+########################################################################
+
+add_executable(pmi_select_test pmi_select_test.c)
+target_link_libraries(pmi_select_test srslte_phy)
+
+add_test(pmi_select_test pmi_select_test)
 
 
diff --git a/lib/src/phy/mimo/test/layermap_test.c b/lib/src/phy/mimo/test/layermap_test.c
index 209a9ac73..6f026f9d0 100644
--- a/lib/src/phy/mimo/test/layermap_test.c
+++ b/lib/src/phy/mimo/test/layermap_test.c
@@ -40,7 +40,7 @@ int nof_cw = 1, nof_layers = 1;
 char *mimo_type_name = NULL;
 
 void usage(char *prog) {
-  printf("Usage: %s -m [single|diversity|multiplex] -c [nof_cw] -l [nof_layers]\n", prog);
+  printf("Usage: %s -m [single|diversity|multiplex|cdd] -c [nof_cw] -l [nof_layers]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
 }
 
@@ -96,19 +96,19 @@ int main(int argc, char **argv) {
   }
 
   for (i=0;i<nof_cw;i++) {
-    d[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    d[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!d[i]) {
       perror("malloc");
       exit(-1);
     }
-    dp[i] = malloc(sizeof(cf_t) * nof_symb_cw[i]);
+    dp[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symb_cw[i]);
     if (!dp[i]) {
       perror("malloc");
       exit(-1);
     }
   }
   for (i=0;i<nof_layers;i++) {
-    x[i] = malloc(sizeof(cf_t) * nof_symbols);
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!x[i]) {
       perror("malloc");
       exit(-1);
diff --git a/lib/src/phy/mimo/test/pmi_select_test.c b/lib/src/phy/mimo/test/pmi_select_test.c
new file mode 100644
index 000000000..c7e5b5c8e
--- /dev/null
+++ b/lib/src/phy/mimo/test/pmi_select_test.c
@@ -0,0 +1,158 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+#include <unistd.h>
+#include <math.h>
+#include <time.h>
+#include <stdbool.h>
+#include <complex.h>
+
+
+#include "srslte/phy/utils/vector.h"
+#include "srslte/phy/mimo/precoding.h"
+#include "pmi_select_test.h"
+#include "srslte/phy/utils/debug.h"
+
+int main(int argc, char **argv) {
+  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  float noise_estimate;
+  float sinr_1l[SRSLTE_MAX_CODEBOOKS];
+  float sinr_2l[SRSLTE_MAX_CODEBOOKS];
+  float cn;
+  uint32_t pmi[2];
+  uint32_t nof_symbols = (uint32_t) SRSLTE_SF_LEN_RE(6, SRSLTE_CP_NORM);
+  int ret = SRSLTE_ERROR;
+
+  /* Allocate channels */
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      h[i][j] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
+      if (!h[i][j]) {
+        goto clean;
+      }
+      bzero(h[i][j], sizeof(cf_t) * nof_symbols);
+    }
+  }
+
+  for (int c = 0; c < PMI_SELECT_TEST_NOF_CASES; c++) {
+    pmi_select_test_case_gold_t *gold = &pmi_select_test_case_gold[c];
+
+    /* Set channel */
+    for (int i = 0; i < 2; i++) {
+      for (int j = 0; j < 2; j++) {
+        cf_t hij = gold->h[i][j];
+
+        for (int k = 0; k < nof_symbols; k++) {
+          h[i][j][k] = hij;
+        }
+      }
+    }
+
+    /* Set noise estimate */
+    noise_estimate = gold->n;
+
+    /* PMI select for 1 layer */
+    ret = srslte_precoding_pmi_select(h, nof_symbols, noise_estimate, 1, &pmi[0], sinr_1l);
+    if (ret < 0) {
+      ERROR("During PMI selection for 1 layer");
+      goto clean;
+    }
+
+    /* Check SINR for 1 layer */
+    for (int i = 0; i < ret; i++) {
+      if (fabsf(gold->snri_1l[i] - sinr_1l[i]) > 0.1) {
+        ERROR("Test case %d failed computing 1 layer SINR for codebook %d (test=%.2f; gold=%.2f)\n",
+              c + 1, i, sinr_1l[i], gold->snri_1l[i]);
+        goto clean;
+      }
+    }
+
+    /* Check PMI select for 1 layer*/
+    if (pmi[0] != gold->pmi[0]) {
+      ERROR("Test case %d failed computing 1 layer PMI (test=%d; gold=%d)\n", c + 1, pmi[0], gold->pmi[0]);
+      goto clean;
+    }
+
+    /* PMI select for 2 layer */
+    ret = srslte_precoding_pmi_select(h, nof_symbols, noise_estimate, 2, &pmi[1], sinr_2l);
+    if (ret < 0) {
+      ERROR("During PMI selection for 2 layer");
+      goto clean;
+    }
+
+    /* Check SINR for 2 layer */
+    for (int i = 0; i < ret; i++) {
+      if (fabsf(gold->snri_2l[i] - sinr_2l[i]) > 0.1) {
+        ERROR("Test case %d failed computing 2 layer SINR for codebook %d (test=%.2f; gold=%.2f)\n",
+              c + 1, i, sinr_2l[i], gold->snri_2l[i]);
+        goto clean;
+      }
+    }
+
+    /* Check PMI select for 2 layer*/
+    if (pmi[1] != gold->pmi[1]) {
+      ERROR("Test case %d failed computing 2 layer PMI (test=%d; gold=%d)\n", c + 1, pmi[1], gold->pmi[1]);
+      goto clean;
+    }
+
+    /* Condition number */
+    if (srslte_precoding_cn(h, 2, 2, nof_symbols, &cn)) {
+      ERROR("Test case %d condition number returned error\n");
+      goto clean;
+    }
+
+    /* Check condition number */
+    if (fabsf(gold->k - cn) > 0.1) {
+      ERROR("Test case %d failed computing condition number (test=%.2f; gold=%.2f)\n",
+            c + 1, cn, gold->k);
+      goto clean;
+    }
+  }
+
+  /* Test passed */
+  ret = SRSLTE_SUCCESS;
+
+  clean:
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      if (h[i][j]) {
+        free(h[i][j]);
+      }
+    }
+  }
+
+  if (ret) {
+    printf("Failed!\n");
+  } else {
+    printf("Passed!\n");
+  }
+
+  return ret;
+}
diff --git a/lib/src/phy/mimo/test/pmi_select_test.h b/lib/src/phy/mimo/test/pmi_select_test.h
new file mode 100644
index 000000000..c9c2ef5b3
--- /dev/null
+++ b/lib/src/phy/mimo/test/pmi_select_test.h
@@ -0,0 +1,237 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#ifndef PMI_SELECT_TEST_H
+#define PMI_SELECT_TEST_H
+
+#define PMI_SELECT_TEST_NOF_CASES 16
+
+typedef struct {
+  cf_t h[2][2];     /* Channel estimate */
+  float n;          /* Noise estimation */
+  float snri_1l[4]; /* SINR Approximation for 1 layer (linear) */
+  float snri_2l[2]; /* SINR Approximation for 2 layers (linear) */
+  uint32_t pmi[2];  /* Precoding Matrix Indicator for 1 and 2 layers */
+  uint32_t ri;      /* Rank indicator */
+  float k;          /* Condition number (κ) in dB */
+} pmi_select_test_case_gold_t;
+
+static pmi_select_test_case_gold_t pmi_select_test_case_gold [PMI_SELECT_TEST_NOF_CASES] = {
+  { /* Test case 1 */
+    .h = {
+      {+0.626226f+0.060103f*_Complex_I, -0.233387f-0.449860f*_Complex_I}, 
+      {+0.234558f-0.502742f*_Complex_I, +0.150990f-0.096722f*_Complex_I}
+      },
+    .n = 0.227713, 
+    .snri_1l = {2.728043f, 1.630673f, 3.226421f, 1.132295f},
+    .snri_2l = {1.797660f, 1.982149f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 6.4007,
+  },
+  { /* Test case 2 */
+    .h = {
+      {+0.608899f-0.825846f*_Complex_I, +0.972208f+0.604183f*_Complex_I}, 
+      {-0.940016f+0.978290f*_Complex_I, +0.071328f-0.866107f*_Complex_I}
+      },
+    .n = 0.939398, 
+    .snri_1l = {0.686850f, 4.591972f, 3.773925f, 1.504897f},
+    .snri_2l = {2.298235f, 1.761859f},
+    .pmi = {1, 0},
+    .ri = 1,
+    .k = 11.1305,
+  },
+  { /* Test case 3 */
+    .h = {
+      {-0.963645f+0.770719f*_Complex_I, +0.367677f+0.798010f*_Complex_I}, 
+      {+0.567473f+0.251875f*_Complex_I, +0.068275f-0.724262f*_Complex_I}
+      },
+    .n = 0.217802, 
+    .snri_1l = {3.209674f, 11.525338f, 11.962786f, 2.772226f},
+    .snri_2l = {3.226053f, 3.526363f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 15.4589,
+  },
+  { /* Test case 4 */
+    .h = {
+      {-0.635718f+0.879322f*_Complex_I, -0.916360f-0.291089f*_Complex_I}, 
+      {-0.786117f-0.178742f*_Complex_I, +0.232887f+0.968699f*_Complex_I}
+      },
+    .n = 0.945579, 
+    .snri_1l = {1.818313f, 2.141519f, 1.995787f, 1.964045f},
+    .snri_2l = {1.965011f, 1.958537f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 1.2910,
+  },
+  { /* Test case 5 */
+    .h = {
+      {+0.353289f+0.324764f*_Complex_I, +0.976605f-0.511669f*_Complex_I}, 
+      {+0.533663f-0.408985f*_Complex_I, -0.326601f+0.360357f*_Complex_I}
+      },
+    .n = 0.527847, 
+    .snri_1l = {1.173803f, 2.869865f, 2.273783f, 1.769885f},
+    .snri_2l = {1.871430f, 1.713879f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 5.5388,
+  },
+  { /* Test case 6 */
+    .h = {
+      {-0.176813f+0.103585f*_Complex_I, +0.205276f+0.167141f*_Complex_I}, 
+      {+0.501040f+0.023640f*_Complex_I, +0.167066f-0.834815f*_Complex_I}
+      },
+    .n = 0.719570, 
+    .snri_1l = {0.490387f, 1.022313f, 1.111245f, 0.401456f},
+    .snri_2l = {0.578124f, 0.597176f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 21.8808,
+  },
+  { /* Test case 7 */
+    .h = {
+      {+0.992312f+0.773088f*_Complex_I, -0.290931f-0.090610f*_Complex_I}, 
+      {+0.942518f-0.173145f*_Complex_I, -0.307102f-0.564536f*_Complex_I}
+      },
+    .n = 0.125655, 
+    .snri_1l = {19.459529f, 4.467420f, 18.044021f, 5.882928f},
+    .snri_2l = {8.055238f, 6.832247f},
+    .pmi = {0, 0},
+    .ri = 1,
+    .k = 9.9136,
+  },
+  { /* Test case 8 */
+    .h = {
+      {-0.382171f-0.980395f*_Complex_I, +0.452209f+0.686427f*_Complex_I}, 
+      {+0.565744f+0.844664f*_Complex_I, +0.387575f+0.541908f*_Complex_I}
+      },
+    .n = 0.042660, 
+    .snri_1l = {26.560881f, 49.864772f, 33.269985f, 43.155668f},
+    .snri_2l = {37.201526f, 34.461078f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 3.1172,
+  },
+  { /* Test case 9 */
+    .h = {
+      {-0.243628f-0.461891f*_Complex_I, +0.408679f+0.346062f*_Complex_I}, 
+      {+0.459026f-0.045016f*_Complex_I, -0.551446f+0.247433f*_Complex_I}
+      },
+    .n = 0.236445, 
+    .snri_1l = {1.429443f, 3.381496f, 0.227617f, 4.583322f},
+    .snri_2l = {1.272903f, 2.118832f},
+    .pmi = {3, 1},
+    .ri = 1,
+    .k = 24.1136,
+  },
+  { /* Test case 10 */
+    .h = {
+      {-0.645752f-0.784222f*_Complex_I, +0.659287f-0.635545f*_Complex_I}, 
+      {+0.533843f-0.801809f*_Complex_I, +0.868957f-0.020472f*_Complex_I}
+      },
+    .n = 0.193245, 
+    .snri_1l = {13.697372f, 4.693597f, 1.561737f, 16.829232f},
+    .snri_2l = {2.961344f, 5.773049f},
+    .pmi = {3, 1},
+    .ri = 1,
+    .k = 17.5194,
+  },
+  { /* Test case 11 */
+    .h = {
+      {+0.791783f+0.544990f*_Complex_I, -0.801821f-0.376120f*_Complex_I}, 
+      {-0.911669f-0.642035f*_Complex_I, +0.114590f-0.322089f*_Complex_I}
+      },
+    .n = 0.210146, 
+    .snri_1l = {2.340213f, 12.261749f, 5.921675f, 8.680286f},
+    .snri_2l = {6.912040f, 4.520201f},
+    .pmi = {1, 0},
+    .ri = 2,
+    .k = 7.7819,
+  },
+  { /* Test case 12 */
+    .h = {
+      {+0.020305f-0.218290f*_Complex_I, +0.812729f-0.890767f*_Complex_I}, 
+      {+0.257848f+0.002566f*_Complex_I, -0.796932f-0.136558f*_Complex_I}
+      },
+    .n = 0.997560, 
+    .snri_1l = {0.591218f, 1.636514f, 1.880263f, 0.347469f},
+    .snri_2l = {0.869026f, 0.967991f},
+    .pmi = {2, 1},
+    .ri = 1,
+    .k = 12.9774,
+  },
+  { /* Test case 13 */
+    .h = {
+      {+0.623205f-0.219990f*_Complex_I, -0.028697f+0.854712f*_Complex_I}, 
+      {+0.788896f+0.834988f*_Complex_I, -0.724907f+0.427148f*_Complex_I}
+      },
+    .n = 0.618337, 
+    .snri_1l = {3.706176f, 1.461946f, 0.479632f, 4.688490f},
+    .snri_2l = {1.444336f, 2.102567f},
+    .pmi = {3, 1},
+    .ri = 1,
+    .k = 17.0493,
+  },
+  { /* Test case 14 */
+    .h = {
+      {-0.313424f+0.292955f*_Complex_I, +0.872055f+0.666304f*_Complex_I}, 
+      {-0.750452f-0.203436f*_Complex_I, +0.461171f+0.499644f*_Complex_I}
+      },
+    .n = 0.835221, 
+    .snri_1l = {2.560265f, 0.379539f, 0.976562f, 1.963242f},
+    .snri_2l = {1.380223f, 1.109300f},
+    .pmi = {0, 0},
+    .ri = 1,
+    .k = 10.1729,
+  },
+  { /* Test case 15 */
+    .h = {
+      {-0.355079f-0.339153f*_Complex_I, +0.104523f+0.238943f*_Complex_I}, 
+      {+0.958258f-0.278727f*_Complex_I, +0.098617f+0.513019f*_Complex_I}
+      },
+    .n = 0.413901, 
+    .snri_1l = {1.633620f, 2.178855f, 0.809297f, 3.003178f},
+    .snri_2l = {1.250898f, 1.512017f},
+    .pmi = {3, 1},
+    .ri = 1,
+    .k = 10.8925,
+  },
+  { /* Test case 16 */
+    .h = {
+      {-0.015310f+0.675606f*_Complex_I, +0.389486f+0.478144f*_Complex_I}, 
+      {+0.945468f+0.908349f*_Complex_I, -0.344490f-0.936155f*_Complex_I}
+      },
+    .n = 0.356869, 
+    .snri_1l = {5.024121f, 4.926495f, 7.364348f, 2.586268f},
+    .snri_2l = {3.165416f, 3.851590f},
+    .pmi = {2, 1},
+    .ri = 2,
+    .k = 7.7799,
+  },
+};
+
+#endif /* PMI_SELECT_TEST_H */
diff --git a/lib/src/phy/mimo/test/precoder_mex.c b/lib/src/phy/mimo/test/precoder_mex.c
index 958f06262..b021f422a 100644
--- a/lib/src/phy/mimo/test/precoder_mex.c
+++ b/lib/src/phy/mimo/test/precoder_mex.c
@@ -78,9 +78,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     x[i] = srslte_vec_malloc(sizeof(cf_t)*nof_symbols/nof_layers);
   }
   
-  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_tx_ports/nof_layers);
+  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_tx_ports);
   for (int i=0;i<nof_tx_ports;i++) {
-    y[i] = &output[i*nof_symbols/nof_layers];
+    y[i] = &output[i*nof_symbols];
   }
   
   char *txscheme = "Port0";
@@ -102,13 +102,30 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
   int symbols_layers[SRSLTE_MAX_LAYERS];
   for (int i=0;i<nof_layers;i++) {
-    symbols_layers[i] = nof_symbols/nof_layers; 
+    symbols_layers[i] = nof_symbols;
   }
   srslte_layermap_type(d, x, nof_codewords, nof_layers, symbols_layers, type);
   srslte_precoding_type(x, y, nof_layers, nof_tx_ports, nof_symbols/nof_layers, type);
   
   if (nlhs >= 1) { 
-    mexutils_write_cf(output, &plhs[0], nof_symbols/nof_layers, nof_tx_ports);  
+    switch (type) {
+      case SRSLTE_MIMO_TYPE_CDD:
+        mexutils_write_cf(output, &plhs[0], nof_symbols/nof_layers, nof_tx_ports);
+        break;
+      case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      default:
+        mexutils_write_cf(output, &plhs[0], (uint32_t) nof_symbols, nof_tx_ports);
+        break;
+    }
+  }
+
+  if (nlhs >= 2) {
+    mexutils_write_cf(x[0], &plhs[1], nof_symbols / nof_layers, 1);
+  }
+  if (nlhs >= 3) {
+    mexutils_write_cf(x[1], &plhs[2], nof_symbols / nof_layers, 1);
   }
   
   if (input) {
diff --git a/lib/src/phy/mimo/test/precoder_test.c b/lib/src/phy/mimo/test/precoder_test.c
index e935a7067..1bbd9e3c0 100644
--- a/lib/src/phy/mimo/test/precoder_test.c
+++ b/lib/src/phy/mimo/test/precoder_test.c
@@ -34,29 +34,41 @@
 #include <stdbool.h>
 
 #include "srslte/srslte.h"
+#include "srslte/phy/channel/ch_awgn.h"
 
-#define MSE_THRESHOLD	0.00001
+#define MSE_THRESHOLD	0.0005
 
 int nof_symbols = 1000;
-int nof_layers = 1, nof_ports = 1;
+uint32_t codebook_idx = 0;
+int nof_layers = 1, nof_tx_ports = 1, nof_rx_ports = 1, nof_re = 1;
 char *mimo_type_name = NULL;
+char decoder_type_name [16] = "zf";
+float snr_db = 100.0f;
 
 void usage(char *prog) {
   printf(
-      "Usage: %s -m [single|diversity|multiplex] -l [nof_layers] -p [nof_ports]\n",
-      prog);
+      "Usage: %s -m [single|diversity|multiplex|cdd] -l [nof_layers] -p [nof_tx_ports]\n"
+          " -r [nof_rx_ports]\n", prog);
   printf("\t-n num_symbols [Default %d]\n", nof_symbols);
+  printf("\t-c codebook_idx [Default %d]\n", codebook_idx);
+  printf("\t-s SNR in dB [Default %.1fdB]*\n", snr_db);
+  printf("\t-d decoder type [zf|mmse] [Default %s]\n", decoder_type_name);
+  printf("\n");
+  printf("* Performance test example:\n\t for snr in {0..20..1}; do ./precoding_test -m single -s $snr; done; \n\n", decoder_type_name);
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "mpln")) != -1) {
+  while ((opt = getopt(argc, argv, "mplnrcds")) != -1) {
     switch (opt) {
     case 'n':
       nof_symbols = atoi(argv[optind]);
       break;
     case 'p':
-      nof_ports = atoi(argv[optind]);
+      nof_tx_ports = atoi(argv[optind]);
+      break;
+    case 'r':
+      nof_rx_ports = atoi(argv[optind]);
       break;
     case 'l':
       nof_layers = atoi(argv[optind]);
@@ -64,6 +76,15 @@ void parse_args(int argc, char **argv) {
     case 'm':
       mimo_type_name = argv[optind];
       break;
+    case 'c':
+      codebook_idx = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'd':
+      strncpy(decoder_type_name, argv[optind], 16);
+      break;
+    case 's':
+      snr_db = (float) atof(argv[optind]);
+      break;
     default:
       usage(argv[0]);
       exit(-1);
@@ -75,129 +96,236 @@ void parse_args(int argc, char **argv) {
   }
 }
 
+void populate_channel_cdd(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t n) {
+  int i, j, k;
+
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      for (k = 0; k < n; k++) {
+        h[i][j][k] = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+      }
+    }
+  }
+}
+
+void populate_channel_diversity(cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t n) {
+  int i, j, k, l;
+
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      for (k = 0; k < n / nof_layers; k++) {
+        cf_t hsymb = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+        for (l = 0; l < nof_layers; l++) {
+          // assume the channel is the same for all symbols
+          h[i][j][k * nof_layers + l] = hsymb;
+        }
+      }
+    }
+  }
+}
+
+void populate_channel_single(cf_t *h) {
+  int i;
+
+  for (i = 0; i < nof_re; i++) {
+    h[i] = (float) rand() / RAND_MAX + ((float) rand() / RAND_MAX) * _Complex_I;
+  }
+}
+
+void populate_channel(srslte_mimo_type_t type, cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]) {
+  switch (type) {
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+    case SRSLTE_MIMO_TYPE_CDD:
+      populate_channel_cdd(h, (uint32_t) nof_re);
+      break;
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      populate_channel_diversity(h, (uint32_t) nof_re);
+      break;
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+    default:
+      populate_channel_single(h[0][0]);
+  }
+}
+
+static void awgn(cf_t *y[SRSLTE_MAX_PORTS], uint32_t n, float snr) {
+  int i;
+  float std_dev = powf(10, - (snr + 3.0f) / 20.0f);
+
+  for (i = 0; i < nof_rx_ports; i++) {
+    srslte_ch_awgn_c(y[i], y[i], std_dev, n);
+  }
+}
+
 int main(int argc, char **argv) {
-  int i, j;
+  int i, j, k, nof_errors = 0, ret = SRSLTE_SUCCESS;
   float mse;
-  cf_t *x[SRSLTE_MAX_LAYERS], *r[SRSLTE_MAX_PORTS], *y[SRSLTE_MAX_PORTS], *h[SRSLTE_MAX_PORTS],
+  cf_t *x[SRSLTE_MAX_LAYERS], *r[SRSLTE_MAX_PORTS], *y[SRSLTE_MAX_PORTS], *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
       *xr[SRSLTE_MAX_LAYERS];
   srslte_mimo_type_t type;
   
   parse_args(argc, argv);
 
-  if (nof_ports > SRSLTE_MAX_PORTS || nof_layers > SRSLTE_MAX_LAYERS) {
+  /* Check input ranges */
+  if (nof_tx_ports > SRSLTE_MAX_PORTS || nof_rx_ports > SRSLTE_MAX_PORTS || nof_layers > SRSLTE_MAX_LAYERS) {
     fprintf(stderr, "Invalid number of layers or ports\n");
     exit(-1);
   }
 
+  /* Parse MIMO Type */
   if (srslte_str2mimotype(mimo_type_name, &type)) {
     fprintf(stderr, "Invalid MIMO type %s\n", mimo_type_name);
     exit(-1);
   }
 
+  /* Check scenario conditions are OK */
+  switch (type) {
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+      nof_re = nof_layers*nof_symbols;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+      nof_re = nof_symbols;
+      break;
+    case SRSLTE_MIMO_TYPE_CDD:
+      nof_re = nof_symbols*nof_tx_ports/nof_layers;
+      if (nof_rx_ports != 2 || nof_tx_ports != 2) {
+        fprintf(stderr, "CDD nof_tx_ports=%d nof_rx_ports=%d is not currently supported\n", nof_tx_ports, nof_rx_ports);
+        exit(-1);
+      }
+      break;
+    default:
+      nof_re = nof_symbols*nof_layers;
+  }
+
+  /* Allocate x and xr (received symbols) in memory for each layer */
   for (i = 0; i < nof_layers; i++) {
+    /* Source data */
     x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!x[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
+
+    /* Sink data */
     xr[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols);
     if (!xr[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
   }
-  for (i = 0; i < nof_ports; i++) {
-    y[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-    // TODO: The number of symbols per port is different in spatial multiplexing.
+
+  /* Allocate y in memory for tx each port */
+  for (i = 0; i < nof_tx_ports; i++) {
+    y[i] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
     if (!y[i]) {
       perror("srslte_vec_malloc");
       exit(-1);
     }
-    h[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-    if (!h[i]) {
-      perror("srslte_vec_malloc");
-      exit(-1);
+  }
+
+  /* Allocate h in memory for each cross channel and layer */
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j = 0; j < nof_rx_ports; j++) {
+      h[i][j] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
+      if (!h[i][j]) {
+        perror("srslte_vec_malloc");
+        exit(-1);
+      }
     }
   }
 
-  /* only 1 receiver antenna supported now */
-  r[0] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols * nof_layers);
-  if (!r[0]) {
-    perror("srslte_vec_malloc");
-    exit(-1);
+  /* Allocate r */
+  for (i = 0; i < nof_rx_ports; i++) {
+    r[i] = srslte_vec_malloc(sizeof(cf_t) * nof_re);
+    if (!r[i]) {
+      perror("srslte_vec_malloc");
+      exit(-1);
+    }
   }
 
-  /* generate random data */
+  /* Generate source random data */
   for (i = 0; i < nof_layers; i++) {
     for (j = 0; j < nof_symbols; j++) {
-      x[i][j] = (2*(rand()%2)-1+(2*(rand()%2)-1)*_Complex_I)/sqrt(2);
+      x[i][j] = (2 * (rand() % 2) - 1 + (2 * (rand() % 2) - 1) * _Complex_I) / sqrt(2);
     }
   }
-  
-  /* precoding */
-  if (srslte_precoding_type(x, y, nof_layers, nof_ports, nof_symbols, type) < 0) {
+
+  /* Execute Precoding (Tx) */
+  if (srslte_precoding_type(x, y, nof_layers, nof_tx_ports, codebook_idx, nof_symbols, type) < 0) {
     fprintf(stderr, "Error layer mapper encoder\n");
     exit(-1);
   }
 
   /* generate channel */
-  for (i = 0; i < nof_ports; i++) {
-    for (j = 0; j < nof_symbols; j++) {
-      h[i][nof_layers*j] = (float) rand()/RAND_MAX+((float) rand()/RAND_MAX)*_Complex_I;
-      // assume the channel is time-invariant in nlayer consecutive symbols
-      for (int k=0;k<nof_layers;k++) {
-        h[i][nof_layers*j+k] = h[i][nof_layers*j];              
-      }
-    }
-  }
+  populate_channel(type, h);
 
   /* pass signal through channel
    (we are in the frequency domain so it's a multiplication) */
-  /* there's only one receiver antenna, signals from different transmitter
-   * ports are simply combined at the receiver
-   */
-  for (j = 0; j < nof_symbols * nof_layers; j++) {
-    r[0][j] = 0;
-    for (i = 0; i < nof_ports; i++) {
-      r[0][j] += y[i][j] * h[i][j];
+  for (i = 0; i < nof_rx_ports; i++) {
+    for (k = 0; k < nof_re; k++) {
+      r[i][k] = (cf_t) (0.0 + 0.0 * _Complex_I);
+      for (j = 0; j < nof_tx_ports; j++) {
+        r[i][k] += y[j][k] * h[j][i][k];
+      }
     }
   }
-    
+
+  awgn(r, (uint32_t) nof_re, snr_db);
+
+  /* If CDD or Spatial muliplex choose decoder */
+  if (strncmp(decoder_type_name, "zf", 16) == 0) {
+    srslte_predecoding_set_mimo_decoder(SRSLTE_MIMO_DECODER_ZF);
+  } else if (strncmp(decoder_type_name, "mmse", 16) == 0) {
+    srslte_predecoding_set_mimo_decoder(SRSLTE_MIMO_DECODER_MMSE);
+  } else {
+    ret = SRSLTE_ERROR;
+    goto quit;
+  }
+
+
   /* predecoding / equalization */
   struct timeval t[3];
   gettimeofday(&t[1], NULL);
-  if (srslte_predecoding_type(r[0], h, xr, nof_ports, nof_layers,
-      nof_symbols * nof_layers, type, 0) < 0) {
-    fprintf(stderr, "Error layer mapper encoder\n");
-    exit(-1);
-  }
+  srslte_predecoding_type_multi(r, h, xr, nof_rx_ports, nof_tx_ports, nof_layers,
+                                codebook_idx, nof_re, type, powf(10, -snr_db/10));
   gettimeofday(&t[2], NULL);
   get_time_interval(t);
-  printf("Execution Time: %ld us\n", t[0].tv_usec);
-  
+
   /* check errors */
   mse = 0;
   for (i = 0; i < nof_layers; i++) {
     for (j = 0; j < nof_symbols; j++) {
       mse += cabsf(xr[i][j] - x[i][j]);
+
+      if ((crealf(xr[i][j]) > 0) != (crealf(x[i][j]) > 0)) {
+        nof_errors ++;
+      }
+      if ((cimagf(xr[i][j]) > 0) != (cimagf(x[i][j]) > 0)) {
+        nof_errors ++;
+      }
     }
   }
-  printf("MSE: %f\n", mse/ nof_layers / nof_symbols );
+  printf("SNR: %5.1fdB;\tExecution time: %5ldus;\tMSE: %.6f;\tBER: %.6f\n", snr_db, t[0].tv_usec,
+         mse / nof_layers / nof_symbols, (float) nof_errors / (4.0f * nof_re));
   if (mse / nof_layers / nof_symbols > MSE_THRESHOLD) {
-    exit(-1);
+    ret = SRSLTE_ERROR;
   } 
 
+  quit:
+  /* Free all data */
   for (i = 0; i < nof_layers; i++) {
     free(x[i]);
     free(xr[i]);
   }
-  for (i = 0; i < nof_ports; i++) {
-    free(y[i]);
-    free(h[i]);
+
+  for (i = 0; i < nof_rx_ports; i++) {
+    free(r[i]);
   }
 
-  free(r[0]);
-  
-  printf("Ok\n");
-  exit(0); 
+  for (i = 0; i < nof_rx_ports; i++) {
+    for (j = 0; j < nof_tx_ports; j++) {
+      free(h[j][i]);
+    }
+  }
+
+  exit(ret);
 }
diff --git a/lib/src/phy/mimo/test/predecoder_mex.c b/lib/src/phy/mimo/test/predecoder_mex.c
index 629ac0dc6..0465e1ef4 100644
--- a/lib/src/phy/mimo/test/predecoder_mex.c
+++ b/lib/src/phy/mimo/test/predecoder_mex.c
@@ -35,8 +35,10 @@
 #define HEST     prhs[1]
 #define NEST     prhs[2]
 #define NLAYERS  prhs[3]
-#define TXSCHEME prhs[4]
-#define NOF_INPUTS 5
+#define NCW      prhs[4]
+#define TXSCHEME prhs[5]
+#define CODEBOOK prhs[6]
+#define NOF_INPUTS 7
 
 
 void help()
@@ -48,118 +50,159 @@ void help()
 /* the gateway function */
 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 {
+  const mwSize *dims = mxGetDimensions(INPUT);
+  mwSize ndims;
   cf_t *input = NULL;
   cf_t *hest = NULL; 
   cf_t *output = NULL; 
   uint32_t nof_symbols = 0; 
+  uint32_t nof_rx_ants  = 1;
+  uint32_t nof_layers;
+  uint32_t nof_tx_ports = 1;
+  uint32_t nof_codewords = 1;
+  uint32_t codebook_idx = 0;
+  float noise_estimate = 0;
+  cf_t *x[SRSLTE_MAX_LAYERS];
+  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  cf_t *y[SRSLTE_MAX_PORTS];
+  int symbols_layers[SRSLTE_MAX_LAYERS];
+  int i, j;
+  srslte_mimo_type_t type;
   
+  /* Print help if number of inputs does not match with expected */
   if (nrhs < NOF_INPUTS) {
     help();
     return;
   }
   
-  // Read input symbols
+  /* Read input symbols */
   if (mexutils_read_cf(INPUT, &input) < 0) {
     mexErrMsgTxt("Error reading input\n");
     return; 
   }
-  uint32_t nof_layers   = mxGetScalar(NLAYERS); 
-  uint32_t nof_tx_ports = 1; 
-  uint32_t nof_codewords = 1; 
 
-  uint32_t nof_rx_ants  = 1; 
-  const mwSize *dims = mxGetDimensions(INPUT);
-  mwSize ndims = mxGetNumberOfDimensions(INPUT);
-  nof_symbols = dims[0];
-  
+  /* Read number of layers */
+  nof_layers = (uint32_t) mxGetScalar(NLAYERS);
+
+  /* Read number of codewords */
+  nof_codewords = (uint32_t) mxGetScalar(NCW);
+
+  if (nof_layers > SRSLTE_MAX_LAYERS) {
+    mexErrMsgTxt("Too many layers\n");
+    return;
+  }
+
+  /* Read number of symbols and Rx antennas */
+  ndims = mxGetNumberOfDimensions(INPUT);
+  nof_symbols = (uint32_t) dims[0];
+
   if (ndims >= 2) {
-    nof_rx_ants = dims[1];
+    nof_rx_ants = (uint32_t) dims[1];
   }
   
-  // Read channel estimates
+  /* Read channel estimates */
   if (mexutils_read_cf(HEST, &hest) < 0) {
     mexErrMsgTxt("Error reading hest\n");
     return; 
   }
+
+  /* Get number of tx ports */
   dims = mxGetDimensions(HEST);
   ndims = mxGetNumberOfDimensions(HEST);
 
   if (ndims == 3) {
-    nof_tx_ports = dims[2];        
+    nof_tx_ports = (uint32_t) dims[2];
   }
-  
-  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_symbols=%d\n", nof_tx_ports, nof_rx_ants, nof_layers, nof_symbols);
 
-  // Read noise estimate
-  float noise_estimate = 0; 
+  /* Print parameters trace */
+  mexPrintf("nof_tx_ports=%d, nof_rx_ants=%d, nof_layers=%d, nof_codewords=%d, codebook_idx=%d, nof_symbols=%d\n",
+            nof_tx_ports, nof_rx_ants, nof_layers, nof_codewords, codebook_idx, nof_symbols);
+
+  /* Read noise estimate */
   if (nrhs >= NOF_INPUTS) {
-    noise_estimate = mxGetScalar(NEST);
+    noise_estimate = (float) mxGetScalar(NEST);
   }
   
-  cf_t *x[SRSLTE_MAX_LAYERS]; 
-  cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  cf_t *y[SRSLTE_MAX_PORTS];
-
-  for (int i=0;i<SRSLTE_MAX_LAYERS;i++) {
+  /* Initialise x, h & y pointers */
+  for (i=0;i<SRSLTE_MAX_LAYERS;i++) {
     x[i] = NULL; 
   }
-  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
-    for (int j=0;j<SRSLTE_MAX_PORTS;j++) {
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j=0;j<SRSLTE_MAX_PORTS;j++) {
       h[i][j] = NULL; 
     }
   }
-  
-  /* Allocate memory */
-  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols);
-  for (int i = 0; i < nof_tx_ports; i++) {
-    x[i] = srslte_vec_malloc(sizeof(cf_t)*nof_symbols);
-    for (int j=0;j<nof_rx_ants;j++) {
-      h[i][j] = &hest[i*nof_symbols*nof_rx_ants + j*nof_symbols];
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    y[i] = NULL;
+  }
+
+  /* Allocate memory for ouput */
+  output = srslte_vec_malloc(sizeof(cf_t)*nof_symbols*nof_layers);
+
+  /* Allocate memory for intermediate data */
+  for (i = 0; i < nof_tx_ports; i++) {
+    x[i] = srslte_vec_malloc(sizeof(cf_t) * nof_symbols*nof_layers);
+  }
+
+  /* Allocate memory for channel estimate */
+  for (i = 0; i < nof_tx_ports; i++) {
+    for (j=0; j<nof_rx_ants; j++) {
+      h[i][j] = &hest[(i*nof_rx_ants + j)*nof_symbols];
     }
   }
-  
-  for (int j=0;j<nof_rx_ants;j++) {
+
+  /* Allocate memory for input */
+  for (j = 0; j < nof_rx_ants; j++) {
     y[j] = &input[j*nof_symbols];
   }
-    
-  char *txscheme = "Port0";
+
+  /* Parse Tx scheme */
+  char txscheme[32] = "Port0";
   if (nrhs >= NOF_INPUTS) {
-    txscheme = mxArrayToString(TXSCHEME);
-  }  
-  srslte_mimo_type_t type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA; 
-  if (!strcmp(txscheme, "Port0")) {
-    type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
-  } else if (!strcmp(txscheme, "TxDiversity")) {
-    type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-  } else if (!strcmp(txscheme, "CDD")) {    
-    type = SRSLTE_MIMO_TYPE_CDD;
-  } else if (!strcmp(txscheme, "SpatialMux")) {    
-    type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-  } else {
+    mxGetString_700(TXSCHEME, txscheme, 32);
+  }
+
+  codebook_idx = (uint32_t) mxGetScalar(CODEBOOK);
+
+  if (srslte_str2mimotype(txscheme, &type)) {
     mexPrintf("Unsupported TxScheme=%s\n", txscheme);
     return; 
   }
-  int symbols_layers[SRSLTE_MAX_LAYERS];
-  for (int i=0;i<nof_layers;i++) {
+
+  /* Populate symbols in layers */
+  for (i = 0; i < nof_layers; i++) {
     symbols_layers[i] = nof_symbols; 
   }
-  cf_t *d[SRSLTE_MAX_LAYERS]; 
-  d[0] = output; 
-  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, nof_symbols/nof_layers, type, noise_estimate);
-  srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
-  
 
-  if (nlhs >= 1) { 
-    mexutils_write_cf(output, &plhs[0], nof_symbols, 1);  
+  /* Set output pointer */
+  cf_t *d[SRSLTE_MAX_CODEWORDS];
+  for (i = 0; i<nof_codewords; i++) {
+    d[i] = &output[i*nof_symbols*nof_layers/nof_codewords];
   }
+
+  /* Pre-decode */
+  srslte_predecoding_type_multi(y, h, x, nof_rx_ants, nof_tx_ports, nof_layers, codebook_idx, nof_symbols, type,
+                                noise_estimate);
+
+  /* Layer de-mapper */
+  srslte_layerdemap_type(x, d, nof_layers, nof_codewords, nof_symbols, symbols_layers, type);
   
+  /* Write output */
+  if (nlhs >= 1) {
+    mexutils_write_cf(output, &plhs[0], nof_symbols, nof_codewords);
+  }
+
+  /* Free memory */
   if (input) {
     free(input);
   }
+  if (hest) {
+    free(hest);
+  }
   if (output) {
     free(output);
   }
-  for (int i=0;i<SRSLTE_MAX_LAYERS;i++) {
+  for (i=0;i<SRSLTE_MAX_LAYERS;i++) {
     if (x[i]) {
       free(x[i]);      
     }
diff --git a/lib/src/phy/phch/cqi.c b/lib/src/phy/phch/cqi.c
index a684db8c9..0041c3fcb 100644
--- a/lib/src/phy/phch/cqi.c
+++ b/lib/src/phy/phch/cqi.c
@@ -159,46 +159,54 @@ int srslte_cqi_size(srslte_cqi_value_t *value) {
   return -1;
 }
 
-bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
-  
-  uint32_t N_p = 0;
-  uint32_t N_offset = 0;
-  
+static bool srslte_cqi_get_N(uint32_t I_cqi_pmi, uint32_t *N_p, uint32_t *N_offset) {
   if (I_cqi_pmi <= 1) {
-    N_p = 2; 
-    N_offset = I_cqi_pmi; 
+    *N_p = 2;
+    *N_offset = I_cqi_pmi;
   } else if (I_cqi_pmi <= 6) {
-    N_p = 5; 
-    N_offset = I_cqi_pmi - 2;     
+    *N_p = 5;
+    *N_offset = I_cqi_pmi - 2;
   } else if (I_cqi_pmi <= 16) {
-    N_p = 10; 
-    N_offset = I_cqi_pmi - 7;     
+    *N_p = 10;
+    *N_offset = I_cqi_pmi - 7;
   } else if (I_cqi_pmi <= 36) {
-    N_p = 20; 
-    N_offset = I_cqi_pmi - 17;     
+    *N_p = 20;
+    *N_offset = I_cqi_pmi - 17;
   } else if (I_cqi_pmi <= 76) {
-    N_p = 40; 
-    N_offset = I_cqi_pmi - 37;     
+    *N_p = 40;
+    *N_offset = I_cqi_pmi - 37;
   } else if (I_cqi_pmi <= 156) {
-    N_p = 80; 
-    N_offset = I_cqi_pmi - 77;     
+    *N_p = 80;
+    *N_offset = I_cqi_pmi - 77;
   } else if (I_cqi_pmi <= 316) {
-    N_p = 160; 
-    N_offset = I_cqi_pmi - 157;   
+    *N_p = 160;
+    *N_offset = I_cqi_pmi - 157;
   } else if (I_cqi_pmi == 317) {
-    return false; 
+    return false;
   } else if (I_cqi_pmi <= 349) {
-    N_p = 32; 
-    N_offset = I_cqi_pmi - 318;     
+    *N_p = 32;
+    *N_offset = I_cqi_pmi - 318;
   } else if (I_cqi_pmi <= 413) {
-    N_p = 64; 
-    N_offset = I_cqi_pmi - 350;     
+    *N_p = 64;
+    *N_offset = I_cqi_pmi - 350;
   } else if (I_cqi_pmi <= 541) {
-    N_p = 128; 
-    N_offset = I_cqi_pmi - 414;     
+    *N_p = 128;
+    *N_offset = I_cqi_pmi - 414;
   } else if (I_cqi_pmi <= 1023) {
-    return false; 
+    return false;
+  }
+  return true;
+}
+
+bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
+  
+  uint32_t N_p = 0;
+  uint32_t N_offset = 0;
+
+  if (!srslte_cqi_get_N(I_cqi_pmi, &N_p, &N_offset)) {
+    return false;
   }
+
   if (N_p) {
     if ((tti-N_offset)%N_p == 0) {
       return true; 
@@ -207,6 +215,47 @@ bool srslte_cqi_send(uint32_t I_cqi_pmi, uint32_t tti) {
   return false; 
 }
 
+bool srslte_ri_send(uint32_t I_cqi_pmi, uint32_t I_ri, uint32_t tti) {
+
+  uint32_t M_ri = 0;
+  uint32_t N_offset_ri = 0;
+  uint32_t N_p = 0;
+  uint32_t N_offset_p = 0;
+
+  if (!srslte_cqi_get_N(I_cqi_pmi, &N_p, &N_offset_p)) {
+    return false;
+  }
+
+  if (I_ri <= 160) {
+    M_ri = 1;
+    N_offset_ri = I_ri;
+  } else if (I_ri <= 161) {
+    M_ri = 2;
+    N_offset_ri = I_ri - 161;
+  } else if (I_ri <= 322) {
+    M_ri = 4;
+    N_offset_ri = I_ri - 322;
+  } else if (I_ri <= 483) {
+    M_ri = 8;
+    N_offset_ri = I_ri - 483;
+  } else if (I_ri <= 644) {
+    M_ri = 16;
+    N_offset_ri = I_ri - 644;
+  } else if (I_ri <= 805) {
+    M_ri = 32;
+    N_offset_ri = I_ri - 805;
+  } else if (I_ri <= 966) {
+    return false;
+  }
+
+  if (M_ri) {
+    if ((tti - N_offset_p + N_offset_ri) % (N_p * M_ri) == 0) {
+      return true;
+    }
+  }
+  return false;
+}
+
 
 // CQI-to-Spectral Efficiency:  36.213 Table 7.2.3-1  */
 static float cqi_to_coderate[16] = {0, 0.1523, 0.2344, 0.3770, 0.6016, 0.8770, 1.1758, 1.4766, 1.9141, 2.4063, 2.7305, 3.3223, 3.9023, 4.5234, 5.1152, 5.5547}; 
diff --git a/lib/src/phy/phch/dci.c b/lib/src/phy/phch/dci.c
index 96c446259..2daa7e10d 100644
--- a/lib/src/phy/phch/dci.c
+++ b/lib/src/phy/phch/dci.c
@@ -1051,6 +1051,16 @@ int dci_format2AB_pack(srslte_ra_dl_dci_t *data, srslte_dci_msg_t *msg, uint32_t
   } else {
     *y++ = data->tb_cw_swap;
   }
+
+  /* Force MCS_idx and RV_idx in function of block enable according to 7.1.7 of 36.213 */
+  if (!data->tb_en[0]) {
+    data->mcs_idx = 0;
+    data->rv_idx= 1;
+  }
+  if (!data->tb_en[1]) {
+    data->mcs_idx_1 = 0;
+    data->rv_idx_1 = 1;
+  }
   
   /* pack TB1 */
   srslte_bit_unpack(data->mcs_idx, &y, 5);
diff --git a/lib/src/phy/phch/pcfich.c b/lib/src/phy/phch/pcfich.c
index b38c0266f..d960504fe 100644
--- a/lib/src/phy/phch/pcfich.c
+++ b/lib/src/phy/phch/pcfich.c
@@ -57,14 +57,10 @@ bool srslte_pcfich_exists(int nframe, int nslot) {
   return true;
 }
 
-int srslte_pcfich_init(srslte_pcfich_t *q) {
-  return srslte_pcfich_init_multi(q, 1);
-}
-
 /** Initializes the pcfich channel receiver. 
  * On error, returns -1 and frees the structrure 
  */
-int srslte_pcfich_init_multi(srslte_pcfich_t *q, uint32_t nof_rx_antennas) {
+int srslte_pcfich_init(srslte_pcfich_t *q, uint32_t nof_rx_antennas) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q != NULL)
diff --git a/lib/src/phy/phch/pdcch.c b/lib/src/phy/phch/pdcch.c
index 60ab136af..215324e6b 100644
--- a/lib/src/phy/phch/pdcch.c
+++ b/lib/src/phy/phch/pdcch.c
@@ -61,11 +61,7 @@ float srslte_pdcch_coderate(uint32_t nof_bits, uint32_t l) {
 }
 
 /** Initializes the PDCCH transmitter and receiver */
-int srslte_pdcch_init(srslte_pdcch_t *q, uint32_t max_prb) {
-  return srslte_pdcch_init_multi(q, max_prb, 1);
-}
-
-int srslte_pdcch_init_multi(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
+static int pdcch_init(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas, bool is_ue)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
@@ -74,7 +70,7 @@ int srslte_pdcch_init_multi(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx
     ret = SRSLTE_ERROR;
     bzero(q, sizeof(srslte_pdcch_t));
     q->nof_rx_antennas = nof_rx_antennas;
-    
+    q->is_ue           = is_ue;
     /* Allocate memory for the maximum number of PDCCH bits (CFI=3) */
     q->max_bits = max_prb*3*12*2;
 
@@ -110,22 +106,22 @@ int srslte_pdcch_init_multi(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx
     }
 
     for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
-      for (int j=0;j<q->nof_rx_antennas;j++) {
-        q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
-        if (!q->ce[i][j]) {
-          goto clean;
-        }          
-      }
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
       if (!q->x[i]) {
         goto clean;
       }
-    }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
-      q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
-      if (!q->symbols[j]) {
+      q->symbols[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
+      if (!q->symbols[i]) {
         goto clean;
       }
+      if (q->is_ue) {
+        for (int j = 0; j < q->nof_rx_antennas; j++) {
+          q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_bits / 2);
+          if (!q->ce[i][j]) {
+            goto clean;
+          }
+        }
+      }
     }
 
     ret = SRSLTE_SUCCESS;
@@ -137,6 +133,14 @@ int srslte_pdcch_init_multi(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx
   return ret;
 }
 
+int srslte_pdcch_init_enb(srslte_pdcch_t *q, uint32_t max_prb) {
+  return pdcch_init(q, max_prb, 0, false);
+}
+
+int srslte_pdcch_init_ue(srslte_pdcch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas) {
+  return pdcch_init(q, max_prb, nof_rx_antennas, true);
+}
+
 void srslte_pdcch_free(srslte_pdcch_t *q) {
 
   if (q->e) {
@@ -149,18 +153,18 @@ void srslte_pdcch_free(srslte_pdcch_t *q) {
     free(q->d);
   }
   for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    for (int j=0;j<q->nof_rx_antennas;j++) {
-      if (q->ce[i][j]) {
-        free(q->ce[i][j]);
-      }      
-    }
     if (q->x[i]) {
       free(q->x[i]);
     }
-  }
-  for (int j=0;j<q->nof_rx_antennas;j++) {
-    if (q->symbols[j]) {
-      free(q->symbols[j]);
+    if (q->symbols[i]) {
+      free(q->symbols[i]);
+    }
+    if (q->is_ue) {
+      for (int j=0;j<q->nof_rx_antennas;j++) {
+        if (q->ce[i][j]) {
+          free(q->ce[i][j]);
+        }
+      }
     }
   }
   for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index a10be3e09..c29d048da 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -28,11 +28,16 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
+#include <stdbool.h>
+#include <srslte/phy/phch/pdsch_cfg.h>
+#include <srslte/phy/common/sequence.h>
 #include <srslte/phy/phch/pdsch.h>
-#include "srslte/phy/phch/pdsch.h"
+
 #include "prb_dl.h"
+#include "srslte/phy/phch/pdsch.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
+#include "srslte/phy/utils/bit.h"
 
 
 #define MAX_PDSCH_RE(cp) (2 * SRSLTE_CP_NSYMB(cp) * 12)
@@ -189,50 +194,58 @@ int srslte_pdsch_get(srslte_pdsch_t *q, cf_t *sf_symbols, cf_t *symbols,
   return srslte_pdsch_cp(q, sf_symbols, symbols, grant, lstart, subframe, false);
 }
 
-/** Initializes the PDCCH transmitter and receiver */
-int pdsch_init_multi(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas, bool is_ue)
+/** Initializes the PDSCH transmitter and receiver */
+static int pdsch_init(srslte_pdsch_t *q, uint32_t max_prb, bool is_ue, uint32_t nof_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
-  int i;
 
- if (q != NULL                  &&
-     nof_rx_antennas <= SRSLTE_MAX_PORTS)
-  {   
+  if (q != NULL)
+  {
     
     bzero(q, sizeof(srslte_pdsch_t));
     ret = SRSLTE_ERROR;
     
-    q->max_re = max_prb * MAX_PDSCH_RE(q->cell.cp);
-    q->nof_rx_antennas = nof_rx_antennas; 
-    
+    q->max_re          = max_prb * MAX_PDSCH_RE(q->cell.cp);
+    q->is_ue           = is_ue;
+    q->nof_rx_antennas = nof_antennas;
+
     INFO("Init PDSCH: %d PRBs, max_symbols: %d\n", max_prb, q->max_re);
 
-    for (i = 0; i < 4; i++) {
+    for (int i = 0; i < 4; i++) {
       if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
         goto clean;
       }
       srslte_modem_table_bytes(&q->mod[i]);
     }
-    
-    srslte_sch_init(&q->dl_sch);
-    
-    // Allocate int16_t for reception (LLRs)
-    q->e = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
-    if (!q->e) {
+
+    if (srslte_sch_init(&q->dl_sch)) {
+      ERROR("Initiating DL SCH");
       goto clean;
     }
-    
-    q->d = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-    if (!q->d) {
-      goto clean;
+
+    for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      // Allocate int16_t for reception (LLRs)
+      q->e[i] = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
+      if (!q->e[i]) {
+        goto clean;
+      }
+
+      q->d[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->d[i]) {
+        goto clean;
+      }
     }
 
-    for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+    for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
       q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
       if (!q->x[i]) {
         goto clean;
       }
-      if (is_ue) {
+      q->symbols[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->symbols[i]) {
+        goto clean;
+      }
+      if (q->is_ue) {
         for (int j=0;j<q->nof_rx_antennas;j++) {
           q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
           if (!q->ce[i][j]) {
@@ -241,15 +254,8 @@ int pdsch_init_multi(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antenn
         }
       }
     }
-    for (int j=0;j<SRSLTE_MAX(q->nof_rx_antennas, q->cell.nof_ports);j++) {
-      q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
-      if (!q->symbols[j]) {
-        goto clean;
-      }              
-    }
 
-    q->is_ue = is_ue;
-    q->users = calloc(sizeof(srslte_pdsch_user_t*), is_ue?1:(1+SRSLTE_SIRNTI));
+    q->users = calloc(sizeof(srslte_pdsch_user_t*), q->is_ue?1:(1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean;
@@ -261,6 +267,7 @@ int pdsch_init_multi(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antenn
 
     ret = SRSLTE_SUCCESS;
   }
+
   clean: 
   if (ret == SRSLTE_ERROR) {
     srslte_pdsch_free(q);
@@ -268,51 +275,47 @@ int pdsch_init_multi(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antenn
   return ret;
 }
 
-int srslte_pdsch_init_ue(srslte_pdsch_t *q, uint32_t max_prb)
+int srslte_pdsch_init_ue(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_antennas)
 {
-  return pdsch_init_multi(q, max_prb, 1, true);
+  return pdsch_init(q, max_prb, true, nof_antennas);
 }
 
 int srslte_pdsch_init_enb(srslte_pdsch_t *q, uint32_t max_prb)
 {
-  return pdsch_init_multi(q, max_prb, 1, false);
-}
-
-int srslte_pdsch_init_multi_ue(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
-{
-  return pdsch_init_multi(q, max_prb, nof_rx_antennas, true);
+  return pdsch_init(q, max_prb, false, 0);
 }
 
-int srslte_pdsch_init_multi_enb(srslte_pdsch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
-{
-  return pdsch_init_multi(q, max_prb, nof_rx_antennas, false);
-}
+void srslte_pdsch_free(srslte_pdsch_t *q) {
 
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
 
-void srslte_pdsch_free(srslte_pdsch_t *q) {
-  int i;
+    if (q->e[i]) {
+      free(q->e[i]);
+    }
 
-  if (q->e) {
-    free(q->e);
-  }
-  if (q->d) {
-    free(q->d);
+    if (q->d[i]) {
+      free(q->d[i]);
+    }
   }
-  for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+
+  /* Free sch objects */
+  srslte_sch_free(&q->dl_sch);
+
+  for (int i = 0; i < SRSLTE_MAX_PORTS; i++) {
     if (q->x[i]) {
       free(q->x[i]);
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
-      if (q->ce[i][j]) {
-        free(q->ce[i][j]);
+    if (q->symbols[i]) {
+      free(q->symbols[i]);
+    }
+    if (q->is_ue) {
+      for (int j=0;j<q->nof_rx_antennas;j++) {
+        if (q->ce[i][j]) {
+          free(q->ce[i][j]);
+        }
       }
     }
   }
-  for (int j=0;j<q->nof_rx_antennas;j++) {
-    if (q->symbols[j]) {
-      free(q->symbols[j]);
-    }          
-  }
   if (q->users) {
     if (q->is_ue) {
       srslte_pdsch_free_rnti(q, 0);
@@ -328,14 +331,11 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
 
   srslte_sequence_free(&q->tmp_seq);
 
-  for (i = 0; i < 4; i++) {
+  for (int i = 0; i < 4; i++) {
     srslte_modem_table_free(&q->mod[i]);
   }
 
-  srslte_sch_free(&q->dl_sch);
-
   bzero(q, sizeof(srslte_pdsch_t));
-
 }
 
 int srslte_pdsch_set_cell(srslte_pdsch_t *q, srslte_cell_t cell)
@@ -356,35 +356,10 @@ int srslte_pdsch_set_cell(srslte_pdsch_t *q, srslte_cell_t cell)
   return ret;
 }
 
-/* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg. 
- * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
- */
-int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
-{
-  if (cfg) {
-    if (grant) {
-      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
-    }
-    if (srslte_cbsegm(&cfg->cb_segm, cfg->grant.mcs.tbs)) {
-      fprintf(stderr, "Error computing Codeblock segmentation for TBS=%d\n", cfg->grant.mcs.tbs);
-      return SRSLTE_ERROR; 
-    }
-    srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, &cfg->nbits);
-    cfg->sf_idx = sf_idx; 
-    cfg->rv = rvidx;  
-
-    return SRSLTE_SUCCESS;   
-  } else {
-    return SRSLTE_ERROR_INVALID_INPUTS;
-  }
-}
-
-
 /* Precalculate the PDSCH scramble sequences for a given RNTI. This function takes a while 
  * to execute, so shall be called once the final C-RNTI has been allocated for the session.
  */
 int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
-  uint32_t i;
   uint32_t rnti_idx = q->is_ue?0:rnti;
 
   if (!q->users[rnti_idx] || q->is_ue) {
@@ -395,13 +370,15 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
         return -1;
       }
     }
-    for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[i], rnti, 0, 2 * i, q->cell.id,
-          q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
-      {
-        fprintf(stderr, "Error initializing PDSCH scrambling sequence\n");
-        srslte_pdsch_free_rnti(q, rnti);
-        return SRSLTE_ERROR;
+    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+        if (srslte_sequence_pdsch(&q->users[rnti_idx]->seq[j][i], rnti, j, 2 * i, q->cell.id,
+                                  q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM)))
+        {
+          fprintf(stderr, "Error initializing PDSCH scrambling sequence\n");
+          srslte_pdsch_free_rnti(q, rnti);
+          return SRSLTE_ERROR;
+        }
       }
     }
     q->users[rnti_idx]->cell_id = q->cell.id;
@@ -417,29 +394,134 @@ void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
   uint32_t rnti_idx = q->is_ue?0:rnti;
   if (q->users[rnti_idx]) {
     for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
-      srslte_sequence_free(&q->users[rnti_idx]->seq[i]);
+      for (int j = 0; j < SRSLTE_MAX_CODEWORDS; j++) {
+        srslte_sequence_free(&q->users[rnti_idx]->seq[j][i]);
+      }
     }
     free(q->users[rnti_idx]);
     q->users[rnti_idx] = NULL;
   }
 }
 
-int srslte_pdsch_decode(srslte_pdsch_t *q, 
-                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
-                        cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate, 
-                        uint16_t rnti, uint8_t *data) 
+static void pdsch_decode_debug(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
+                               cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS])
 {
-  cf_t *_sf_symbols[SRSLTE_MAX_PORTS]; 
-  cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
-  _sf_symbols[0] = sf_symbols; 
-  for (int i=0;i<q->cell.nof_ports;i++) {
-    _ce[i][0] = ce[i]; 
+  if (SRSLTE_VERBOSE_ISDEBUG()) {
+    char filename[FILENAME_MAX];
+    for (int j = 0; j < q->nof_rx_antennas; j++) {
+      if (snprintf(filename, FILENAME_MAX, "subframe_p%d.dat", j) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: received subframe symbols\n", filename);
+      srslte_vec_save_file(filename, sf_symbols[j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+
+      for (int i = 0; i < q->cell.nof_ports; i++) {
+        if (snprintf(filename, FILENAME_MAX, "hest_%d%d.dat", i, j) < 0) {
+          ERROR("Generating file name");
+          break;
+        }
+        DEBUG("SAVED FILE %s: channel estimates for Tx %d and Rx %d\n", filename, j, i);
+        srslte_vec_save_file(filename, ce[i][j], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      }
+    }
+    for (int i=0;i<cfg->nof_layers;i++) {
+      if (snprintf(filename, FILENAME_MAX, "pdsch_symbols_%d.dat", i) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: symbols after equalization\n", filename);
+      srslte_vec_save_file(filename, q->d[i], cfg->nbits[0].nof_re*sizeof(cf_t));
+
+      if (snprintf(filename, FILENAME_MAX, "llr_%d.dat", i) < 0) {
+        ERROR("Generating file name");
+        break;
+      }
+      DEBUG("SAVED FILE %s: LLR estimates after demodulation and descrambling\n", filename);
+      srslte_vec_save_file(filename, q->e[i], cfg->nbits[0].nof_bits*sizeof(int16_t));
+    }
   }
-  return srslte_pdsch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, rnti, data); 
 }
 
-static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
+
+/* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg. 
+ * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
+ */
+int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
+                     uint32_t sf_idx, int rvidx) {
+  int _rvids[SRSLTE_MAX_CODEWORDS] = {1};
+  _rvids[0] = rvidx;
+
+  return srslte_pdsch_cfg_mimo(cfg, cell, grant, cfi, sf_idx, _rvids, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0);
+}
+
+/* Configures the structure srslte_pdsch_cfg_t from the DL DCI allocation dci_msg.
+ * If dci_msg is NULL, the grant is assumed to be already stored in cfg->grant
+ */
+int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
+                           uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
+                           uint32_t pmi) {
+  if (cfg) {
+    if (grant) {
+      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
+    }
+
+    for (int i = 0; i < grant->nof_tb; i++) {
+      if (srslte_cbsegm(&cfg->cb_segm[i], (uint32_t) cfg->grant.mcs[i].tbs)) {
+        fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[i].tbs);
+        return SRSLTE_ERROR;
+      }
+    }
+    srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
+
+    cfg->sf_idx = sf_idx;
+    memcpy(cfg->rv, rvidx, sizeof(uint32_t) * SRSLTE_MAX_CODEWORDS);
+    cfg->mimo_type = mimo_type;
+
+    /* Check and configure PDSCH transmission modes */
+    switch(mimo_type) {
+      case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+        if (grant->nof_tb != 1) {
+          ERROR("Number of transport blocks is not supported for single transmission mode.");
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 1;
+        break;
+      case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+        if (grant->nof_tb != 1) {
+          ERROR("Number of transport blocks is not supported for transmit diversity mode.");
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 2;
+        break;
+      case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+        if (grant->nof_tb == 1) {
+          cfg->codebook_idx = pmi;
+          cfg->nof_layers = 1;
+        } else {
+          cfg->codebook_idx = pmi + 1;
+          cfg->nof_layers = 2;
+        }
+        INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n",
+             grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+        break;
+      case SRSLTE_MIMO_TYPE_CDD:
+        if (grant->nof_tb != 2) {
+          ERROR("Number of transport blocks (%d) is not supported for CDD transmission mode.", grant->nof_tb);
+          return SRSLTE_ERROR;
+        }
+        cfg->nof_layers = 2;
+        break;
+    }
+
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
+
+static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti,
+                                            uint32_t codeword_idx, uint32_t sf_idx, uint32_t len)
 {
   uint32_t rnti_idx = q->is_ue?0:rnti;
 
@@ -448,178 +530,264 @@ static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti, ui
       q->users[rnti_idx]->cell_id == q->cell.id &&
       ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
   {
-    return &q->users[rnti_idx]->seq[sf_idx];
+    return &q->users[rnti_idx]->seq[codeword_idx][sf_idx];
   } else {
-    srslte_sequence_pdsch(&q->tmp_seq, rnti, 0, 2 * sf_idx, q->cell.id, len);
+    srslte_sequence_pdsch(&q->tmp_seq, rnti, codeword_idx, 2 * sf_idx, q->cell.id, len);
     return &q->tmp_seq;
   }
 }
 
+static int srslte_pdsch_codeword_encode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
+                                               srslte_softbuffer_tx_t *softbuffer, uint16_t rnti, uint8_t *data,
+                                               uint32_t codeword_idx) {
+  srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
+  srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
+  uint32_t rv = cfg->rv[codeword_idx];
+
+  if (nbits->nof_bits) {
+    INFO("Encoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+         cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
+         nbits->nof_re, nbits->nof_bits, rv);
+
+    /* Channel coding */
+    if (srslte_dlsch_encode2(&q->dl_sch, cfg, softbuffer, data, q->e[codeword_idx], codeword_idx)) {
+      ERROR("Error encoding TB %d", codeword_idx);
+      return SRSLTE_ERROR;
+    }
+
+    /* Select scrambling sequence */
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, codeword_idx, cfg->sf_idx, nbits->nof_bits);
+
+    /* Bit scrambling */
+    srslte_scrambling_bytes(seq, (uint8_t *) q->e[codeword_idx], nbits->nof_bits);
+
+    /* Bit mapping */
+    srslte_mod_modulate_bytes(&q->mod[mcs->mod],
+                              (uint8_t *) q->e[codeword_idx],
+                              q->d[codeword_idx], nbits->nof_bits);
+
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
+                                               srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
+                                               uint32_t codeword_idx) {
+  srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
+  srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
+  uint32_t rv = cfg->rv[codeword_idx];
+
+  if (nbits->nof_bits) {
+    INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+         cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
+         nbits->nof_re, nbits->nof_bits, rv);
+
+    /* 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
+     */
+    srslte_demod_soft_demodulate_s(mcs->mod, q->d[codeword_idx], q->e[codeword_idx], nbits->nof_re);
+
+    /* Select scrambling sequence */
+    srslte_sequence_t *seq = get_user_sequence(q, rnti, codeword_idx, cfg->sf_idx, nbits->nof_bits);
+
+    /* Bit scrambling */
+    srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
+
+    return srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
 /** Decodes the PDSCH from the received symbols
  */
-int srslte_pdsch_decode_multi(srslte_pdsch_t *q, 
-                              srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
-                              cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, 
-                              uint16_t rnti, uint8_t *data) 
+int srslte_pdsch_decode(srslte_pdsch_t *q,
+                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                        cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                        float noise_estimate, uint16_t rnti, uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                        bool acks[SRSLTE_MAX_CODEWORDS])
 {
 
   /* Set pointers for layermapping & precoding */
-  uint32_t i, n;
+  uint32_t i;
   cf_t *x[SRSLTE_MAX_LAYERS];
-  
+
   if (q            != NULL &&
       sf_symbols   != NULL &&
-      data         != NULL && 
+      data         != NULL &&
       cfg          != NULL)
   {
 
-    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d, C_prb=%d\n",
-        cfg->sf_idx, rnti, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, cfg->nbits.nof_re, 
-         cfg->nbits.nof_bits, cfg->rv, cfg->grant.nof_prb);
+    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, nof_layers=%d, nof_tb=%d\n",
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, cfg->grant.nof_tb);
 
-    /* number of layers equals number of ports */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      x[i] = q->x[i];
-    }
-    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
-      
+    // Extract Symbols and Channel Estimates
     for (int j=0;j<q->nof_rx_antennas;j++) {
-      /* extract symbols */
-      n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
-      if (n != cfg->nbits.nof_re) {
-        fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
+      int n = srslte_pdsch_get(q, sf_symbols[j], q->symbols[j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
+      if (n != cfg->nbits[0].nof_re) {
+        fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
         return SRSLTE_ERROR;
       }
-      
-      /* extract channel estimates */
+
       for (i = 0; i < q->cell.nof_ports; i++) {
-        n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
-        if (n != cfg->nbits.nof_re) {
-          fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits.nof_re, n);
+        n = srslte_pdsch_get(q, ce[i][j], q->ce[i][j], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
+        if (n != cfg->nbits[0].nof_re) {
+          fprintf(stderr, "Error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
           return SRSLTE_ERROR;
         }
-      }      
+      }
     }
-    
-    /* TODO: only diversity is supported */
-    if (q->cell.nof_ports == 1) {
-      /* no need for layer demapping */
-      srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits.nof_re, noise_estimate);
+
+    // Prepare layers
+    int nof_symbols [SRSLTE_MAX_CODEWORDS];
+    nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+    nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+
+    if (cfg->nof_layers == cfg->grant.nof_tb) {
+      /* Skip layer demap */
+      for (i = 0; i < cfg->nof_layers; i++) {
+        x[i] = q->d[i];
+      }
     } else {
-      srslte_predecoding_diversity_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nbits.nof_re);
-      srslte_layerdemap_diversity(x, q->d, q->cell.nof_ports, cfg->nbits.nof_re / q->cell.nof_ports);
-    }
-    
-    if (SRSLTE_VERBOSE_ISDEBUG()) {
-      DEBUG("SAVED FILE subframe.dat: received subframe symbols\n",0);
-      srslte_vec_save_file("subframe.dat", sf_symbols[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-      DEBUG("SAVED FILE hest0.dat and hest1.dat: channel estimates for port 0 and port 1\n",0);
-      srslte_vec_save_file("hest0.dat", ce[0][0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
-      if (q->cell.nof_ports > 1) {
-        srslte_vec_save_file("hest1.dat", ce[1][0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      /* number of layers equals number of ports */
+      for (i = 0; i < cfg->nof_layers; i++) {
+        x[i] = q->x[i];
       }
-      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));
+      memset(&x[cfg->nof_layers], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
     }
-    
-    /* 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
-    */
-    srslte_demod_soft_demodulate_s(cfg->grant.mcs.mod, q->d, q->e, cfg->nbits.nof_re);
 
-    // Generate scrambling sequence if not pre-generated
-    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+    // Pre-decoder
+    srslte_predecoding_type_multi(q->symbols, q->ce, x, q->nof_rx_antennas, q->cell.nof_ports, cfg->nof_layers,
+                                    cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
-    // Run scrambling
-    srslte_scrambling_s_offset(seq, q->e, 0, cfg->nbits.nof_bits);
+    // Layer demapping only if necessary
+    if (cfg->nof_layers != cfg->grant.nof_tb) {
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+                             nof_symbols[0], nof_symbols, cfg->mimo_type);
+    }
 
-    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));
+    // Codeword decoding
+    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
+      int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+      acks[tb] = (ret == SRSLTE_SUCCESS);
     }
 
-    return srslte_dlsch_decode(&q->dl_sch, cfg, softbuffer, q->e, data);
-    
+    pdsch_decode_debug(q, cfg, sf_symbols, ce);
+
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
+
+int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
+                                  srslte_pdsch_cfg_t *cfg,
+                                  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
+                                  uint32_t pmi[SRSLTE_MAX_LAYERS], float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]) {
+
+  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+    for (int nof_layers = 1; nof_layers <= 2; nof_layers++ ) {
+      if (sinr[nof_layers - 1] && pmi) {
+        if (srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, nof_layers, &pmi[nof_layers - 1],
+                                        sinr[nof_layers - 1]) < 0) {
+          ERROR("PMI Select for %d layers", nof_layers);
+          return SRSLTE_ERROR;
+        }
+      }
+    }
   } else {
+    ERROR("Not implemented configuration");
     return SRSLTE_ERROR_INVALID_INPUTS;
   }
+
+  return SRSLTE_SUCCESS;
+}
+
+int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
+                            cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t nof_ce, float *cn) {
+  return srslte_precoding_cn(ce, q->cell.nof_ports, q->nof_rx_antennas, nof_ce, cn);
 }
 
-int srslte_pdsch_encode(srslte_pdsch_t *q, 
-                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
-                        uint8_t *data, uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS]) 
+int srslte_pdsch_encode(srslte_pdsch_t *q,
+                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                        uint8_t *data[SRSLTE_MAX_CODEWORDS], uint16_t rnti, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
 {
-  
+
   int i;
   /* Set pointers for layermapping & precoding */
   cf_t *x[SRSLTE_MAX_LAYERS];
-  int ret = SRSLTE_ERROR_INVALID_INPUTS; 
-   
-   if (q             != NULL &&
-       cfg  != NULL)
-  {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+
+  if (q != NULL &&
+      cfg != NULL) {
 
-    for (i=0;i<q->cell.nof_ports;i++) {
+    for (i = 0; i < q->cell.nof_ports; i++) {
       if (sf_symbols[i] == NULL) {
         return SRSLTE_ERROR_INVALID_INPUTS;
       }
     }
-    
-    if (cfg->grant.mcs.tbs == 0) {
-      return SRSLTE_ERROR_INVALID_INPUTS;      
-    }
-    
-    if (cfg->nbits.nof_re > q->max_re) {
-      fprintf(stderr,
-          "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
-          cfg->nbits.nof_re, q->max_re, q->cell.nof_prb);
+
+    /* If both transport block size is zero return error */
+    if (cfg->grant.mcs[0].tbs == 0) {
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    INFO("Encoding PDSCH SF: %d, Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
-        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs.mod), cfg->grant.mcs.tbs, 
-         cfg->nbits.nof_re, cfg->nbits.nof_bits, cfg->rv);
-
-    /* number of layers equals number of ports */
-    for (i = 0; i < q->cell.nof_ports; i++) {
-      x[i] = q->x[i];
+    if (cfg->nbits[0].nof_re > q->max_re) {
+      fprintf(stderr,
+              "Error too many RE per subframe (%d). PDSCH configured for %d RE (%d PRB)\n",
+              cfg->nbits[0].nof_re, q->max_re, q->cell.nof_prb);
+      return SRSLTE_ERROR_INVALID_INPUTS;
     }
-    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
 
-    if (srslte_dlsch_encode(&q->dl_sch, cfg, softbuffer, data, q->e)) {
-      fprintf(stderr, "Error encoding TB\n");
-      return SRSLTE_ERROR;
+    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
+      ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
     }
 
-    // Generate scrambling sequence if not pre-generated
-    srslte_sequence_t *seq = get_user_sequence(q, rnti, cfg->sf_idx, cfg->nbits.nof_bits);
+    // Layer mapping & precode if necessary
+    if (q->cell.nof_ports > 1) {
+      int nof_symbols;
+      /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
+      if (cfg->nof_layers == cfg->grant.nof_tb) {
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->d[i];
+        }
+        nof_symbols = cfg->nbits[0].nof_re;
+      } else {
+        /* Initialise layer map pointers */
+        for (i = 0; i < cfg->nof_layers; i++) {
+          x[i] = q->x[i];
+        }
+        memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-    srslte_scrambling_bytes(seq, (uint8_t*) q->e, cfg->nbits.nof_bits);
+        nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
+                                           (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits[0].nof_re, cfg->nbits[1].nof_re},
+                                           cfg->mimo_type);
+      }
 
-    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs.mod], (uint8_t*) q->e, q->d, cfg->nbits.nof_bits);
-    
-    /* TODO: only diversity supported */
-    if (q->cell.nof_ports > 1) {
-      srslte_layermap_diversity(q->d, x, q->cell.nof_ports, cfg->nbits.nof_re);
-      srslte_precoding_diversity(x, q->symbols, q->cell.nof_ports,
-          cfg->nbits.nof_re / q->cell.nof_ports);
+      /* Precode */
+      srslte_precoding_type(x, q->symbols, cfg->nof_layers, q->cell.nof_ports, cfg->codebook_idx,
+                            nof_symbols, cfg->mimo_type);
     } else {
-      memcpy(q->symbols[0], q->d, cfg->nbits.nof_re * sizeof(cf_t));
+      memcpy(q->symbols[0], q->d[0], cfg->nbits[0].nof_re * sizeof(cf_t));
     }
 
     /* mapping to resource elements */
     for (i = 0; i < q->cell.nof_ports; i++) {
-      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits.lstart, cfg->sf_idx);
+      srslte_pdsch_put(q, q->symbols[i], sf_symbols[i], &cfg->grant, cfg->nbits[0].lstart, cfg->sf_idx);
     }
-    
+
     ret = SRSLTE_SUCCESS;
-  } 
-  return ret; 
+  }
+  return ret;
 }
 
-float srslte_pdsch_average_noi(srslte_pdsch_t *q) 
-{
+void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter) {
+  srslte_sch_set_max_noi(&q->dl_sch, max_iter);
+}
+
+float srslte_pdsch_average_noi(srslte_pdsch_t *q) {
   return q->dl_sch.average_nof_iterations;
 }
 
diff --git a/lib/src/phy/phch/phich.c b/lib/src/phy/phch/phich.c
index 7e7f0e8c8..a7cb282fd 100644
--- a/lib/src/phy/phch/phich.c
+++ b/lib/src/phy/phch/phich.c
@@ -68,13 +68,8 @@ void srslte_phich_reset(srslte_phich_t *q, cf_t *slot_symbols[SRSLTE_MAX_PORTS])
   }
 }
 
-int srslte_phich_init(srslte_phich_t *q)
-{
-  return srslte_phich_init_multi(q, 1);
-}
-
 /** Initializes the phich channel receiver */
-int srslte_phich_init_multi(srslte_phich_t *q, uint32_t nof_rx_antennas)
+int srslte_phich_init(srslte_phich_t *q, uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index eadf8b911..6b271d02e 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -434,6 +434,10 @@ int srslte_pucch_init(srslte_pucch_t *q) {
     
     srslte_uci_cqi_pucch_init(&q->cqi);
 
+    q->z = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
+    q->z_tmp = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
+    q->ce = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_PUCCH_MAX_SYMBOLS);
+
     q->threshold_format1  = 0.8;
 
     ret = SRSLTE_SUCCESS;
@@ -851,4 +855,4 @@ int srslte_pucch_decode(srslte_pucch_t* q, srslte_pucch_format_t format,
 }
 
 
-  
\ No newline at end of file
+  
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 976a669f4..00713e6c1 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -492,44 +492,44 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       fprintf(stderr, "Error decoding DCI: P/SI/RA-RNTI supports Format1A/1C only\n");
       return SRSLTE_ERROR; 
     }
-    grant->mcs.mod = SRSLTE_MOD_QPSK;      
-    grant->mcs.tbs = (uint32_t) tbs; 
+    grant->mcs[0].mod = SRSLTE_MOD_QPSK;
+    grant->mcs[0].tbs = (uint32_t) tbs;
   } else {
     n_prb = grant->nof_prb;
-    grant->nof_tb = 0; 
     if (dci->tb_en[0]) {
-      grant->mcs.idx = dci->mcs_idx;
-      tbs   = dl_fill_ra_mcs(&grant->mcs, n_prb);
+      grant->mcs[0].idx = dci->mcs_idx;
+      tbs   = dl_fill_ra_mcs(&grant->mcs[0], n_prb);
       if (tbs) {
         last_dl_tbs[dci->harq_process%8] = tbs;
       } else {
         // For mcs>=29, set last TBS received for this PID
-        grant->mcs.tbs = last_dl_tbs[dci->harq_process%8]; 
+        grant->mcs[0].tbs = last_dl_tbs[dci->harq_process%8];
       }
-      grant->nof_tb++;
     } else {
-      grant->mcs.tbs = 0; 
+      grant->mcs[0].tbs = 0;
     }
     if (dci->tb_en[1]) {
-      grant->mcs2.idx = dci->mcs_idx_1;
-      tbs = dl_fill_ra_mcs(&grant->mcs2, n_prb);
+      grant->mcs[1].idx = dci->mcs_idx_1;
+      tbs = dl_fill_ra_mcs(&grant->mcs[1], n_prb);
       if (tbs) {
         last_dl_tbs2[dci->harq_process%8] = tbs;
       } else {
         // For mcs>=29, set last TBS received for this PID
-        grant->mcs2.tbs = last_dl_tbs2[dci->harq_process%8]; 
+        grant->mcs[1].tbs = last_dl_tbs2[dci->harq_process%8];
       }
-      grant->nof_tb++;
     } else {
-      grant->mcs2.tbs = 0; 
+      grant->mcs[1].tbs = 0;
     }
-  }  
-  if (dci->tb_en[0]) {
-    grant->Qm = srslte_mod_bits_x_symbol(grant->mcs.mod);      
   }
-  if (dci->tb_en[1]) {
-    grant->Qm2 = srslte_mod_bits_x_symbol(grant->mcs2.mod);      
+  grant->nof_tb = 0;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (dci->tb_en[tb]) {
+      grant->Qm[tb] = srslte_mod_bits_x_symbol(grant->mcs[tb].mod);
+      grant->nof_tb++;
+    }
   }
+  grant->pinfo = dci->pinfo;
+
   if (tbs < 0) {
     return SRSLTE_ERROR; 
   } else {    
@@ -537,13 +537,17 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
   }
 }
 
-void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx, srslte_ra_nbits_t *nbits) 
+void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srslte_cell_t cell, uint32_t sf_idx,
+                                 srslte_ra_nbits_t nbits [SRSLTE_MAX_CODEWORDS])
 {
   // Compute number of RE 
-  nbits->nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb<10?(cfi+1):cfi);
-  nbits->lstart = cell.nof_prb<10?(cfi+1):cfi;
-  nbits->nof_symb = 2*SRSLTE_CP_NSYMB(cell.cp)-nbits->lstart;
-  nbits->nof_bits = nbits->nof_re * grant->Qm;      
+  for (int i = 0; i < grant->nof_tb; i++) {
+    /* Compute number of RE for first transport block */
+    nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
+    nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
+    nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+    nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+  }
 }
 
 /** Obtains a DL grant from a DCI grant for PDSCH */
@@ -796,18 +800,32 @@ void srslte_ra_pdsch_fprint(FILE *f, srslte_ra_dl_dci_t *dci, uint32_t nof_prb)
     }
     break;
   }
-  fprintf(f, " - Modulation and coding scheme index:\t%d\n", dci->mcs_idx);
   fprintf(f, " - HARQ process:\t\t\t%d\n", dci->harq_process);
-  fprintf(f, " - New data indicator:\t\t\t%s\n", dci->ndi ? "Yes" : "No");
-  fprintf(f, " - Redundancy version:\t\t\t%d\n", dci->rv_idx);
   fprintf(f, " - TPC command for PUCCH:\t\t--\n");
+  fprintf(f, " - Transport blocks swapped:\t\t%s\n", (dci->tb_cw_swap)?"true":"false");
+  fprintf(f, " - Transport block 1 enabled:\t\t%s\n", (dci->tb_en[0])?"true":"false");
+  if (dci->tb_en[0]) {
+    fprintf(f, "   + Modulation and coding scheme index:\t%d\n", dci->mcs_idx);
+    fprintf(f, "   + New data indicator:\t\t\t%s\n", dci->ndi ? "Yes" : "No");
+    fprintf(f, "   + Redundancy version:\t\t\t%d\n", dci->rv_idx);
+  }
+  fprintf(f, " - Transport block 2 enabled:\t\t%s\n", (dci->tb_en[1])?"true":"false");
+  if (dci->tb_en[1]) {
+    fprintf(f, "   + Modulation and coding scheme index:\t%d\n", dci->mcs_idx_1);
+    fprintf(f, "   + New data indicator:\t\t\t%s\n", dci->ndi_1 ? "Yes" : "No");
+    fprintf(f, "   + Redundancy version:\t\t\t%d\n", dci->rv_idx_1);
+  }
 }
 
 void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
-  fprintf(f, " - Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs.mod));
-  fprintf(f, " - Transport block size:\t\t%d\n", grant->mcs.tbs);
+  fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
+  for (int i = 0; i < grant->nof_tb; i++) {
+    fprintf(f, "  - Transport block:\t\t\t%d\n", i);
+    fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
+    fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+  }
 }
 
 void srslte_ra_prb_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index b2c008e6c..e0002429e 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -455,7 +455,7 @@ static int decode_tb(srslte_sch_t *q,
       e_bits       != NULL &&
       cb_segm      != NULL)
   {
-
+    
     if (cb_segm->tbs == 0 || cb_segm->C == 0) {
       return SRSLTE_SUCCESS;
     }
@@ -476,8 +476,8 @@ static int decode_tb(srslte_sch_t *q,
     
     data[cb_segm->tbs/8+0] = 0; 
     data[cb_segm->tbs/8+1] = 0; 
-    data[cb_segm->tbs/8+2] = 0;
-
+    data[cb_segm->tbs/8+2] = 0; 
+    
     // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_NPAR
     for (uint32_t i=0;i<nof_cb_groups && crc_ok;i++) {
       crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i);            
@@ -514,12 +514,22 @@ static int decode_tb(srslte_sch_t *q,
   }
 }
 
-int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer, 
-                        int16_t *e_bits, uint8_t *data) 
-{
-  return decode_tb(q,                    
-                   softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits,
+int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                        int16_t *e_bits, uint8_t *data) {
+  return srslte_dlsch_decode2(q, cfg, softbuffer, e_bits, data, 0);
+}
+
+
+int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                         int16_t *e_bits, uint8_t *data, int codeword_idx) {
+  uint32_t Nl = 1;
+
+  if (cfg->nof_layers != cfg->grant.nof_tb) {
+    Nl = 2;
+  }
+
+  return decode_tb(q, softbuffer, &cfg->cb_segm[codeword_idx],
+                   cfg->grant.Qm[codeword_idx] * Nl, cfg->rv[codeword_idx], cfg->nbits[codeword_idx].nof_bits,
                    e_bits, data);
 }
 
@@ -536,13 +546,22 @@ int srslte_dlsch_decode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuf
 int srslte_dlsch_encode(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
                         uint8_t *data, uint8_t *e_bits) 
 {
-  return encode_tb(q, 
-                   softbuffer, &cfg->cb_segm, 
-                   cfg->grant.Qm, cfg->rv, cfg->nbits.nof_bits,
-                   data, e_bits);
+  return srslte_dlsch_encode2(q, cfg, softbuffer, data, e_bits, 0);
+}
+
+int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
+                              uint8_t *data, uint8_t *e_bits, int codeword_idx) {
+  uint32_t Nl = 1;
+
+  if (cfg->nof_layers != cfg->grant.nof_tb) {
+    Nl = 2;
+  }
+
+  return encode_tb(q, softbuffer, &cfg->cb_segm[codeword_idx], cfg->grant.Qm[codeword_idx]*Nl, cfg->rv[codeword_idx],
+                   cfg->nbits[codeword_idx].nof_bits, data, e_bits);
 }
 
-/* Compute the interleaving function on-the-fly, because it depends on number of RI bits 
+/* Compute the interleaving function on-the-fly, because it depends on number of RI bits
  * Profiling show that the computation of this matrix is neglegible. 
  */
 static void ulsch_interleave_gen(uint32_t H_prime_total, uint32_t N_pusch_symbs, uint32_t Qm,
diff --git a/lib/src/phy/phch/test/CMakeLists.txt b/lib/src/phy/phch/test/CMakeLists.txt
index 6170f82fe..832f18d1f 100644
--- a/lib/src/phy/phch/test/CMakeLists.txt
+++ b/lib/src/phy/phch/test/CMakeLists.txt
@@ -85,7 +85,79 @@ target_link_libraries(pdsch_test srslte_phy)
 add_test(pdsch_test_qpsk pdsch_test -m 10 -n 50 -r 1)
 add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100)
 add_test(pdsch_test_qam16 pdsch_test -m 20 -n 100 -r 2)
-add_test(pdsch_test_qam64 pdsch_test -m 28 -n 100)
+add_test(pdsch_test_qam64 pdsch_test -n 100)
+
+# PDSCH test for single transmision mode and 2 Rx antennas
+add_test(pdsch_test_sin_6   pdsch_test -x single -a 2 -n 6)
+add_test(pdsch_test_sin_12  pdsch_test -x single -a 2 -n 12)
+add_test(pdsch_test_sin_25  pdsch_test -x single -a 2 -n 25)
+add_test(pdsch_test_sin_50  pdsch_test -x single -a 2 -n 50)
+add_test(pdsch_test_sin_75  pdsch_test -x single -a 2 -n 75)
+add_test(pdsch_test_sin_100 pdsch_test -x single -a 2 -n 100)
+
+# PDSCH test for transmit diversity transmision mode (1 codeword)
+add_test(pdsch_test_div_6   pdsch_test -x diversity -a 2 -n 6)
+add_test(pdsch_test_div_12  pdsch_test -x diversity -a 2 -n 12)
+add_test(pdsch_test_div_25  pdsch_test -x diversity -a 2 -n 25)
+add_test(pdsch_test_div_50  pdsch_test -x diversity -a 2 -n 50)
+add_test(pdsch_test_div_75  pdsch_test -x diversity -a 2 -n 75)
+add_test(pdsch_test_div_100 pdsch_test -x diversity -a 2 -n 100)
+
+# PDSCH test for CDD transmision mode (2 codeword)
+add_test(pdsch_test_cdd_6   pdsch_test -x cdd -a 2 -t 0 -n 6)
+add_test(pdsch_test_cdd_12  pdsch_test -x cdd -a 2 -t 0 -n 12)
+add_test(pdsch_test_cdd_25  pdsch_test -x cdd -a 2 -t 0 -n 25)
+add_test(pdsch_test_cdd_50  pdsch_test -x cdd -a 2 -t 0 -n 50)
+add_test(pdsch_test_cdd_75  pdsch_test -x cdd -a 2 -t 0 -n 75)
+add_test(pdsch_test_cdd_100 pdsch_test -x cdd -a 2 -t 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 0 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p0_6   pdsch_test -x multiplex -a 2 -p 0 -n 6)
+add_test(pdsch_test_multiplex1cw_p0_12  pdsch_test -x multiplex -a 2 -p 0 -n 12)
+add_test(pdsch_test_multiplex1cw_p0_25  pdsch_test -x multiplex -a 2 -p 0 -n 25)
+add_test(pdsch_test_multiplex1cw_p0_50  pdsch_test -x multiplex -a 2 -p 0 -n 50)
+add_test(pdsch_test_multiplex1cw_p0_75  pdsch_test -x multiplex -a 2 -p 0 -n 75)
+add_test(pdsch_test_multiplex1cw_p0_100 pdsch_test -x multiplex -a 2 -p 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 1 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p1_6   pdsch_test -x multiplex -a 2 -p 1 -n 6)
+add_test(pdsch_test_multiplex1cw_p1_12  pdsch_test -x multiplex -a 2 -p 1 -n 12)
+add_test(pdsch_test_multiplex1cw_p1_25  pdsch_test -x multiplex -a 2 -p 1 -n 25)
+add_test(pdsch_test_multiplex1cw_p1_50  pdsch_test -x multiplex -a 2 -p 1 -n 50)
+add_test(pdsch_test_multiplex1cw_p1_75  pdsch_test -x multiplex -a 2 -p 1 -n 75)
+add_test(pdsch_test_multiplex1cw_p1_100 pdsch_test -x multiplex -a 2 -p 1 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 2 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p2_6   pdsch_test -x multiplex -a 2 -p 2 -n 6)
+add_test(pdsch_test_multiplex1cw_p2_12  pdsch_test -x multiplex -a 2 -p 2 -n 12)
+add_test(pdsch_test_multiplex1cw_p2_25  pdsch_test -x multiplex -a 2 -p 2 -n 25)
+add_test(pdsch_test_multiplex1cw_p2_50  pdsch_test -x multiplex -a 2 -p 2 -n 50)
+add_test(pdsch_test_multiplex1cw_p2_75  pdsch_test -x multiplex -a 2 -p 2 -n 75)
+add_test(pdsch_test_multiplex1cw_p2_100 pdsch_test -x multiplex -a 2 -p 2 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 3 (1 codeword)
+add_test(pdsch_test_multiplex1cw_p3_6   pdsch_test -x multiplex -a 2 -p 3 -n 6)
+add_test(pdsch_test_multiplex1cw_p3_12  pdsch_test -x multiplex -a 2 -p 3 -n 12)
+add_test(pdsch_test_multiplex1cw_p3_25  pdsch_test -x multiplex -a 2 -p 3 -n 25)
+add_test(pdsch_test_multiplex1cw_p3_50  pdsch_test -x multiplex -a 2 -p 3 -n 50)
+add_test(pdsch_test_multiplex1cw_p3_75  pdsch_test -x multiplex -a 2 -p 3 -n 75)
+add_test(pdsch_test_multiplex1cw_p3_100 pdsch_test -x multiplex -a 2 -p 3 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 0 (2 codeword)
+add_test(pdsch_test_multiplex2cw_p0_6   pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 6)
+add_test(pdsch_test_multiplex2cw_p0_12  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 12)
+add_test(pdsch_test_multiplex2cw_p0_25  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 25)
+add_test(pdsch_test_multiplex2cw_p0_50  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 50)
+add_test(pdsch_test_multiplex2cw_p0_75  pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 75)
+add_test(pdsch_test_multiplex2cw_p0_100 pdsch_test -x multiplex -a 2 -t 0 -p 0 -n 100)
+
+# PDSCH test for Spatial Multiplex transmision mode with PMI = 1 (2 codeword)
+add_test(pdsch_test_multiplex2cw_p1_6   pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 6)
+add_test(pdsch_test_multiplex2cw_p1_12  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 12)
+add_test(pdsch_test_multiplex2cw_p1_25  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 25)
+add_test(pdsch_test_multiplex2cw_p1_50  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 50)
+add_test(pdsch_test_multiplex2cw_p1_75  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 75)
+add_test(pdsch_test_multiplex2cw_p1_100 pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 100)
 
 ########################################################################
 # FILE TEST  
diff --git a/lib/src/phy/phch/test/dlsch_encode_test_mex.c b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
index 8c2318583..c6181d9c5 100644
--- a/lib/src/phy/phch/test/dlsch_encode_test_mex.c
+++ b/lib/src/phy/phch/test/dlsch_encode_test_mex.c
@@ -43,9 +43,14 @@ void help()
 /* the gateway function */
 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 {
+  int i;
   srslte_sch_t dlsch;
   srslte_pdsch_cfg_t cfg; 
-  srslte_softbuffer_tx_t softbuffer; 
+  srslte_softbuffer_tx_t softbuffers[SRSLTE_MAX_CODEWORDS];
+  uint32_t nof_codewords = 1;
+
+  memset(&dlsch, 0, sizeof(srslte_sch_t));
+  memset(&cfg, 0, sizeof(srslte_pdsch_cfg_t));
 
   if (nrhs < NOF_INPUTS) {
     help();
@@ -62,6 +67,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   srslte_verbose = SRSLTE_VERBOSE_NONE; 
   
   uint8_t *trblkin_bits = NULL;
+  cfg.grant.nof_tb = 1;
   cfg.grant.mcs.tbs = mexutils_read_uint8(TRBLKIN, &trblkin_bits);
   if (cfg.grant.mcs.tbs == 0) {
     mexErrMsgTxt("Error trblklen is zero\n");
@@ -76,6 +82,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     return;
   }
 
+  if (mexutils_read_uint32_struct(PUSCHCFG, "NLayers", &cfg.nof_layers)) {
+    mexErrMsgTxt("Field NLayers not found in dlsch config\n");
+    return;
+  }
+
   char *mod_str = mexutils_get_char_struct(PUSCHCFG, "Modulation");
   
   if (!strcmp(mod_str, "QPSK")) {
@@ -94,9 +105,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 
   mxFree(mod_str);
 
-  if (srslte_softbuffer_tx_init(&softbuffer, cell.nof_prb)) {
-    mexErrMsgTxt("Error initiating DL-SCH soft buffer\n");
-    return; 
+  /* Initialise buffers */
+  for (i = 0; i < nof_codewords; i++) {
+    if (srslte_softbuffer_tx_init(&softbuffers[i], cell.nof_prb)) {
+      mexErrMsgTxt("Error initiating DL-SCH soft buffer\n");
+      return;
+    }
   }
 
   cfg.nbits.nof_bits = mxGetScalar(OUTLEN); 
@@ -111,13 +125,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   uint32_t tmp_rv=cfg.rv;
   if (tmp_rv) {
     cfg.rv = 0; 
-    if (srslte_dlsch_encode(&dlsch, &cfg, &softbuffer, trblkin, e_bits)) {
+    if (srslte_dlsch_encode_multi(&dlsch, &cfg, softbuffers, &trblkin, &e_bits)) {
       mexErrMsgTxt("Error encoding TB\n");
       return;
     }
     cfg.rv = tmp_rv;
   }
-  if (srslte_dlsch_encode(&dlsch, &cfg, &softbuffer, trblkin, e_bits)) {
+  if (srslte_dlsch_encode_multi(&dlsch, &cfg, softbuffers, &trblkin, &e_bits)) {
     mexErrMsgTxt("Error encoding TB\n");
     return;
   }
@@ -135,7 +149,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   free(trblkin);
   free(e_bits);
   free(e_bits_unpacked);
-  
+  for (i = 0; i < nof_codewords; i++) {
+    srslte_softbuffer_tx_free(&softbuffers[i]);
+  }
   return;
 }
 
diff --git a/lib/src/phy/phch/test/pcfich_file_test.c b/lib/src/phy/phch/test/pcfich_file_test.c
index bd1bc87c2..dfb8d72e3 100644
--- a/lib/src/phy/phch/test/pcfich_file_test.c
+++ b/lib/src/phy/phch/test/pcfich_file_test.c
@@ -161,7 +161,7 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_pcfich_init(&pcfich)) {
+  if (srslte_pcfich_init(&pcfich, 1)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/pcfich_test.c b/lib/src/phy/phch/test/pcfich_test.c
index d9217586f..32e9925b2 100644
--- a/lib/src/phy/phch/test/pcfich_test.c
+++ b/lib/src/phy/phch/test/pcfich_test.c
@@ -124,7 +124,7 @@ int main(int argc, char **argv) {
       exit(-1);
     }
 
-    if (srslte_pcfich_init(&pcfich)) {
+    if (srslte_pcfich_init(&pcfich, 1)) {
       fprintf(stderr, "Error creating PBCH object\n");
       exit(-1);
     }
diff --git a/lib/src/phy/phch/test/pdcch_file_test.c b/lib/src/phy/phch/test/pdcch_file_test.c
index 4d61e529a..d4ceed4b6 100644
--- a/lib/src/phy/phch/test/pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdcch_file_test.c
@@ -171,7 +171,7 @@ int base_init() {
     fprintf(stderr, "Error setting CFI %d\n", cfi);
     return -1;
   }
-  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
+  if (srslte_pdcch_init_ue(&pdcch, cell.nof_prb, 1)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
diff --git a/lib/src/phy/phch/test/pdcch_test.c b/lib/src/phy/phch/test/pdcch_test.c
index 6b8ee1731..d5c33a709 100644
--- a/lib/src/phy/phch/test/pdcch_test.c
+++ b/lib/src/phy/phch/test/pdcch_test.c
@@ -27,22 +27,26 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
+#include <srslte/phy/common/phy_common.h>
+#include <srslte/phy/phch/ra.h>
+#include <srslte/phy/phch/dci.h>
+#include <srslte/phy/phch/pdcch.h>
 
 #include "srslte/srslte.h"
 
 srslte_cell_t cell = {
-  6,            // nof_prb
-  1,            // nof_ports
-  1,            // cell_id
-  SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+  .nof_prb = 6,
+  .nof_ports = 1,
+  .id = 1,
+  .cp = SRSLTE_CP_NORM,
+  .phich_resources = SRSLTE_PHICH_R_1,
+  .phich_length = SRSLTE_PHICH_NORM
 };
 
 uint32_t cfi = 1;
-bool print_dci_table; 
+uint32_t nof_rx_ant = 1;
+bool print_dci_table;
 
 void usage(char *prog) {
   printf("Usage: %s [cfpndv]\n", prog);
@@ -50,25 +54,29 @@ void usage(char *prog) {
   printf("\t-f cfi [Default %d]\n", cfi);
   printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
   printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-A nof_rx_ant [Default %d]\n", nof_rx_ant);
   printf("\t-d Print DCI table [Default %s]\n", print_dci_table?"yes":"no");
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "cfpndv")) != -1) {
+  while ((opt = getopt(argc, argv, "cfpndvA")) != -1) {
     switch (opt) {
     case 'p':
-      cell.nof_ports = atoi(argv[optind]);
+      cell.nof_ports = (uint32_t) atoi(argv[optind]);
       break;
     case 'f':
-      cfi = atoi(argv[optind]);
+      cfi = (uint32_t) atoi(argv[optind]);
       break;
     case 'n':
-      cell.nof_prb = atoi(argv[optind]);
+      cell.nof_prb = (uint32_t) atoi(argv[optind]);
       break;
     case 'c':
-      cell.id = atoi(argv[optind]);
+      cell.id = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'A':
+      nof_rx_ant = (uint32_t) atoi(argv[optind]);
       break;
     case 'd':
       print_dci_table = true;
@@ -85,25 +93,26 @@ void parse_args(int argc, char **argv) {
 
 int test_dci_payload_size() {
   int i, j;
-  int x[4];
-  const srslte_dci_format_t formats[4] = { SRSLTE_DCI_FORMAT0, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1C };
+  int x[5];
+  const srslte_dci_format_t formats[] = { SRSLTE_DCI_FORMAT0, SRSLTE_DCI_FORMAT1, SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1C, SRSLTE_DCI_FORMAT2A };
   const int prb[6] = { 6, 15, 25, 50, 75, 100 };
-  const int dci_sz[6][5] = { { 21, 19, 21, 8 }, { 22, 23, 22, 10 }, { 25, 27,
-      25, 12 }, { 27, 31, 27, 13 }, { 27, 33, 27, 14 }, { 28, 39, 28, 15 } };
+  const int dci_sz[6][5] = { { 21, 19, 21, 8, 28 }, { 22, 23, 22, 10 , 31}, { 25, 27,
+      25, 12 , 36}, { 27, 31, 27, 13 , 41}, { 27, 33, 27, 14 , 42}, { 28, 39, 28, 15, 48 }};
+
 
   printf("Testing DCI payload sizes...\n");
-  printf("  PRB\t0\t1\t1A\t1C\n");
+  printf("  PRB\t0\t1\t1A\t1C\t2A\n");
   for (i = 0; i < 6; i++) {
     int n = prb[i];
-    for (j = 0; j < 4; j++) {
-      x[j] = srslte_dci_format_sizeof(formats[j], n, 1);
+    for (j = 0; j < 5; j++) {
+      x[j] = srslte_dci_format_sizeof(formats[j], (uint32_t) n, 1);
       if (x[j] != dci_sz[i][j]) {
         fprintf(stderr, "Invalid DCI payload size for %s\n",
             srslte_dci_format_string(formats[j]));
         return -1;
       }
     }
-    printf("  %2d:\t%2d\t%2d\t%2d\t%2d\n", n, x[0], x[1], x[2], x[3]);
+    printf("  %2d:\t%2d\t%2d\t%2d\t%2d\t%2d\n", n, x[0], x[1], x[2], x[3], x[4]);
   }
   printf("Ok\n");
   
@@ -111,8 +120,8 @@ int test_dci_payload_size() {
     printf("dci_sz_table[101][4] = {\n");
     for (i=0;i<=100;i++) {
       printf("  {");
-      for (int j=0;j<4;j++) {
-        printf("%d",srslte_dci_format_sizeof(formats[j], i, 1));
+      for (j=0;j<4;j++) {
+        printf("%d",srslte_dci_format_sizeof(formats[j], (uint32_t) i, 1));
         if (j<3) {
           printf(", ");
         }
@@ -128,16 +137,23 @@ int test_dci_payload_size() {
   return 0;
 }
 
+typedef struct {
+  srslte_dci_msg_t dci_tx, dci_rx;
+  srslte_dci_location_t dci_location;
+  srslte_dci_format_t dci_format;
+  srslte_ra_dl_dci_t ra_dl_tx;
+  srslte_ra_dl_dci_t ra_dl_rx;
+} testcase_dci_t;
+
 int main(int argc, char **argv) {
-  srslte_pdcch_t pdcch;
-  srslte_dci_msg_t dci_tx[2], dci_rx[2], dci_tmp;
-  srslte_dci_location_t dci_locations[2];
+  srslte_pdcch_t pdcch_tx, pdcch_rx;
+  testcase_dci_t testcases[10] = {0};
   srslte_ra_dl_dci_t ra_dl;
   srslte_regs_t regs;
-  int i, j;
-  cf_t *ce[SRSLTE_MAX_PORTS];
+  int i, j, k;
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   int nof_re;
-  cf_t *slot_symbols[SRSLTE_MAX_PORTS];
+  cf_t *tx_slot_symbols[SRSLTE_MAX_PORTS], *rx_slot_symbols[SRSLTE_MAX_PORTS];
   int nof_dcis; 
 
   int ret = -1;
@@ -152,19 +168,30 @@ int main(int argc, char **argv) {
 
   /* init memory */
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    ce[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!ce[i]) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      ce[i][j] = malloc(sizeof(cf_t) * nof_re);
+      if (!ce[i][j]) {
+        perror("malloc");
+        exit(-1);
+      }
+      for (k = 0; k < nof_re; k++) {
+        //ce[i][j][k] = (i == j) ? 1 : 0;
+        ce[i][j][k] = ((float)rand()/(float)RAND_MAX) + _Complex_I*((float)rand()/(float)RAND_MAX);
+      }
+    }
+    tx_slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
+    if (!tx_slot_symbols[i]) {
       perror("malloc");
       exit(-1);
     }
-    for (j = 0; j < nof_re; j++) {
-      ce[i][j] = 1;
-    }
-    slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!slot_symbols[i]) {
+    bzero(tx_slot_symbols[i], sizeof(cf_t) * nof_re);
+
+    rx_slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
+    if (!rx_slot_symbols[i]) {
       perror("malloc");
       exit(-1);
     }
+    bzero(rx_slot_symbols[i], sizeof(cf_t) * nof_re);
   }
 
   if (srslte_regs_init(&regs, cell)) {
@@ -177,16 +204,26 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  if (srslte_pdcch_init(&pdcch, cell.nof_prb)) {
+  if (srslte_pdcch_init_enb(&pdcch_tx, cell.nof_prb)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
-  if (srslte_pdcch_set_cell(&pdcch, &regs, cell)) {
+  if (srslte_pdcch_set_cell(&pdcch_tx, &regs, cell)) {
+    fprintf(stderr, "Error setting cell in PDCCH object\n");
+    exit(-1);
+  }
+
+  if (srslte_pdcch_init_ue(&pdcch_rx, cell.nof_prb, nof_rx_ant)) {
     fprintf(stderr, "Error creating PDCCH object\n");
     exit(-1);
   }
+  if (srslte_pdcch_set_cell(&pdcch_rx, &regs, cell)) {
+    fprintf(stderr, "Error setting cell in PDCCH object\n");
+    exit(-1);
+  }
 
-  nof_dcis = 2;
+  /* Resource allocate init */
+  nof_dcis = 0;
   bzero(&ra_dl, sizeof(srslte_ra_dl_dci_t));
   ra_dl.harq_process = 0;
   ra_dl.mcs_idx = 5;
@@ -194,62 +231,131 @@ int main(int argc, char **argv) {
   ra_dl.rv_idx = 0;
   ra_dl.alloc_type = SRSLTE_RA_ALLOC_TYPE0;
   ra_dl.type0_alloc.rbg_bitmask = 0x5;
+  ra_dl.tb_en[0] = true;
 
-  srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_tx[0], cell.nof_prb, cell.nof_ports, false);
-  srslte_dci_location_set(&dci_locations[0], 0, 0);
+  /* Format 1 Test case */
+  testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT1;
+  testcases[nof_dcis].ra_dl_tx = ra_dl;
+  nof_dcis++;
 
+  /* Format 1 Test case */
   ra_dl.mcs_idx = 15;
-  srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_tx[1], cell.nof_prb, cell.nof_ports, false);
-  srslte_dci_location_set(&dci_locations[1], 0, 1);
-  
+  testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT1;
+  testcases[nof_dcis].ra_dl_tx = ra_dl;
+  nof_dcis++;
+
+  /* Tx Diversity Test case */
+  if (cell.nof_ports > 1) {
+    ra_dl.mcs_idx_1 = 0;
+    ra_dl.rv_idx_1 = 0;
+    ra_dl.ndi_1 = false;
+    ra_dl.tb_en[1] = false;
+    testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT2A;
+    testcases[nof_dcis].ra_dl_tx = ra_dl;
+    nof_dcis++;
+  }
+
+  /* CDD Spatial Multiplexing Test case */
+  if (cell.nof_ports > 1) {
+    ra_dl.mcs_idx_1 = 28;
+    ra_dl.rv_idx_1 = 1;
+    ra_dl.ndi_1 = false;
+    ra_dl.tb_en[1] = true;
+    testcases[nof_dcis].dci_format = SRSLTE_DCI_FORMAT2A;
+    testcases[nof_dcis].ra_dl_tx = ra_dl;
+    nof_dcis++;
+  }
+
+  /* Execute Rx */
   for (i=0;i<nof_dcis;i++) {
-    if (srslte_pdcch_encode(&pdcch, &dci_tx[i], dci_locations[i], 1234+i, slot_symbols, 0, cfi)) {
+    srslte_dci_msg_pack_pdsch(&testcases[i].ra_dl_tx, testcases[i].dci_format, &testcases[i].dci_tx,
+                              cell.nof_prb, cell.nof_ports, false);
+    srslte_dci_location_set(&testcases[i].dci_location, 0, (uint32_t) i);
+
+    if (srslte_pdcch_encode(&pdcch_tx, &testcases[i].dci_tx, testcases[i].dci_location, (uint16_t) (1234 + i),
+                            tx_slot_symbols, 0, cfi)) {
       fprintf(stderr, "Error encoding DCI message\n");
       goto quit;
     }
   }
 
-  srslte_vec_fprint_b(stdout, dci_tx[0].data, dci_tx[0].nof_bits);
-  /* combine outputs */
-  for (i = 1; i < cell.nof_ports; i++) {
-    for (j = 0; j < nof_re; j++) {
-      slot_symbols[0][j] += slot_symbols[i][j];
+  /* Apply channel */
+  for (j = 0; j < nof_rx_ant; j++) {
+    for (k = 0; k < nof_re; k++) {
+      for (i = 0; i < cell.nof_ports; i++) {
+        rx_slot_symbols[j][k] += tx_slot_symbols[i][k]*ce[i][j][k];
+      }
     }
   }
 
-  for (i=0;i<2;i++) {
-    if (srslte_pdcch_extract_llr(&pdcch, slot_symbols[0], ce, 0, 0, cfi)) {
-      fprintf(stderr, "Error extracting LLRs\n");
+  /* Execute 'Rx' */
+  if (srslte_pdcch_extract_llr_multi(&pdcch_rx, rx_slot_symbols, ce, 0, 0, cfi)) {
+    fprintf(stderr, "Error extracting LLRs\n");
+    goto quit;
+  }
+
+  /* Decode DCIs */
+  for (i=0;i<nof_dcis;i++) {
+    uint16_t crc_rem;
+    if (srslte_pdcch_decode_msg(&pdcch_rx, &testcases[i].dci_rx, &testcases[i].dci_location, testcases[i].dci_format, &crc_rem)) {
+      fprintf(stderr, "Error decoding DCI message\n");
       goto quit;
     }
-    uint16_t crc_rem; 
-    if (srslte_pdcch_decode_msg(&pdcch, &dci_tmp, &dci_locations[i], SRSLTE_DCI_FORMAT1, &crc_rem)) {
-      fprintf(stderr, "Error decoding DCI message\n");
+    if (srslte_dci_msg_unpack_pdsch(&testcases[i].dci_rx, &testcases[i].ra_dl_rx, cell.nof_prb, cell.nof_ports, false)) {
+      fprintf(stderr, "Error unpacking DCI message\n");
       goto quit;
-    }      
+    }
     if (crc_rem >= 1234 && crc_rem < 1234 + nof_dcis) {
       crc_rem -= 1234;
-        memcpy(&dci_rx[crc_rem], &dci_tmp, sizeof(srslte_dci_msg_t));
     } else {
       printf("Received invalid DCI CRC 0x%x\n", crc_rem);
       goto quit;
     }
   }
+
+  /* Compare Tx and Rx */
   for (i = 0; i < nof_dcis; i++) {
-    if (memcmp(dci_tx[i].data, dci_rx[i].data, dci_tx[i].nof_bits)) {
+    if (memcmp(testcases[i].dci_tx.data, testcases[i].dci_rx.data, testcases[i].dci_tx.nof_bits)) {
       printf("Error in DCI %d: Received data does not match\n", i);
       goto quit;
     }
+    if (memcmp(&testcases[i].ra_dl_tx, &testcases[i].ra_dl_rx, sizeof(srslte_ra_dl_dci_t))) {
+      printf("Error in RA %d: Received data does not match\n", i);
+      printf("     Field    |    Tx    |    Rx    \n");
+      printf("--------------+----------+----------\n");
+      printf(" harq_process | %8d | %8d\n", testcases[i].ra_dl_tx.harq_process, testcases[i].ra_dl_rx.harq_process);
+      printf("      mcs_idx | %8d | %8d\n", testcases[i].ra_dl_tx.mcs_idx, testcases[i].ra_dl_rx.mcs_idx);
+      printf("       rv_idx | %8d | %8d\n", testcases[i].ra_dl_tx.rv_idx, testcases[i].ra_dl_rx.rv_idx);
+      printf("          ndi | %8d | %8d\n", testcases[i].ra_dl_tx.ndi, testcases[i].ra_dl_rx.ndi);
+      printf("    mcs_idx_1 | %8d | %8d\n", testcases[i].ra_dl_tx.mcs_idx_1, testcases[i].ra_dl_rx.mcs_idx_1);
+      printf("     rv_idx_1 | %8d | %8d\n", testcases[i].ra_dl_tx.rv_idx_1, testcases[i].ra_dl_rx.rv_idx_1);
+      printf("        ndi_1 | %8d | %8d\n", testcases[i].ra_dl_tx.ndi_1, testcases[i].ra_dl_rx.ndi_1);
+      printf("   tb_cw_swap | %8d | %8d\n", testcases[i].ra_dl_tx.tb_cw_swap, testcases[i].ra_dl_rx.tb_cw_swap);
+      printf("      sram_id | %8d | %8d\n", testcases[i].ra_dl_tx.sram_id, testcases[i].ra_dl_rx.sram_id);
+      printf("        pinfo | %8d | %8d\n", testcases[i].ra_dl_tx.pinfo, testcases[i].ra_dl_rx.pinfo);
+      printf("        pconf | %8d | %8d\n", testcases[i].ra_dl_tx.pconf, testcases[i].ra_dl_rx.pconf);
+      printf(" power_offset | %8d | %8d\n", testcases[i].ra_dl_tx.power_offset, testcases[i].ra_dl_rx.power_offset);
+      printf("    tpc_pucch | %8d | %8d\n", testcases[i].ra_dl_tx.tpc_pucch, testcases[i].ra_dl_rx.tpc_pucch);
+      printf("     tb_en[0] | %8d | %8d\n", testcases[i].ra_dl_tx.tb_en[0], testcases[i].ra_dl_rx.tb_en[0]);
+      printf("     tb_en[1] | %8d | %8d\n", testcases[i].ra_dl_tx.tb_en[1], testcases[i].ra_dl_rx.tb_en[1]);
+      printf("    dci_is_1a | %8d | %8d\n", testcases[i].ra_dl_tx.dci_is_1a, testcases[i].ra_dl_rx.dci_is_1a);
+      printf("    dci_is_1c | %8d | %8d\n", testcases[i].ra_dl_tx.dci_is_1c, testcases[i].ra_dl_rx.dci_is_1c);
+      goto quit;
+    }
   }
   ret = 0;
 
 quit: 
-  srslte_pdcch_free(&pdcch);
+  srslte_pdcch_free(&pdcch_tx);
+  srslte_pdcch_free(&pdcch_rx);
   srslte_regs_free(&regs);
 
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
-    free(ce[i]);
-    free(slot_symbols[i]);
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      free(ce[i][j]);
+    }
+    free(tx_slot_symbols[i]);
+    free(rx_slot_symbols[i]);
   }
   if (ret) {
     printf("Error\n");
diff --git a/lib/src/phy/phch/test/pdcch_test_mex.c b/lib/src/phy/phch/test/pdcch_test_mex.c
index e30ef900f..76b0bb021 100644
--- a/lib/src/phy/phch/test/pdcch_test_mex.c
+++ b/lib/src/phy/phch/test/pdcch_test_mex.c
@@ -33,13 +33,14 @@
 
 #define ENBCFG  prhs[0]
 #define RNTI    prhs[1]
-#define INPUT   prhs[2]
-#define NOF_INPUTS 3
+#define AMP     prhs[2]
+#define INPUT   prhs[3]
+#define NOF_INPUTS 4
 
 
 
-srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1}; // SRSLTE_DCI_FORMAT1B should go here also
-const uint32_t nof_ue_formats = 2; 
+srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1,SRSLTE_DCI_FORMAT2B}; // SRSLTE_DCI_FORMAT1B should go here also
+const uint32_t nof_ue_formats = 3; 
 
 srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
@@ -162,7 +163,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   } else {
     noise_power = srslte_chest_dl_get_noise_estimate(&chest);
   }
-  mexPrintf("noise power=%f, RNTI=0x%x, cfi=%d\n", noise_power, rnti, cfi);
+  
+  float amplitude = mxGetScalar(AMP);
+  
+  srslte_viterbi_set_gain_quant(&pdcch.decoder, amplitude);
   
   srslte_pdcch_extract_llr(&pdcch, input_fft, ce, noise_power, sf_idx, cfi);
   
diff --git a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
index d6bd10018..90c0e1c17 100644
--- a/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
+++ b/lib/src/phy/phch/test/pdsch_pdcch_file_test.c
@@ -54,7 +54,7 @@ uint32_t sf_idx = 0;
 srslte_dci_format_t dci_format = SRSLTE_DCI_FORMAT1A;
 srslte_filesource_t fsrc;
 srslte_ue_dl_t ue_dl;
-cf_t *input_buffer;
+cf_t *input_buffer[SRSLTE_MAX_PORTS];
 
 void usage(char *prog) {
   printf("Usage: %s [rovfcenmps] -i input_file\n", prog);
@@ -131,13 +131,13 @@ int base_init() {
 
   flen = 2 * (SRSLTE_SLOT_LEN(srslte_symbol_sz(cell.nof_prb)));
 
-  input_buffer = malloc(flen * sizeof(cf_t));
-  if (!input_buffer) {
+  input_buffer[0] = malloc(flen * sizeof(cf_t));
+  if (!input_buffer[0]) {
     perror("malloc");
     exit(-1);
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, cell.nof_prb, 1)) {
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, 1)) {
     fprintf(stderr, "Error initializing UE DL\n");
     return -1;
   }
@@ -155,12 +155,13 @@ int base_init() {
 void base_free() {
   srslte_filesource_free(&fsrc);
   srslte_ue_dl_free(&ue_dl);
-  free(input_buffer);  
+  free(input_buffer[0]);
 }
 
 int main(int argc, char **argv) {
   int nof_frames;
   int ret;
+  bool acks[SRSLTE_MAX_TB];
 
   if (argc < 3) {
     usage(argv[0]);
@@ -173,15 +174,15 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  uint8_t *data = malloc(100000);
+  uint8_t *data[] = {malloc(100000)};
 
   ret = -1;
   nof_frames = 0;
   do {
-    srslte_filesource_read(&fsrc, input_buffer, flen);
+    srslte_filesource_read(&fsrc, input_buffer[0], flen);
     INFO("Reading %d samples sub-frame %d\n", flen, sf_idx);
 
-    ret = srslte_ue_dl_decode(&ue_dl, input_buffer, data, sf_idx); 
+    ret = srslte_ue_dl_decode(&ue_dl, input_buffer, data, 0, sf_idx, acks);
     if(ret > 0) {
       printf("PDSCH Decoded OK!\n");       
     } else if (ret == 0) {
@@ -194,7 +195,8 @@ int main(int argc, char **argv) {
   } while (nof_frames <= max_frames && ret == 0);
 
   base_free();
-  if (ret > 0) {        
+  free(data[0]);
+  if (ret > 0) {
     exit(0);
   } else {
     exit(-1); 
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index 34150f4db..e820f677a 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -27,60 +27,80 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <strings.h>
 #include <unistd.h>
 #include <sys/time.h>
+#include <srslte/phy/phch/ra.h>
 
 #include "srslte/srslte.h"
 
 // Enable to measure execution time
 //#define DO_OFDM
 
+#ifdef DO_OFDM
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_PRB(cell.nof_prb)
+#else
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp)
+#endif
+
 srslte_cell_t cell = {
   6,            // nof_prb
   1,            // nof_ports
   0,            // cell_id
   SRSLTE_CP_NORM,       // cyclic prefix
-  SRSLTE_PHICH_R_1_6,          // PHICH resources      
-  SRSLTE_PHICH_NORM    // PHICH length
+  SRSLTE_PHICH_NORM,    // PHICH length
+  SRSLTE_PHICH_R_1_6    // PHICH resources
 };
 
+char mimo_type_str [32] = "single";
+srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
 uint32_t cfi = 2;
-uint32_t mcs = 0;
+uint32_t mcs[SRSLTE_MAX_CODEWORDS] = {0, 0};
 uint32_t subframe = 1;
-uint32_t rv_idx = 0;
-uint16_t rnti = 1234; 
+int rv_idx[SRSLTE_MAX_CODEWORDS] = {0, 1};
+uint16_t rnti = 1234;
+uint32_t nof_rx_antennas = 1;
+uint32_t pmi = 0;
 char *input_file = NULL; 
 
 void usage(char *prog) {
-  printf("Usage: %s [fmcsrRFpnv] \n", prog);
+  printf("Usage: %s [fmMcsrtRFpnwav] \n", prog);
   printf("\t-f read signal from file [Default generate it with pdsch_encode()]\n");
-  printf("\t-m MCS [Default %d]\n", mcs);
+  printf("\t-m MCS [Default %d]\n", mcs[0]);
+  printf("\t-M MCS2 [Default %d]\n", mcs[1]);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-s subframe [Default %d]\n", subframe);
-  printf("\t-r rv_idx [Default %d]\n", rv_idx);
+  printf("\t-r rv_idx [Default %d]\n", rv_idx[0]);
+  printf("\t-t rv_idx2 [Default %d]\n", rv_idx[1]);
   printf("\t-R rnti [Default %d]\n", rnti);
   printf("\t-F cfi [Default %d]\n", cfi);
-  printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
+  printf("\t-x Transmission mode [single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
   printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-a nof_rx_antennas [Default %d]\n", nof_rx_antennas);
+  printf("\t-p pmi (multiplex only)  [Default %d]\n", pmi);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "fmcsrRFpnv")) != -1) {
+  while ((opt = getopt(argc, argv, "fmMcsrtRFpnavx")) != -1) {
     switch(opt) {
     case 'f':
       input_file = argv[optind];
       break;
     case 'm':
-      mcs = atoi(argv[optind]);
+      mcs[0] = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'M':
+      mcs[1] = (uint32_t) atoi(argv[optind]);
       break;
     case 's':
       subframe = atoi(argv[optind]);
       break;
     case 'r':
-      rv_idx = atoi(argv[optind]);
+      rv_idx[0] = (uint32_t) atoi(argv[optind]);
+      break;
+    case 't':
+      rv_idx[1] = (uint32_t) atoi(argv[optind]);
       break;
     case 'R':
       rnti = atoi(argv[optind]);
@@ -88,8 +108,11 @@ void parse_args(int argc, char **argv) {
     case 'F':
       cfi = atoi(argv[optind]);
       break;
+    case 'x':
+      strncpy(mimo_type_str, argv[optind], 32);
+      break;
     case 'p':
-      cell.nof_ports = atoi(argv[optind]);
+      pmi = (uint32_t) atoi(argv[optind]);
       break;
     case 'n':
       cell.nof_prb = atoi(argv[optind]);
@@ -97,6 +120,9 @@ void parse_args(int argc, char **argv) {
     case 'c':
       cell.id = atoi(argv[optind]);
       break;
+    case 'a':
+      nof_rx_antennas = (uint32_t) atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -107,199 +133,399 @@ void parse_args(int argc, char **argv) {
   }
 }
 
-uint8_t *data = NULL;
-cf_t *ce[SRSLTE_MAX_PORTS];
-srslte_softbuffer_rx_t softbuffer_rx;
+static uint8_t *data_tx[SRSLTE_MAX_CODEWORDS] = {NULL};
+static uint8_t *data_rx[SRSLTE_MAX_CODEWORDS] = {NULL};
+cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+srslte_softbuffer_rx_t *softbuffers_rx[SRSLTE_MAX_CODEWORDS];
 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;
+#ifdef DO_OFDM
+cf_t *tx_sf_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_sf_symbols[SRSLTE_MAX_PORTS];
+#endif /* DO_OFDM */
+cf_t *tx_slot_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_slot_symbols[SRSLTE_MAX_PORTS];
+srslte_pdsch_t pdsch_tx, pdsch_rx;
 srslte_ofdm_t ofdm_tx, ofdm_rx; 
 
 int main(int argc, char **argv) {
-  uint32_t i, j;
+  uint32_t i, j, k;
   int ret = -1;
   struct timeval t[3];
-  srslte_softbuffer_tx_t softbuffer_tx;
-  
+  srslte_softbuffer_tx_t *softbuffers_tx[SRSLTE_MAX_CODEWORDS];
+  int M=1;
+  bool acks[SRSLTE_MAX_CODEWORDS] = {false};
+
   parse_args(argc,argv);
 
-  bzero(&pdsch, sizeof(srslte_pdsch_t));
+  /* Initialise to zeros */
+  bzero(&pdsch_tx, sizeof(srslte_pdsch_t));
+  bzero(&pdsch_rx, sizeof(srslte_pdsch_t));
   bzero(&pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  bzero(slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
-  bzero(&softbuffer_rx, sizeof(srslte_softbuffer_rx_t));
-  bzero(&softbuffer_tx, sizeof(srslte_softbuffer_tx_t));
-  
+  bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+
+  /* Parse transmission mode */
+  if (srslte_str2mimotype(mimo_type_str, &mimo_type)) {
+    ERROR("Wrong transmission mode.");
+    goto quit;
+  }
+
+  switch(mimo_type) {
+
+    case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+      cell.nof_ports = 1;
+      break;
+    case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+    case SRSLTE_MIMO_TYPE_CDD:
+      if (nof_rx_antennas < 2) {
+        ERROR("At least two receiving antennas are required");
+        goto quit;
+      }
+    case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+    default:
+      cell.nof_ports = 2;
+      break;
+  }
+
   srslte_ra_dl_dci_t dci;
   bzero(&dci, sizeof(srslte_ra_dl_dci_t));
-  dci.mcs_idx = mcs;
-  dci.rv_idx = rv_idx;
   dci.type0_alloc.rbg_bitmask = 0xffffffff;
-  dci.tb_en[0] = true; 
+
+  /* If transport block 0 is enabled */
+  if (mcs[0] != 0 || rv_idx[0] != 1) {
+    dci.mcs_idx = mcs[0];
+    dci.rv_idx = rv_idx[0];
+    dci.tb_en[0] = true;
+  }
+
+  /* If transport block 0 is disabled */
+  if (mcs[1] != 0 || rv_idx[1] != 1) {
+    dci.mcs_idx_1 = mcs[1];
+    dci.rv_idx_1 = rv_idx[1];
+    dci.tb_en[1] = true;
+  }
+
+  /* Generate grant from DCI */
   if (srslte_ra_dl_dci_to_grant(&dci, cell.nof_prb, rnti, &grant)) {
     fprintf(stderr, "Error computing resource allocation\n");
     return ret;
   }
-  
+
+#ifdef DO_OFDM
   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));
-  
+  srslte_ofdm_set_normalize(&ofdm_tx, true);
+  srslte_ofdm_set_normalize(&ofdm_rx, true);
+
+  for (i = 0; i < cell.nof_ports; i++) {
+    tx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+
+  for (i = 0; i < nof_rx_antennas; i++) {
+    rx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+#endif /* DO_OFDM */
+
   /* Configure PDSCH */
-  if (srslte_pdsch_cfg(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx)) {
+  if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, subframe, rv_idx, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
-    exit(-1);
+    goto quit;
   }
 
   /* init memory */
   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");
-      goto quit;
-    }
-    for (j=0;j<SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);j++) {
-      ce[i][j] = 1;
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * NOF_CE_SYMBOLS);
+      if (!ce[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+        ce[i][j][k] = (i == j) ? 1.0f : 0.0f;
+      }
     }
-    slot_symbols[i] = srslte_vec_malloc(sizeof(cf_t)*SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
-    if (!slot_symbols[i]) {
+    rx_slot_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
+    if (!rx_slot_symbols[i]) {
       perror("srslte_vec_malloc");
       goto quit;
     }
   }
-  
-  data = srslte_vec_malloc(sizeof(uint8_t) * (grant.mcs.tbs/8)+24);
-  if (!data) {
-    perror("srslte_vec_malloc");
-    goto quit;
+
+
+  for (int i = 0; i < grant.nof_tb; i++) {
+    if (grant.mcs[i].tbs) {
+      data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_tx[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data_tx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
+      data_rx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_rx[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data_rx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
+    }
   }
 
-  if (srslte_pdsch_init_ue(&pdsch, cell.nof_prb)) {
+  if (srslte_pdsch_init_ue(&pdsch_rx, cell.nof_prb, nof_rx_antennas)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
-  if (srslte_pdsch_set_cell(&pdsch, cell)) {
+  if (srslte_pdsch_set_cell(&pdsch_rx, cell)) {
     fprintf(stderr, "Error creating PDSCH object\n");
     goto quit;
   }
 
-  srslte_pdsch_set_rnti(&pdsch, rnti);
-  
-  if (srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb)) {
-    fprintf(stderr, "Error initiating RX soft buffer\n");
-    goto quit;
+  srslte_pdsch_set_rnti(&pdsch_rx, rnti);
+
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    softbuffers_rx[i] = calloc(sizeof(srslte_softbuffer_rx_t), 1);
+    if (!softbuffers_rx[i]) {
+      fprintf(stderr, "Error allocating RX soft buffer\n");
+      goto quit;
+    }
+
+    if (srslte_softbuffer_rx_init(softbuffers_rx[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating RX soft buffer\n");
+      goto quit;
+    }
+  }
+
+  INFO(" Global:\n");
+  INFO("         nof_prb=%d\n", cell.nof_prb);
+  INFO("       nof_ports=%d\n", cell.nof_ports);
+  INFO("              id=%d\n", cell.id);
+  INFO("              cp=%s\n", srslte_cp_string(cell.cp));
+  INFO("    phich_length=%d\n", (int) cell.phich_length);
+  INFO(" phich_resources=%d\n", (int) cell.phich_resources);
+  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
+  INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
+  INFO("       mimo_type=%s\n", srslte_mimotype2str(pdsch_cfg.mimo_type));
+  INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
+  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    INFO(" Tranport block index %d:\n", i);
+    INFO("              Qm=%d\n", pdsch_cfg.grant.Qm[i]);
+    INFO("         mcs.idx=0x%X\n", pdsch_cfg.grant.mcs[i].idx);
+    INFO("         mcs.tbs=%d\n", pdsch_cfg.grant.mcs[i].tbs);
+    INFO("         mcs.mod=%s\n", srslte_mod_string(pdsch_cfg.grant.mcs[i].mod));
+    INFO("              rv=%d\n", pdsch_cfg.rv[i]);
+    INFO("          lstart=%d\n", pdsch_cfg.nbits[i].lstart);
+    INFO("        nof_bits=%d\n", pdsch_cfg.nbits[i].nof_bits);
+    INFO("          nof_re=%d\n", pdsch_cfg.nbits[i].nof_re);
+    INFO("        nof_symb=%d\n", pdsch_cfg.nbits[i].nof_symb);
   }
 
   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);
+      goto quit;
     }
 #ifdef DO_OFDM
-    srslte_filesource_read(&fsrc, sf_symbols, SRSLTE_SF_LEN_PRB(cell.nof_prb));
+    srslte_filesource_read(&fsrc, rx_slot_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));
+    srslte_filesource_read(&fsrc, rx_slot_symbols[0], SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
 #endif
     
-    srslte_chest_dl_t chest; 
+    srslte_chest_dl_t chest;
     if (srslte_chest_dl_init(&chest, cell.nof_prb)) {
-      printf("Error initializing equalizer\n");
+      fprintf(stderr, "Error initializing equalizer\n");
       exit(-1);
     }
     if (srslte_chest_dl_set_cell(&chest, cell)) {
       printf("Error initializing equalizer\n");
       exit(-1);
     }
-    srslte_chest_dl_estimate(&chest, slot_symbols[0], ce, subframe);
+    srslte_chest_dl_estimate(&chest, rx_slot_symbols[0], ce[0], subframe);
     srslte_chest_dl_free(&chest);
     
     srslte_filesource_free(&fsrc);
-  } else { 
-    if (srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb)) {
-      fprintf(stderr, "Error initiating TX soft buffer\n");
+  } else {
+
+    if (srslte_pdsch_init_enb(&pdsch_tx, cell.nof_prb)) {
+      fprintf(stderr, "Error creating PDSCH object\n");
       goto quit;
     }
+    if (srslte_pdsch_set_cell(&pdsch_tx, cell)) {
+      fprintf(stderr, "Error creating PDSCH object\n");
+      goto quit;
+    }
+
+    srslte_pdsch_set_rnti(&pdsch_tx, rnti);
+
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      softbuffers_tx[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
+      if (!softbuffers_tx[i]) {
+        fprintf(stderr, "Error allocating TX soft buffer\n");
+      }
 
-    for (i=0;i<grant.mcs.tbs/8;i++) {
-      data[i] = rand()%256;
+      if (srslte_softbuffer_tx_init(softbuffers_tx[i], cell.nof_prb)) {
+        fprintf(stderr, "Error initiating TX soft buffer\n");
+        goto quit;
+      }
     }
-    
-    uint8_t databit[100000];
+
+    for (i = 0; i < cell.nof_ports; i++) {
+      tx_slot_symbols[i] = calloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp), sizeof(cf_t));
+      if (!tx_slot_symbols[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+    }
+
+    for (int tb = 0; tb < grant.nof_tb; tb++) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        data_tx[tb][byte] = (uint8_t)(rand() % 256);
+      }
+    }
+
+    /*uint8_t databit[100000];
     srslte_bit_unpack_vector(data, databit, grant.mcs.tbs);
-    srslte_vec_save_file("data_in", databit, grant.mcs.tbs);
+    srslte_vec_save_file("data_in", databit, grant.mcs.tbs);*/
     
-    if (!input_file) {
-      
-      if (rv_idx) {
-        /* Do 1st transmission for rv_idx!=0 */
-        pdsch_cfg.rv = 0;
-        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, rnti, slot_symbols)) {
-          fprintf(stderr, "Error encoding PDSCH\n");
-          goto quit;
-        }
-      }
-      pdsch_cfg.rv = rv_idx; 
-      
-      if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, &softbuffer_tx, data, rnti, slot_symbols)) {
+    if (rv_idx[0] != 0 || rv_idx[1] != 0) {
+      /* Do 1st transmission for rv_idx!=0 */
+      bzero(pdsch_cfg.rv, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
+      if (srslte_pdsch_encode(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
         fprintf(stderr, "Error encoding PDSCH\n");
         goto quit;
       }
     }
-    
+    memcpy(pdsch_cfg.rv, rv_idx, sizeof(uint32_t)*SRSLTE_MAX_CODEWORDS);
+    gettimeofday(&t[1], NULL);
+    for (k = 0; k < M; k++) {
+      if (srslte_pdsch_encode(&pdsch_tx, &pdsch_cfg, softbuffers_tx, data_tx, rnti, tx_slot_symbols)) {
+        ERROR("Error encoding PDSCH");
+        goto quit;
+      }
+    }
+    gettimeofday(&t[2], NULL);
+    get_time_interval(t);
+    printf("ENCODED in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n",
+           (float) t[0].tv_usec/M, (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)/1000.0f,
+           (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
+
+  #ifdef DO_OFDM
+    for (i = 0; i < cell.nof_ports; i++) {
+      /* For each Tx antenna modulate OFDM */
+      srslte_ofdm_tx_sf(&ofdm_tx, tx_slot_symbols[i], tx_sf_symbols[i]);
+    }
+
     /* 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];
+    for (j = 0; j < nof_rx_antennas; j++) {
+      for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+        rx_sf_symbols[j][k] = 0.0f;
+        for (i = 0; i < cell.nof_ports; i++) {
+          rx_sf_symbols[j][k] += tx_sf_symbols[i][k] * ce[i][j][k];
+        }
+      }
+    }
+  #else
+    /* combine outputs */
+    for (j = 0; j < nof_rx_antennas; j++) {
+      for (k = 0; k < SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp); k++) {
+        rx_slot_symbols[j][k] = 0.0f;
+        for (i = 0; i < cell.nof_ports; i++) {
+          rx_slot_symbols[j][k] += tx_slot_symbols[i][k] * ce[i][j][k];
         }
-        ce[i][j] = 1;
       }
     }
-    
-  #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);
+
+
+  }
+  int r=0;
+  srslte_pdsch_set_max_noi(&pdsch_rx, 10);
+
   gettimeofday(&t[1], NULL);
-  for (i=0;i<M;i++) {
-  #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);    
-    r = srslte_pdsch_decode(&pdsch, &pdsch_cfg, &softbuffer_rx, slot_symbols[0], ce, 0, rnti, data);
+  for (k = 0; k < M; k++) {
+#ifdef DO_OFDM
+    /* For each Rx antenna demodulate OFDM */
+    for (i = 0; i < nof_rx_antennas; i++) {
+      srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
+    }
+#endif
+    for (i = 0; i < grant.nof_tb; i++) {
+      if (grant.mcs[i].tbs) {
+        srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
+      }
+    }
+    r = srslte_pdsch_decode(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data_rx, acks);
   }
   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);                
+         (float) t[0].tv_usec/M, (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)/1000.0f,
+         (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
+
+  /* If there is an error in PDSCH decode */
   if (r) {
     ret = -1;
     goto quit;
-  } 
+  }
+
+  /* Check Tx and Rx bytes */
+  for (int tb = 0; tb < grant.nof_tb; tb++) {
+    for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+      if (data_tx[tb][byte] != data_rx[tb][byte]) {
+        ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+        ret = SRSLTE_ERROR;
+        goto quit;
+      }
+    }
+  }
+
+  /* Check all transport blocks have been decoded OK */
+  for (int tb = 0; tb < grant.nof_tb; tb++) {
+    ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+  }
+
+  ret = SRSLTE_SUCCESS;
 
-  ret = 0;
 quit:
-  srslte_pdsch_free(&pdsch);
-  srslte_softbuffer_tx_free(&softbuffer_tx);
-  srslte_softbuffer_rx_free(&softbuffer_rx);
-  
-  for (i=0;i<cell.nof_ports;i++) {
-    if (ce[i]) {
-      free(ce[i]);
+  srslte_pdsch_free(&pdsch_tx);
+  srslte_pdsch_free(&pdsch_rx);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    srslte_softbuffer_tx_free(softbuffers_tx[i]);
+    if (softbuffers_tx[i]) {
+      free(softbuffers_tx[i]);
+    }
+
+    srslte_softbuffer_rx_free(softbuffers_rx[i]);
+    if (softbuffers_rx[i]) {
+      free(softbuffers_rx[i]);
+    }
+
+    if (data_tx[i]) {
+      free(data_tx[i]);
     }
-    if (slot_symbols[i]) {
-      free(slot_symbols[i]);
+
+    if (data_rx[i]) {
+      free(data_rx[i]);
     }
   }
-  if (data) {
-    free(data);
+
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      if (ce[i][j]) {
+        free(ce[i][j]);
+      }
+    }
+    if (tx_slot_symbols[i]) {
+      free(tx_slot_symbols[i]);
+    }
+    if (rx_slot_symbols[i]) {
+      free(rx_slot_symbols[i]);
+    }
   }
   if (ret) {
     printf("Error\n");
diff --git a/lib/src/phy/phch/test/pdsch_test_mex.c b/lib/src/phy/phch/test/pdsch_test_mex.c
index c2273916a..eb198bcbe 100644
--- a/lib/src/phy/phch/test/pdsch_test_mex.c
+++ b/lib/src/phy/phch/test/pdsch_test_mex.c
@@ -101,7 +101,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     nof_antennas = dims[2];
   }
   
-  if (srslte_pdsch_init_multi(&pdsch, cell, nof_antennas)) {
+  if (srslte_pdsch_init_rx_multi(&pdsch, cell, nof_antennas)) {
     mexErrMsgTxt("Error initiating PDSCH\n");
     return;
   }
@@ -202,11 +202,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       ce[i][j] = srslte_vec_malloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp) * sizeof(cf_t));
     }
   }
-  uint8_t *data_bytes = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs/8);
-  if (!data_bytes) {
+  uint8_t *data_bytes[SRSLTE_MAX_CODEWORDS];
+  data_bytes[0] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs.tbs/8);
+  if (!data_bytes[0]) {
     return;
   }
-  srslte_sch_set_max_noi(&pdsch.dl_sch, max_iterations);
+
+  data_bytes[1] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs2.tbs/8);
+  if (!data_bytes[1]) {
+    return;
+  }
+
+  srslte_pdsch_set_max_noi(&pdsch, max_iterations);
 
   bool input_fft_allocated = false; 
   int r=-1;
@@ -272,7 +279,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   }
   
   uint8_t *data = malloc(grant.mcs.tbs);
-  srslte_bit_unpack_vector(data_bytes, data, grant.mcs.tbs);
+  srslte_bit_unpack_vector(data_bytes[0], data, grant.mcs.tbs);
   
   if (nlhs >= 1) { 
     plhs[0] = mxCreateLogicalScalar(r == 0);
@@ -284,10 +291,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexutils_write_cf(pdsch.symbols[0], &plhs[2], cfg.nbits.nof_re, 1);  
   }
   if (nlhs >= 4) {
-    mexutils_write_cf(pdsch.d, &plhs[3], cfg.nbits.nof_re, 1);  
+    mexutils_write_cf(pdsch.d[0], &plhs[3], cfg.nbits.nof_re, 1);
   }
   if (nlhs >= 5) {
-    mexutils_write_s(pdsch.e, &plhs[4], cfg.nbits.nof_bits, 1);  
+    mexutils_write_s(pdsch.e[0], &plhs[4], cfg.nbits.nof_bits, 1);
   }
   if (nlhs >= 6) {
     uint32_t len = nof_antennas*cell.nof_ports*SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp);
@@ -323,7 +330,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       }
     }
   }
-  free(data_bytes);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (data_bytes[i]) {
+      free(data_bytes[i]);
+    }
+  }
   free(data);
   
   return;
diff --git a/lib/src/phy/phch/test/phich_file_test.c b/lib/src/phy/phch/test/phich_file_test.c
index 27e9bc14e..0250c6f0b 100644
--- a/lib/src/phy/phch/test/phich_file_test.c
+++ b/lib/src/phy/phch/test/phich_file_test.c
@@ -185,7 +185,7 @@ int base_init() {
     return -1;
   }
 
-  if (srslte_phich_init(&phich)) {
+  if (srslte_phich_init(&phich, 1)) {
     fprintf(stderr, "Error creating PBCH object\n");
     return -1;
   }
diff --git a/lib/src/phy/phch/test/phich_test.c b/lib/src/phy/phch/test/phich_test.c
index 1269e02bd..5334b210f 100644
--- a/lib/src/phy/phch/test/phich_test.c
+++ b/lib/src/phy/phch/test/phich_test.c
@@ -142,7 +142,7 @@ int main(int argc, char **argv) {
     cid = cell.id;
     max_cid = cell.id;
   }
-  if (srslte_phich_init(&phich)) {
+  if (srslte_phich_init(&phich, 1)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
diff --git a/lib/src/phy/phch/test/prach_detect_test_mex.c b/lib/src/phy/phch/test/prach_detect_test_mex.c
index bfec8c23a..8db26eaff 100644
--- a/lib/src/phy/phch/test/prach_detect_test_mex.c
+++ b/lib/src/phy/phch/test/prach_detect_test_mex.c
@@ -104,6 +104,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     srslte_prach_set_detect_factor(&prach, factor);
   }
 
+  mexPrintf("format=%d config=%d, N_cp=%d, ifft=%d\n", prach.f, prach.config_idx, prach.N_cp, prach.N_ifft_ul);
+
   if (srslte_prach_detect_offset(&prach, frequency_offset, &input_signal[prach.N_cp], nof_samples, preambles, offsets, NULL, &nof_detected)) {    
     mexErrMsgTxt("Error detecting PRACH\n");
     return; 
diff --git a/lib/src/phy/phch/test/pucch_encode_test_mex.c b/lib/src/phy/phch/test/pucch_encode_test_mex.c
index ba8d41980..a7e5a94cb 100644
--- a/lib/src/phy/phch/test/pucch_encode_test_mex.c
+++ b/lib/src/phy/phch/test/pucch_encode_test_mex.c
@@ -172,7 +172,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   
   srslte_pucch_set_cfg(&pucch, &pucch_cfg, group_hopping_en);
   
-  if (srslte_pucch_encode(&pucch, format, n_pucch, sf_idx, bits, sf_symbols)) {
+  if (srslte_pucch_encode(&pucch, format, n_pucch, sf_idx, (uint16_t) rnti, bits, sf_symbols)) {
     mexErrMsgTxt("Error encoding PUCCH\n");
     return; 
   }
diff --git a/lib/src/phy/phch/test/pucch_test_mex.c b/lib/src/phy/phch/test/pucch_test_mex.c
index baf6c84db..9672230b2 100644
--- a/lib/src/phy/phch/test/pucch_test_mex.c
+++ b/lib/src/phy/phch/test/pucch_test_mex.c
@@ -187,12 +187,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
   
   srslte_pucch_set_cfg(&pucch, &pucch_cfg, group_hopping_en);
   
-  if (srslte_chest_ul_estimate_pucch(&chest_ul, sf_symbols, ce, format, n_pucch, sf_idx)) {
+  uint8_t pucch2_ack_bits[2] = {0};
+
+  if (srslte_chest_ul_estimate_pucch(&chest_ul, sf_symbols, ce, format, n_pucch, sf_idx, &pucch2_ack_bits)) {
     mexErrMsgTxt("Error estimating PUCCH DMRS\n");
     return;
   }
   
-  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, sf_symbols, ce, 0, bits)<0) {
+  if (srslte_pucch_decode(&pucch, format, n_pucch, sf_idx, (uint16_t) rnti, sf_symbols, ce, 0, bits)<0) {
     mexErrMsgTxt("Error decoding PUCCH\n");
     return; 
   }
diff --git a/lib/src/phy/rf/rf_blade_imp.c b/lib/src/phy/rf/rf_blade_imp.c
index f3dd471f6..d8996925f 100644
--- a/lib/src/phy/rf/rf_blade_imp.c
+++ b/lib/src/phy/rf/rf_blade_imp.c
@@ -178,7 +178,7 @@ float rf_blade_get_rssi(void *h)
   return 0;
 }
 
-int rf_blade_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
+int rf_blade_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   return rf_blade_open(args, h); 
 }
@@ -469,6 +469,20 @@ int rf_blade_recv_with_time(void *h,
   return nsamples;
 }
                    
+int rf_blade_send_timed_multi(void *h,
+                     void *data[4],
+                     int nsamples,
+                     time_t secs,
+                     double frac_secs,                      
+                     bool has_time_spec,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst)
+{
+  return rf_blade_send_timed(h, data[0], nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst,
+                             is_end_of_burst);
+}
+
 int rf_blade_send_timed(void *h,
                      void *data,
                      int nsamples,
diff --git a/lib/src/phy/rf/rf_blade_imp.h b/lib/src/phy/rf/rf_blade_imp.h
index 07e9bb7cf..fbaeab1df 100644
--- a/lib/src/phy/rf/rf_blade_imp.h
+++ b/lib/src/phy/rf/rf_blade_imp.h
@@ -34,7 +34,7 @@ SRSLTE_API int rf_blade_open(char *args,
                         void **handler);
 
 SRSLTE_API int rf_blade_open_multi(char *args, 
-                        void **handler, uint32_t nof_rx_antennas);
+                        void **handler, uint32_t nof_channels);
 
 SRSLTE_API char* rf_blade_devname(void *h);
 
@@ -111,6 +111,16 @@ SRSLTE_API void rf_blade_get_time(void *h,
                               time_t *secs, 
                               double *frac_secs); 
 
+SRSLTE_API int rf_blade_send_timed_multi(void *h,
+                              void *data[4],
+                              int nsamples,
+                              time_t secs,
+                              double frac_secs,
+                              bool has_time_spec,
+                              bool blocking,
+                              bool is_start_of_burst,
+                              bool is_end_of_burst);
+
 SRSLTE_API int  rf_blade_send_timed(void *h, 
                                   void *data, 
                                   int nsamples,
diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index e1b761419..00f157b6b 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -38,7 +38,7 @@ typedef struct {
   void   (*srslte_rf_suppress_stdout)(void *h);
   void   (*srslte_rf_register_error_handler)(void *h, srslte_rf_error_handler_t error_handler);
   int    (*srslte_rf_open)(char *args, void **h);
-  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_rx_antennas);
+  int    (*srslte_rf_open_multi)(char *args, void **h, uint32_t nof_channels);
   int    (*srslte_rf_close)(void *h);
   void   (*srslte_rf_set_master_clock_rate)(void *h, double rate);
   bool   (*srslte_rf_is_master_clock_dynamic)(void *h);
@@ -58,6 +58,9 @@ typedef struct {
   int    (*srslte_rf_send_timed)(void *h, void *data, int nsamples,
                      time_t secs, double frac_secs, bool has_time_spec,
                      bool blocking, bool is_start_of_burst, bool is_end_of_burst);
+  int    (*srslte_rf_send_timed_multi)(void *h, void *data[4], int nsamples,
+                     time_t secs, double frac_secs, bool has_time_spec,
+                     bool blocking, bool is_start_of_burst, bool is_end_of_burst);
   void   (*srslte_rf_set_tx_cal)(void *h, srslte_rf_cal_t *cal);
 
   void   (*srslte_rf_set_rx_cal)(void *h, srslte_rf_cal_t *cal);
@@ -81,7 +84,7 @@ static rf_dev_t dev_uhd = {
   rf_uhd_suppress_stdout,
   rf_uhd_register_error_handler,
   rf_uhd_open,
-  rf_uhd_open_multi,
+  .srslte_rf_open_multi = rf_uhd_open_multi,
   rf_uhd_close,
   rf_uhd_set_master_clock_rate,
   rf_uhd_is_master_clock_dynamic,
@@ -97,6 +100,7 @@ static rf_dev_t dev_uhd = {
   rf_uhd_recv_with_time,
   rf_uhd_recv_with_time_multi,
   rf_uhd_send_timed,
+  .srslte_rf_send_timed_multi = rf_uhd_send_timed_multi,
   rf_uhd_set_tx_cal,
   rf_uhd_set_rx_cal
 };
@@ -119,7 +123,7 @@ static rf_dev_t dev_blade = {
   rf_blade_suppress_stdout,
   rf_blade_register_error_handler,
   rf_blade_open,
-  rf_blade_open_multi,
+  .srslte_rf_open_multi = rf_blade_open_multi,
   rf_blade_close,
   rf_blade_set_master_clock_rate,
   rf_blade_is_master_clock_dynamic,
@@ -135,6 +139,7 @@ static rf_dev_t dev_blade = {
   rf_blade_recv_with_time,
   rf_blade_recv_with_time_multi,
   rf_blade_send_timed,
+  .srslte_rf_send_timed_multi = rf_blade_send_timed_multi,
   rf_blade_set_tx_cal,
   rf_blade_set_rx_cal
 };
@@ -172,6 +177,7 @@ static rf_dev_t dev_soapy = {
   rf_soapy_recv_with_time,
   rf_soapy_recv_with_time_multi,
   rf_soapy_send_timed,
+  .srslte_rf_send_timed_multi = /* FIXME: Implement srslte_rf_send_timed_multi for Soapy SDR */ NULL,
   rf_soapy_set_tx_cal,
   rf_soapy_set_rx_cal
 };
diff --git a/lib/src/phy/rf/rf_imp.c b/lib/src/phy/rf/rf_imp.c
index 92b8143da..c1074ebc5 100644
--- a/lib/src/phy/rf/rf_imp.c
+++ b/lib/src/phy/rf/rf_imp.c
@@ -102,7 +102,7 @@ int srslte_rf_open_devname(srslte_rf_t *rf, char *devname, char *args) {
   return srslte_rf_open_devname_multi(rf, devname, args, 1);
 }
 
-int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_rx_antennas) {
+int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uint32_t nof_channels) {
   /* Try to open the device if name is provided */
   if (devname) {
     if (devname[0] != '\0') {
@@ -110,7 +110,7 @@ int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uin
       while(available_devices[i] != NULL) {
         if (!strcmp(available_devices[i]->name, devname)) {
           rf->dev = available_devices[i];
-          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_rx_antennas);
+          return available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_channels);
         }
         i++;
       }    
@@ -121,7 +121,7 @@ int srslte_rf_open_devname_multi(srslte_rf_t *rf, char *devname, char *args, uin
   /* If in auto mode or provided device not found, try to open in order of apperance in available_devices[] array */
   int i=0;
   while(available_devices[i] != NULL) {
-    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_rx_antennas)) {
+    if (!available_devices[i]->srslte_rf_open_multi(args, &rf->handler, nof_channels)) {
       rf->dev = available_devices[i];
       return 0; 
     }
@@ -301,6 +301,32 @@ int srslte_rf_send_timed3(srslte_rf_t *rf,
                                  has_time_spec, blocking, is_start_of_burst, is_end_of_burst);  
 }
 
+int srslte_rf_send_timed_multi(srslte_rf_t *rf,
+                               void *data[4],
+                               int nsamples,
+                               time_t secs,
+                               double frac_secs,
+                               bool blocking,
+                               bool is_start_of_burst,
+                               bool is_end_of_burst)
+{
+
+  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, secs, frac_secs,
+                                                           true, blocking, is_start_of_burst, is_end_of_burst);
+}
+
+int srslte_rf_send_multi(srslte_rf_t *rf,
+                     void *data[4],
+                     int nsamples,
+                     bool blocking,
+                     bool is_start_of_burst,
+                     bool is_end_of_burst)
+{
+
+  return ((rf_dev_t*) rf->dev)->srslte_rf_send_timed_multi(rf->handler, data, nsamples, 0, 0,
+                                                           false, blocking, is_start_of_burst, is_end_of_burst);
+}
+
 int srslte_rf_send(srslte_rf_t *rf, void *data, uint32_t nsamples, bool blocking)
 {
   return srslte_rf_send2(rf, data, nsamples, blocking, true, true); 
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 70917d777..2505fc02b 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -289,7 +289,7 @@ int rf_uhd_open(char *args, void **h)
   return rf_uhd_open_multi(args, h, 1);
 }
 
-int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
+int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   if (h) {
     *h = NULL; 
@@ -397,11 +397,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
           .otw_format = "sc16",
           .args = "",
           .channel_list = channel,
-          .n_channels = 1
+          .n_channels = nof_channels,
       };
       
-    handler->nof_rx_channels = nof_rx_antennas; 
-    handler->nof_tx_channels = 1;
+    handler->nof_rx_channels = nof_channels;
+    handler->nof_tx_channels = nof_channels;
 
     /* Set default rate to avoid decimation warnings */
     uhd_usrp_set_rx_rate(handler->usrp, 1.92e6, 0);
@@ -656,8 +656,28 @@ int rf_uhd_send_timed(void *h,
                      bool is_start_of_burst,
                      bool is_end_of_burst) 
 {
+  void *_data[SRSLTE_MAX_PORTS]= {data, zero_mem, zero_mem, zero_mem};
+
+  return rf_uhd_send_timed_multi(h, _data, nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
+}
+
+int rf_uhd_send_timed_multi(void *h,
+                            void *data[4],
+                            int nsamples,
+                            time_t secs,
+                            double frac_secs,
+                            bool has_time_spec,
+                            bool blocking,
+                            bool is_start_of_burst,
+                            bool is_end_of_burst) {
   rf_uhd_handler_t* handler = (rf_uhd_handler_t*) h;
   
+  /* Resets the USRP time FIXME: this might cause problems for burst transmissions */
+  if (!has_time_spec && is_start_of_burst && handler->nof_tx_channels > 1) {
+    uhd_usrp_set_time_now(handler->usrp, 0, 0, 0);
+    uhd_tx_metadata_set_time_spec(&handler->tx_md, 0, 0.1);
+  }
+
   size_t txd_samples;
   if (has_time_spec) {
     uhd_tx_metadata_set_time_spec(&handler->tx_md, secs, frac_secs);
@@ -665,7 +685,10 @@ int rf_uhd_send_timed(void *h,
   int trials = 0; 
   if (blocking) {
     int n = 0;
-    cf_t *data_c = (cf_t*) data;
+    cf_t *data_c[4];
+    for (int i = 0; i < 4; i++) {
+      data_c[i] = data[i];
+    }
     do {
       size_t tx_samples = handler->tx_nof_samples;
       
@@ -683,9 +706,12 @@ int rf_uhd_send_timed(void *h,
         tx_samples = nsamples - n; 
         uhd_tx_metadata_set_end(&handler->tx_md, is_end_of_burst);
       }
-      
-      void *buff = (void*) &data_c[n];
-      const void *buffs_ptr[4] = {buff, zero_mem, zero_mem, zero_mem};
+
+      const void *buffs_ptr[4];
+      for (int i = 0; i < 4; i++) {
+        void *buff = (void*) &data_c[i][n];
+        buffs_ptr[i] = buff;
+      }
       uhd_error error = uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, 
                                              tx_samples, &handler->tx_md, 3.0, &txd_samples);
       if (error) {
@@ -699,7 +725,10 @@ int rf_uhd_send_timed(void *h,
     } while (n < nsamples && trials < 100);
     return nsamples;
   } else {
-    const void *buffs_ptr[4] = {data, zero_mem, zero_mem, zero_mem};
+    const void *buffs_ptr[4];
+    for (int i = 0; i < 4; i++) {
+     buffs_ptr[i] = data[i];
+    }
     uhd_tx_metadata_set_start(&handler->tx_md, is_start_of_burst);
     uhd_tx_metadata_set_end(&handler->tx_md, is_end_of_burst);
     return uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, nsamples, &handler->tx_md, 0.0, &txd_samples);
diff --git a/lib/src/phy/rf/rf_uhd_imp.h b/lib/src/phy/rf/rf_uhd_imp.h
index 7c26f015c..2be799333 100644
--- a/lib/src/phy/rf/rf_uhd_imp.h
+++ b/lib/src/phy/rf/rf_uhd_imp.h
@@ -39,7 +39,7 @@ SRSLTE_API int rf_uhd_open(char *args,
 
 SRSLTE_API int rf_uhd_open_multi(char *args, 
                                  void **handler, 
-                                 uint32_t nof_rx_antennas);
+                                 uint32_t nof_channels);
 
 SRSLTE_API char* rf_uhd_devname(void *h);
 
@@ -123,3 +123,13 @@ SRSLTE_API int  rf_uhd_send_timed(void *h,
                                   bool is_start_of_burst, 
                                   bool is_end_of_burst);
 
+SRSLTE_API int rf_uhd_send_timed_multi(void *h,
+                                       void *data[SRSLTE_MAX_PORTS],
+                                       int nsamples,
+                                       time_t secs,
+                                       double frac_secs,
+                                       bool has_time_spec,
+                                       bool blocking,
+                                       bool is_start_of_burst,
+                                       bool is_end_of_burst);
+
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index ab3fa526e..1c56f5ae1 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -26,8 +26,6 @@
 
 #include "srslte/phy/ue/ue_dl.h"
 
-#include <complex.h>
-#include <math.h>
 #include <string.h>
 
 
@@ -39,21 +37,23 @@
 
 #define MAX_SFLEN_RE SRSLTE_SF_LEN_RE(max_prb, q->cell.cp)
 
-static srslte_dci_format_t ue_formats[] = {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1}; // Only TM1 and TM2 are currently supported 
-const uint32_t nof_ue_formats = 2; 
+const static srslte_dci_format_t ue_dci_formats[8][2] = {
+    /* Mode 1 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 2 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 3 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2A},
+    /* Mode 4 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2},
+    /* Mode 5 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1D},
+    /* Mode 6 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1B},
+    /* Mode 7 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT1},
+    /* Mode 8 */ {SRSLTE_DCI_FORMAT1A, SRSLTE_DCI_FORMAT2B}
+};
 
 static srslte_dci_format_t common_formats[] = {SRSLTE_DCI_FORMAT1A,SRSLTE_DCI_FORMAT1C};
 const uint32_t nof_common_formats = 2; 
 
-int srslte_ue_dl_init(srslte_ue_dl_t *q, 
-                      uint32_t max_prb)
-{
-  return srslte_ue_dl_init_multi(q, max_prb, 1);
-}
-
-int srslte_ue_dl_init_multi(srslte_ue_dl_t *q, 
-                            uint32_t max_prb,
-                            uint32_t nof_rx_antennas) 
+int srslte_ue_dl_init(srslte_ue_dl_t *q,
+                      uint32_t max_prb,
+                      uint32_t nof_rx_antennas)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS; 
   
@@ -78,33 +78,41 @@ int srslte_ue_dl_init_multi(srslte_ue_dl_t *q,
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit;
     }
-    if (srslte_pcfich_init_multi(&q->pcfich, nof_rx_antennas)) {
+    if (srslte_pcfich_init(&q->pcfich, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PCFICH object\n");
       goto clean_exit;
     }
-    if (srslte_phich_init(&q->phich)) {
+    if (srslte_phich_init(&q->phich, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PHICH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdcch_init_multi(&q->pdcch, max_prb, nof_rx_antennas)) {
+    if (srslte_pdcch_init_ue(&q->pdcch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDCCH object\n");
       goto clean_exit;
     }
 
-    if (srslte_pdsch_init_multi_ue(&q->pdsch, max_prb, nof_rx_antennas)) {
+    if (srslte_pdsch_init_ue(&q->pdsch, max_prb, nof_rx_antennas)) {
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
-    if (srslte_softbuffer_rx_init(&q->softbuffer, max_prb)) {
-      fprintf(stderr, "Error initiating soft buffer\n");
-      goto clean_exit;
+    for (int i = 0; i < SRSLTE_MAX_TB; i++) {
+      q->softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_rx_t));
+      if (!q->softbuffers[i]) {
+        fprintf(stderr, "Error allocating soft buffer\n");
+        goto clean_exit;
+      }
+
+      if (srslte_softbuffer_rx_init(q->softbuffers[i], max_prb)) {
+        fprintf(stderr, "Error initiating soft buffer\n");
+        goto clean_exit;
+      }
     }
     if (srslte_cfo_init(&q->sfo_correct, max_prb*SRSLTE_NRE)) {
       fprintf(stderr, "Error initiating SFO correct\n");
       goto clean_exit;
     }
-    srslte_cfo_set_tol(&q->sfo_correct, 1e-5/q->fft.symbol_sz);
+    srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft.symbol_sz);
     
     for (int j=0;j<nof_rx_antennas;j++) {
       q->sf_symbols_m[j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
@@ -148,7 +156,12 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     srslte_cfo_free(&q->sfo_correct);
-    srslte_softbuffer_rx_free(&q->softbuffer);
+    for (int i = 0; i < SRSLTE_MAX_TB; i++) {
+      srslte_softbuffer_rx_free(q->softbuffers[i]);
+      if (q->softbuffers[i]) {
+        free(q->softbuffers[i]);
+      }
+    }
     for (int j=0;j<q->nof_rx_antennas;j++) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
@@ -243,7 +256,9 @@ void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
 }
 
 void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
-  srslte_softbuffer_rx_reset(&q->softbuffer);
+  for(int i = 0; i < SRSLTE_MAX_CODEWORDS; i++){
+    srslte_softbuffer_rx_reset(q->softbuffers[i]);
+  }
   bzero(&q->pdsch_cfg, sizeof(srslte_pdsch_cfg_t));
 }
 
@@ -258,24 +273,12 @@ void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
  *    - PDCCH decoding: Find DCI for RNTI given by previous call to srslte_ue_dl_set_rnti()
  *    - PDSCH decoding: Decode TB scrambling with RNTI given by srslte_ue_dl_set_rnti()
  */
-int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti) {
-  cf_t *_input[SRSLTE_MAX_PORTS]; 
-  _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, data, tti, q->current_rnti);
-}
-
-int srslte_ue_dl_decode_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data, uint32_t tti) {
-  return srslte_ue_dl_decode_rnti_multi(q, input, data, tti, q->current_rnti);
+int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                              uint32_t tm, uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]) {
+  return srslte_ue_dl_decode_rnti(q, input, data, tm, tti, q->current_rnti, acks);
 }
 
-int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input, uint32_t sf_idx, uint32_t *cfi) 
-{
-  cf_t *_input[SRSLTE_MAX_PORTS]; 
-  _input[0] = input; 
-  return srslte_ue_dl_decode_fft_estimate_multi(q, _input, sf_idx, cfi);
-}
-
-int srslte_ue_dl_decode_fft_estimate_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi) 
+int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi)
 {
   if (input && q && cfi && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
     
@@ -328,29 +331,43 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 }
 
 
-int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx, uint32_t rvidx) 
-{
-  return srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx);
-}
+int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
+                                 int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type) {
+  uint32_t pmi = 0;
 
-int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input, uint8_t *data, uint32_t tti, uint16_t rnti) 
-{
-  cf_t *_input[SRSLTE_MAX_PORTS];
-  _input[0] = input; 
-  return srslte_ue_dl_decode_rnti_multi(q, _input, data, tti, rnti);
+  /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
+  if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+    if (grant->nof_tb == 1) {
+      if (grant->pinfo > 0 && grant->pinfo < 5) {
+        pmi = grant->pinfo - 1;
+      } else {
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        return SRSLTE_ERROR;
+      }
+    } else {
+      if (grant->pinfo < 2) {
+        pmi = grant->pinfo;
+      } else {
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        return SRSLTE_ERROR;
+      }
+    }
+  }
+  return srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
 }
 
-int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data, uint32_t tti, uint16_t rnti) 
-{
+int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
+                             uint8_t *data[SRSLTE_MAX_CODEWORDS], uint32_t tm, uint32_t tti, uint16_t rnti,
+                             bool acks[SRSLTE_MAX_CODEWORDS]) {
+  srslte_mimo_type_t mimo_type;
   srslte_dci_msg_t dci_msg;
   srslte_ra_dl_dci_t dci_unpacked;
   srslte_ra_dl_grant_t grant; 
   int ret = SRSLTE_ERROR; 
   uint32_t cfi;
+  uint32_t sf_idx = tti%10;
   
-  uint32_t sf_idx = tti%10; 
-  
-  if ((ret = srslte_ue_dl_decode_fft_estimate_multi(q, input, sf_idx, &cfi)) < 0) {
+  if ((ret = srslte_ue_dl_decode_fft_estimate(q, input, sf_idx, &cfi)) < 0) {
     return ret; 
   }
   
@@ -363,7 +380,7 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     return SRSLTE_ERROR;
   }
 
-  int found_dci = srslte_ue_dl_find_dl_dci(q, cfi, sf_idx, rnti, &dci_msg);   
+  int found_dci = srslte_ue_dl_find_dl_dci(q, tm, cfi, sf_idx, rnti, &dci_msg);
   if (found_dci == 1) {
     
     if (srslte_dci_msg_to_dl_grant(&dci_msg, rnti, q->cell.nof_prb, q->cell.nof_ports, &dci_unpacked, &grant)) {
@@ -374,18 +391,68 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     /* ===== These lines of code are supposed to be MAC functionality === */
 
     
-    uint32_t rvidx = 0; 
+    int rvidx[SRSLTE_MAX_CODEWORDS] = {1};
     if (dci_unpacked.rv_idx < 0) {
       uint32_t sfn = tti/10; 
-      uint32_t k   = (sfn/2)%4; 
-      rvidx        = ((uint32_t) ceilf((float)1.5*k))%4;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffer, grant.mcs.tbs);      
+      uint32_t k   = (sfn/2)%4;
+      for (int i = 0; i < grant.nof_tb; i++) {
+        rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+      }
     } else {
-      rvidx = dci_unpacked.rv_idx;
-      srslte_softbuffer_rx_reset_tbs(&q->softbuffer, grant.mcs.tbs);      
+      for (int i = 0; i < grant.nof_tb; i++) {
+        switch(i) {
+          case 0:
+            rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
+            break;
+          case 1:
+            rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
+            break;
+          default:
+            ERROR("Wrong number of transport blocks");
+            return SRSLTE_ERROR;
+        }
+        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+      }
+    }
+
+    switch(dci_msg.format) {
+      case SRSLTE_DCI_FORMAT1:
+      case SRSLTE_DCI_FORMAT1A:
+        if (q->cell.nof_ports == 1) {
+          mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        }
+        break;
+      case SRSLTE_DCI_FORMAT2:
+        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+        }
+        break;
+      case SRSLTE_DCI_FORMAT2A:
+        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+          mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+        } else {
+          mimo_type = SRSLTE_MIMO_TYPE_CDD;
+        }
+        break;
+
+      /* Not implemented formats */
+      case SRSLTE_DCI_FORMAT0:
+      case SRSLTE_DCI_FORMAT1C:
+      case SRSLTE_DCI_FORMAT1B:
+      case SRSLTE_DCI_FORMAT1D:
+      case SRSLTE_DCI_FORMAT2B:
+      default:
+        ERROR("Transmission mode not supported.");
+        return SRSLTE_ERROR;
     }
 
-    if (srslte_ue_dl_cfg_grant(q, &grant, cfi, sf_idx, rvidx)) {
+    if (srslte_ue_dl_cfg_grant(q, &grant, cfi, sf_idx, rvidx, mimo_type)) {
+      ERROR("Configuing PDSCH");
       return SRSLTE_ERROR; 
     }
     
@@ -394,17 +461,26 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
     q->nof_detected++;
   
     
-    if (q->pdsch_cfg.grant.mcs.mod > 0 && q->pdsch_cfg.grant.mcs.tbs >= 0) {
-      ret = srslte_pdsch_decode_multi(&q->pdsch, &q->pdsch_cfg, &q->softbuffer, 
-                                    q->sf_symbols_m, q->ce_m, 
-                                    noise_estimate, 
-                                    rnti, data);
-      
+    if (q->pdsch_cfg.grant.mcs[0].mod > 0 && q->pdsch_cfg.grant.mcs[0].tbs >= 0) {
+      ret = srslte_pdsch_decode(&q->pdsch, &q->pdsch_cfg, q->softbuffers,
+                                q->sf_symbols_m, q->ce_m,
+                                noise_estimate,
+                                rnti, data, acks);
+
+      for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
+        if (!acks[tb]) {
+          q->pkt_errors++;
+        }
+        q->pkts_total++;
+      }
+
       if (ret == SRSLTE_ERROR) {
-        q->pkt_errors++;
       } else if (ret == SRSLTE_ERROR_INVALID_INPUTS) {
         fprintf(stderr, "Error calling srslte_pdsch_decode()\n");      
-      } 
+      }
+
+      /* If we are in TM4 (Closed-Loop MIMO), compute condition number */
+
     }
   
   /*
@@ -416,15 +492,95 @@ int srslte_ue_dl_decode_rnti_multi(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_POR
   
   }
 
-  q->pkts_total++;
-
   if (found_dci == 1 && ret == SRSLTE_SUCCESS) { 
-    return q->pdsch_cfg.grant.mcs.tbs;    
+    return q->pdsch_cfg.grant.mcs[0].tbs;
   } else {
     return 0;
   }
 }
 
+/* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
+ * Noise Ratio (SINR), valid for TM4 */
+int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
+  float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
+  float best_sinr = -INFINITY;
+  uint32_t best_pmi = 0, best_ri = 0;
+
+  if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
+    /* Do nothing */
+    return SRSLTE_SUCCESS;
+  } else if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+    if (srslte_pdsch_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
+                                  SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), q->pmi, q->sinr)) {
+      ERROR("SINR calculation error");
+      return SRSLTE_ERROR;
+    }
+
+    /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
+    for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
+      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers;
+      if (_sinr > best_sinr + 0.1) {
+        best_sinr = _sinr;
+        best_pmi = q->pmi[nof_layers - 1];
+        best_ri = nof_layers;
+      }
+    }
+
+    /* Set RI */
+    if (ri != NULL) {
+      *ri = best_ri;
+    }
+
+    /* Set PMI */
+    if (pmi != NULL) {
+      *pmi = best_pmi;
+    }
+
+    /* Set current SINR */
+    if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+      if (q->pdsch_cfg.nof_layers == 1) {
+        *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
+      } else if (q->pdsch_cfg.nof_layers == 2) {
+        *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
+      } else {
+        ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
+        return SRSLTE_ERROR;
+      }
+    }
+
+    /* Print Trace */
+    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
+      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
+           10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+    }
+  } else {
+    ERROR("Not implemented configuration");
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
+  return SRSLTE_SUCCESS;
+}
+
+
+/* Compute the Rank Indicator (RI) by computing the condition number, valid for TM3 */
+int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
+  float _cn;
+  int ret = srslte_pdsch_cn_compute(&q->pdsch, q->ce_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), &_cn);
+
+  /* Set Condition number */
+  if (cn) {
+    *cn = _cn;
+  }
+
+  /* Set rank indicator */
+  if (!ret && ri) {
+    *ri = (_cn > 3.0f)? 1:0;
+  }
+
+  return ret;
+}
+
+
 uint32_t srslte_ue_dl_get_ncce(srslte_ue_dl_t *q) {
   return q->last_location.ncce; 
 }
@@ -502,7 +658,7 @@ int srslte_ue_dl_find_ul_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
   }
 }
 
-int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
+int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
 {
   srslte_rnti_type_t rnti_type; 
   if (rnti == SRSLTE_SIRNTI) {
@@ -514,7 +670,7 @@ int srslte_ue_dl_find_dl_dci(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, u
   } else {
     rnti_type = SRSLTE_RNTI_USER;
   }
-  return srslte_ue_dl_find_dl_dci_type(q, cfi, sf_idx, rnti, rnti_type, dci_msg);
+  return srslte_ue_dl_find_dl_dci_type(q, tm, cfi, sf_idx, rnti, rnti_type, dci_msg);
 }
 
 // Blind search for SI/P/RA-RNTI
@@ -538,12 +694,17 @@ static int find_dl_dci_type_siprarnti(srslte_ue_dl_t *q, uint32_t cfi, uint16_t
 }
 
 // Blind search for C-RNTI
-static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg)
-{
+static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi,
+                                  uint32_t sf_idx, uint16_t rnti, srslte_dci_msg_t *dci_msg) {
   int ret = SRSLTE_SUCCESS; 
   dci_blind_search_t search_space; 
   dci_blind_search_t *current_ss = &search_space;
-  
+
+  if (cfi < 1 || cfi > 3) {
+    ERROR("CFI must be 1 ≤ cfi ≤ 3", cfi);
+    return SRSLTE_ERROR;
+  }
+
   // Search UE-specific search space 
   if (q->current_rnti == rnti) {
     current_ss = &q->current_ss_ue[cfi-1][sf_idx];
@@ -553,15 +714,19 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_i
   }
   
   srslte_pdcch_set_cfi(&q->pdcch, cfi);
-  
-  INFO("Searching DL C-RNTI in %d ue locations, %d formats\n", current_ss->nof_locations, nof_ue_formats);
-  for (int f=0;f<nof_ue_formats;f++) {
-    current_ss->format = ue_formats[f];   
+
+  for (int f = 0; f < 2; f++) {
+    srslte_dci_format_t format = ue_dci_formats[tm][f];
+
+    INFO("Searching DL C-RNTI %s in %d ue locations\n", srslte_dci_format_string(format),
+         current_ss->nof_locations);
+
+    current_ss->format = format;
     if ((ret = dci_blind_search(q, current_ss, rnti, dci_msg))) {
       return ret; 
     }
   }
-  
+
   // Search Format 1A in the Common SS also
   if (q->current_rnti == rnti) {
     current_ss = &q->current_ss_common[cfi-1];
@@ -581,13 +746,13 @@ static int find_dl_dci_type_crnti(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_i
   return SRSLTE_SUCCESS; 
 }
 
-int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, uint32_t cfi, uint32_t sf_idx, 
+int srslte_ue_dl_find_dl_dci_type(srslte_ue_dl_t *q, uint32_t tm, uint32_t cfi, uint32_t sf_idx,
                                   uint16_t rnti, srslte_rnti_type_t rnti_type, srslte_dci_msg_t *dci_msg)
 {  
   if (rnti_type == SRSLTE_RNTI_SI || rnti_type == SRSLTE_RNTI_PCH || rnti_type == SRSLTE_RNTI_RAR) {
     return find_dl_dci_type_siprarnti(q, cfi, rnti, dci_msg);
   } else {
-    return find_dl_dci_type_crnti(q, cfi, sf_idx, rnti, dci_msg);
+    return find_dl_dci_type_crnti(q, tm, cfi, sf_idx, rnti, dci_msg);
   }
 }
 
@@ -639,18 +804,17 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pdcch_eq_symbols", q->pdcch.d, q->pdcch.nof_cce*36*sizeof(cf_t));
   srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
 
-
-  srslte_vec_save_file("pdsch_symbols", q->pdsch.symbols[0], q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  srslte_vec_save_file("pdsch_eq_symbols", q->pdsch.d, q->pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  srslte_vec_save_file("pdsch_llr", q->pdsch.e, q->pdsch_cfg.nbits.nof_bits*sizeof(cf_t));
-  int cb_len = q->pdsch_cfg.cb_segm.K1; 
-  for (int i=0;i<q->pdsch_cfg.cb_segm.C;i++) {
-    char tmpstr[64]; 
+  
+  srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits[0].nof_bits*sizeof(cf_t));
+  int cb_len = q->pdsch_cfg.cb_segm[0].K1;
+  for (int i=0;i<q->pdsch_cfg.cb_segm[0].C;i++) {
+    char tmpstr[64];
     snprintf(tmpstr,64,"rmout_%d.dat",i);
     srslte_vec_save_file(tmpstr, softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));  
   }
   printf("Saved files for tti=%d, sf=%d, cfi=%d, mcs=%d, rv=%d, rnti=0x%x\n", tti, tti%10, cfi, 
-         q->pdsch_cfg.grant.mcs.idx, rv_idx, rnti);
+         q->pdsch_cfg.grant.mcs[0].idx, rv_idx, rnti);
 }
 
 
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index 967b02d63..02d28c258 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -54,6 +54,11 @@ cf_t dummy_buffer1[15*2048/2];
 cf_t *dummy_offset_buffer[SRSLTE_MAX_PORTS] = {dummy_buffer0, dummy_buffer1};
 
 int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset_time, float offset_freq) {
+  return srslte_ue_sync_init_file_multi(q, nof_prb, file_name, offset_time, offset_freq, 1);
+}
+
+int srslte_ue_sync_init_file_multi(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_name, int offset_time,
+                                   float offset_freq, uint32_t nof_rx_ant) {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   
   if (q                   != NULL && 
@@ -67,6 +72,7 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     q->file_cfo = -offset_freq; 
     q->correct_cfo = true; 
     q->fft_size = srslte_symbol_sz(nof_prb);
+    q->nof_rx_antennas = nof_rx_ant;
     
     if (srslte_cfo_init(&q->file_cfo_correct, 2*q->sf_len)) {
       fprintf(stderr, "Error initiating CFO\n");
@@ -81,12 +87,12 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     INFO("Offseting input file by %d samples and %.1f kHz\n", offset_time, offset_freq/1000);
 
     if (offset_time) {
-      cf_t *file_offset_buffer = srslte_vec_malloc(offset_time * sizeof(cf_t));
+      cf_t *file_offset_buffer = srslte_vec_malloc(offset_time * nof_rx_ant * sizeof(cf_t));
       if (!file_offset_buffer) {
         perror("malloc");
         goto clean_exit;
       }
-      srslte_filesource_read(&q->file_source, file_offset_buffer, offset_time);
+      srslte_filesource_read(&q->file_source, file_offset_buffer, offset_time * nof_rx_ant);
       free(file_offset_buffer);
     }
 
@@ -581,26 +587,27 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
   {
     
     if (q->file_mode) {
-      int n = srslte_filesource_read(&q->file_source, input_buffer[0], q->sf_len);
+      int n = srslte_filesource_read_multi(&q->file_source, (void **) input_buffer, q->sf_len, q->nof_rx_antennas);
       if (n < 0) {
         fprintf(stderr, "Error reading input file\n");
-        return SRSLTE_ERROR; 
+        return SRSLTE_ERROR;
       }
       if (n == 0) {
         srslte_filesource_seek(&q->file_source, 0);
-        q->sf_idx = 9; 
-        int n = srslte_filesource_read(&q->file_source, input_buffer[0], q->sf_len);
+        q->sf_idx = 9;
+        n = srslte_filesource_read_multi(&q->file_source, (void **) input_buffer, q->sf_len, q->nof_rx_antennas);
         if (n < 0) {
           fprintf(stderr, "Error reading input file\n");
-          return SRSLTE_ERROR; 
+          return SRSLTE_ERROR;
         }
       }
       if (q->correct_cfo) {
-        srslte_cfo_correct(&q->file_cfo_correct, 
-                    input_buffer[0], 
-                    input_buffer[0], 
-                    q->file_cfo / 15000 / q->fft_size);               
-                    
+        for (int i = 0; i < q->nof_rx_antennas; i++) {
+          srslte_cfo_correct(&q->file_cfo_correct,
+                             input_buffer[i],
+                             input_buffer[i],
+                             q->file_cfo / 15000 / q->fft_size);
+        }
       }
       q->sf_idx++;
       if (q->sf_idx == 10) {
diff --git a/lib/src/phy/utils/mat.c b/lib/src/phy/utils/mat.c
new file mode 100644
index 000000000..439daa2ce
--- /dev/null
+++ b/lib/src/phy/utils/mat.c
@@ -0,0 +1,284 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <complex.h>
+#include <math.h>
+
+#include "srslte/phy/utils/mat.h"
+
+
+/* Generic implementation for complex reciprocal */
+inline cf_t srslte_mat_cf_recip_gen(cf_t a) {
+  return conjf(a) / (crealf(a) * crealf(a) + cimagf(a) * cimagf(a));
+}
+
+/* Generic implementation for 2x2 determinant */
+inline cf_t srslte_mat_2x2_det_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11) {
+  return a00 * a11 - a01 * a10;
+}
+
+/* 2x2 Matrix inversion, generic implementation */
+inline void srslte_mat_2x2_inv_gen(cf_t a00, cf_t a01, cf_t a10, cf_t a11,
+                                       cf_t *r00, cf_t *r01, cf_t *r10, cf_t *r11) {
+  cf_t div = srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(a00, a01, a10, a11));
+  *r00 = a11 * div;
+  *r01 = -a01 * div;
+  *r10 = -a10 * div;
+  *r11 = a00 * div;
+}
+
+/* Generic implementation for Zero Forcing (ZF) solver */
+inline void srslte_mat_2x2_zf_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                      cf_t *x0, cf_t *x1, float norm) {
+  cf_t _norm = srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(h00, h01, h10, h11)) * norm;
+  *x0 = (y0 * h11 - h01 * y1) * _norm;
+  *x1 = (y1 * h00 - h10 * y0) * _norm;
+}
+
+/* Generic implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_mat_2x2_mmse_gen(cf_t y0, cf_t y1, cf_t h00, cf_t h01, cf_t h10, cf_t h11,
+                                        cf_t *x0, cf_t *x1, float noise_estimate, float norm) {
+  /* Create conjugated matrix */
+  cf_t _h00 = conjf(h00);
+  cf_t _h01 = conjf(h01);
+  cf_t _h10 = conjf(h10);
+  cf_t _h11 = conjf(h11);
+
+  /* 1. A = H' x H + No*/
+  cf_t a00 = _h00 * h00 + _h10 * h10 + noise_estimate;
+  cf_t a01 = _h00 * h01 + _h10 * h11;
+  cf_t a10 = _h01 * h00 + _h11 * h10;
+  cf_t a11 = _h01 * h01 + _h11 * h11 + noise_estimate;
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  cf_t b00 = a11;
+  cf_t b01 = -a01;
+  cf_t b10 = -a10;
+  cf_t b11 = a00;
+  cf_t _norm = norm * srslte_mat_cf_recip_gen(srslte_mat_2x2_det_gen(a00, a01, a10, a11));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+  cf_t w00 = b00 * _h00 + b01 * _h01;
+  cf_t w01 = b00 * _h10 + b01 * _h11;
+  cf_t w10 = b10 * _h00 + b11 * _h01;
+  cf_t w11 = b10 * _h10 + b11 * _h11;
+
+  /* 4. X = W x Y */
+  *x0 = (y0 * w00 + y1 * w01) * _norm;
+  *x1 = (y0 * w10 + y1 * w11) * _norm;
+}
+
+inline float srslte_mat_2x2_cn(cf_t h00, cf_t h01, cf_t h10, cf_t h11) {
+  /* 1. A = H * H' (A = A') */
+  float a00 =
+      crealf(h00) * crealf(h00) + crealf(h01) * crealf(h01) + cimagf(h00) * cimagf(h00) + cimagf(h01) * cimagf(h01);
+  cf_t a01 = h00 * conjf(h10) + h01 * conjf(h11);
+  //cf_t a10 = h10*conjf(h00) + h11*conjf(h01) = conjf(a01);
+  float a11 =
+      crealf(h10) * crealf(h10) + crealf(h11) * crealf(h11) + cimagf(h10) * cimagf(h10) + cimagf(h11) * cimagf(h11);
+
+  /* 2. |H * H' - {λ0, λ1}| = 0 -> aλ² + bλ + c = 0 */
+  float b = a00 + a11;
+  float c = a00 * a11 - (crealf(a01) * crealf(a01) + cimagf(a01) * cimagf(a01));
+
+  /* 3. λ = (-b ± sqrt(b² - 4 * c))/2 */
+  float sqr = sqrtf(b * b - 4.0f * c);
+  float xmax = b + sqr;
+  float xmin = b - sqr;
+
+  /* 4. κ = sqrt(λ_max / λ_min) */
+  return 10 * log10f(xmax / xmin);
+}
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+
+/* SSE implementation for complex reciprocal */
+inline __m128 srslte_mat_cf_recip_sse(__m128 a) {
+  __m128 conj = _MM_CONJ_PS(a);
+  __m128 sqabs = _mm_mul_ps(a, a);
+  sqabs = _mm_add_ps(_mm_movehdup_ps(sqabs), _mm_moveldup_ps(sqabs));
+
+  __m128 recp = _mm_rcp_ps(sqabs);
+
+  return _mm_mul_ps(recp, conj);
+}
+
+/* SSE implementation for 2x2 determinant */
+inline __m128 srslte_mat_2x2_det_sse(__m128 a00, __m128 a01, __m128 a10, __m128 a11) {
+  return _mm_sub_ps(_MM_PROD_PS(a00, a11), _MM_PROD_PS(a01, a10));
+}
+
+/* SSE implementation for Zero Forcing (ZF) solver */
+inline void srslte_mat_2x2_zf_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                      __m128 *x0, __m128 *x1, float norm) {
+  __m128 detmult1 = _MM_PROD_PS(h00, h11);
+  __m128 detmult2 = _MM_PROD_PS(h01, h10);
+
+  __m128 det = _mm_sub_ps(detmult1, detmult2);
+  __m128 detrec = _mm_mul_ps(srslte_mat_cf_recip_sse(det), _mm_set1_ps(norm));
+
+  *x0 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h11, y0), _MM_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM_PROD_PS(_mm_sub_ps(_MM_PROD_PS(h00, y1), _MM_PROD_PS(h10, y0)), detrec);
+}
+
+/* SSE implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_mat_2x2_mmse_sse(__m128 y0, __m128 y1, __m128 h00, __m128 h01, __m128 h10, __m128 h11,
+                                        __m128 *x0, __m128 *x1, float noise_estimate, float norm) {
+  __m128 _noise_estimate = _mm_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate);
+  __m128 _norm = _mm_set1_ps(norm);
+
+  /* Create conjugated matrix */
+  __m128 _h00 = _MM_CONJ_PS(h00);
+  __m128 _h01 = _MM_CONJ_PS(h01);
+  __m128 _h10 = _MM_CONJ_PS(h10);
+  __m128 _h11 = _MM_CONJ_PS(h11);
+
+  /* 1. A = H' x H + No*/
+  __m128 a00 = _mm_add_ps(_mm_add_ps(_MM_SQMOD_PS(h00), _MM_SQMOD_PS(h10)), _noise_estimate);
+  __m128 a01 = _mm_add_ps(_MM_PROD_PS(_h00, h01), _MM_PROD_PS(_h10, h11));
+  __m128 a10 = _mm_add_ps(_MM_PROD_PS(_h01, h00), _MM_PROD_PS(_h11, h10));
+  __m128 a11 = _mm_add_ps(_mm_add_ps(_MM_SQMOD_PS(h01), _MM_SQMOD_PS(h11)), _noise_estimate);
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  __m128 b00 = a11;
+  __m128 b01 = _mm_xor_ps(a01, _mm_set1_ps(-0.0f));
+  __m128 b10 = _mm_xor_ps(a10, _mm_set1_ps(-0.0f));
+  __m128 b11 = a00;
+  _norm = _mm_mul_ps(_norm, srslte_mat_cf_recip_sse(srslte_mat_2x2_det_sse(a00, a01, a10, a11)));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+  __m128 w00 = _mm_add_ps(_MM_PROD_PS(b00, _h00), _MM_PROD_PS(b01, _h01));
+  __m128 w01 = _mm_add_ps(_MM_PROD_PS(b00, _h10), _MM_PROD_PS(b01, _h11));
+  __m128 w10 = _mm_add_ps(_MM_PROD_PS(b10, _h00), _MM_PROD_PS(b11, _h01));
+  __m128 w11 = _mm_add_ps(_MM_PROD_PS(b10, _h10), _MM_PROD_PS(b11, _h11));
+
+  /* 4. X = W x Y */
+  *x0 = _MM_PROD_PS(_mm_add_ps(_MM_PROD_PS(y0, w00), _MM_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM_PROD_PS(_mm_add_ps(_MM_PROD_PS(y0, w10), _MM_PROD_PS(y1, w11)), _norm);
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+#include <immintrin.h>
+
+/* AVX implementation for complex reciprocal */
+inline __m256 srslte_mat_cf_recip_avx(__m256 a) {
+  __m256 conj = _MM256_CONJ_PS(a);
+  __m256 sqabs = _mm256_mul_ps(a, a);
+  sqabs = _mm256_add_ps(_mm256_movehdup_ps(sqabs), _mm256_moveldup_ps(sqabs));
+
+  __m256 recp = _mm256_rcp_ps(sqabs);
+
+  return _mm256_mul_ps(recp, conj);
+}
+
+/* AVX implementation for 2x2 determinant */
+inline __m256 srslte_mat_2x2_det_avx(__m256 a00, __m256 a01, __m256 a10, __m256 a11) {
+#ifdef LV_HAVE_FMA
+  return _MM256_PROD_SUB_PS(a00, a11, _MM256_PROD_PS(a01, a10));
+#else
+  return _mm256_sub_ps(_MM256_PROD_PS(a00, a11), _MM256_PROD_PS(a01, a10));
+#endif /* LV_HAVE_FMA */
+}
+
+/* AVX implementation for Zero Forcing (ZF) solver */
+inline void srslte_mat_2x2_zf_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                      __m256 *x0, __m256 *x1, float norm) {
+
+  __m256 det = srslte_mat_2x2_det_avx(h00, h01, h10, h11);
+  __m256 detrec = _mm256_mul_ps(srslte_mat_cf_recip_avx(det), _mm256_set1_ps(norm));
+
+#ifdef LV_HAVE_FMA
+  *x0 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h11, y0, _MM256_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM256_PROD_PS(_MM256_PROD_SUB_PS(h00, y1, _MM256_PROD_PS(h10, y0)), detrec);
+#else
+  *x0 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h11, y0), _MM256_PROD_PS(h01, y1)), detrec);
+  *x1 = _MM256_PROD_PS(_mm256_sub_ps(_MM256_PROD_PS(h00, y1), _MM256_PROD_PS(h10, y0)), detrec);
+#endif /* LV_HAVE_FMA */
+}
+
+/* AVX implementation for Minimum Mean Squared Error (MMSE) solver */
+inline void srslte_mat_2x2_mmse_avx(__m256 y0, __m256 y1, __m256 h00, __m256 h01, __m256 h10, __m256 h11,
+                                        __m256 *x0, __m256 *x1, float noise_estimate, float norm) {
+  __m256 _noise_estimate = _mm256_set_ps(0.0f, noise_estimate, 0.0f, noise_estimate,
+                                         0.0f, noise_estimate, 0.0f, noise_estimate);
+  __m256 _norm = _mm256_set1_ps(norm);
+
+  /* Create conjugated matrix */
+  __m256 _h00 = _MM256_CONJ_PS(h00);
+  __m256 _h01 = _MM256_CONJ_PS(h01);
+  __m256 _h10 = _MM256_CONJ_PS(h10);
+  __m256 _h11 = _MM256_CONJ_PS(h11);
+
+  /* 1. A = H' x H + No*/
+#ifdef LV_HAVE_FMA
+  __m256 a00 = _MM256_SQMOD_ADD_PS(h00, h10, _noise_estimate);
+  __m256 a01 = _MM256_PROD_ADD_PS(_h00, h01, _MM256_PROD_PS(_h10, h11));
+  __m256 a10 = _MM256_PROD_ADD_PS(_h01, h00, _MM256_PROD_PS(_h11, h10));
+  __m256 a11 = _MM256_SQMOD_ADD_PS(h01, h11, _noise_estimate);
+#else
+  __m256 a00 = _mm256_add_ps(_MM256_SQMOD_PS(h00, h10), _noise_estimate);
+  __m256 a01 = _mm256_add_ps(_MM256_PROD_PS(_h00, h01), _MM256_PROD_PS(_h10, h11));
+  __m256 a10 = _mm256_add_ps(_MM256_PROD_PS(_h01, h00), _MM256_PROD_PS(_h11, h10));
+  __m256 a11 = _mm256_add_ps(_MM256_SQMOD_PS(h01, h11), _noise_estimate);
+#endif /* LV_HAVE_FMA */
+
+  /* 2. B = inv(H' x H + No) = inv(A) */
+  __m256 b00 = a11;
+  __m256 b01 = _mm256_xor_ps(a01, _mm256_set1_ps(-0.0f));
+  __m256 b10 = _mm256_xor_ps(a10, _mm256_set1_ps(-0.0f));
+  __m256 b11 = a00;
+  _norm = _mm256_mul_ps(_norm, srslte_mat_cf_recip_avx(srslte_mat_2x2_det_avx(a00, a01, a10, a11)));
+
+
+  /* 3. W = inv(H' x H + No) x H' = B x H' */
+#ifdef LV_HAVE_FMA
+  __m256 w00 = _MM256_PROD_ADD_PS(b00, _h00, _MM256_PROD_PS(b01, _h01));
+  __m256 w01 = _MM256_PROD_ADD_PS(b00, _h10, _MM256_PROD_PS(b01, _h11));
+  __m256 w10 = _MM256_PROD_ADD_PS(b10, _h00, _MM256_PROD_PS(b11, _h01));
+  __m256 w11 = _MM256_PROD_ADD_PS(b10, _h10, _MM256_PROD_PS(b11, _h11));
+#else
+  __m256 w00 = _mm256_add_ps(_MM256_PROD_PS(b00, _h00), _MM256_PROD_PS(b01, _h01));
+  __m256 w01 = _mm256_add_ps(_MM256_PROD_PS(b00, _h10), _MM256_PROD_PS(b01, _h11));
+  __m256 w10 = _mm256_add_ps(_MM256_PROD_PS(b10, _h00), _MM256_PROD_PS(b11, _h01));
+  __m256 w11 = _mm256_add_ps(_MM256_PROD_PS(b10, _h10), _MM256_PROD_PS(b11, _h11));
+#endif /* LV_HAVE_FMA */
+
+  /* 4. X = W x Y */
+#ifdef LV_HAVE_FMA
+  *x0 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w00, _MM256_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM256_PROD_PS(_MM256_PROD_ADD_PS(y0, w10, _MM256_PROD_PS(y1, w11)), _norm);
+#else
+  *x0 = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w00), _MM256_PROD_PS(y1, w01)), _norm);
+  *x1 = _MM256_PROD_PS(_mm256_add_ps(_MM256_PROD_PS(y0, w10), _MM256_PROD_PS(y1, w11)), _norm);
+#endif /* LV_HAVE_FMA */
+}
+
+#endif /* LV_HAVE_AVX */
diff --git a/lib/src/phy/utils/test/CMakeLists.txt b/lib/src/phy/utils/test/CMakeLists.txt
index 494c20c16..4dccbf2a0 100644
--- a/lib/src/phy/utils/test/CMakeLists.txt
+++ b/lib/src/phy/utils/test/CMakeLists.txt
@@ -33,3 +33,12 @@ add_test(dft_dc dft_test -b -d)   # Backwards first & handle dc internally
 add_test(dft_odd dft_test -N 255) # Odd-length
 add_test(dft_odd_dc dft_test -N 255 -b -d) # Odd-length, backwards first, handle dc
 
+########################################################################
+# Algebra TEST
+########################################################################
+
+add_executable(algebra_test mat_test.c)
+target_link_libraries(algebra_test srslte_phy)
+
+add_test(algebra_2x2_zf_solver_test algebra_test -z)
+add_test(algebra_2x2_mmse_solver_test algebra_test -m)
diff --git a/lib/src/phy/utils/test/mat_test.c b/lib/src/phy/utils/test/mat_test.c
new file mode 100644
index 000000000..49be5c9ae
--- /dev/null
+++ b/lib/src/phy/utils/test/mat_test.c
@@ -0,0 +1,415 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <complex.h>
+#include <stdbool.h>
+#include <immintrin.h>
+#include <sys/time.h>
+
+#include "srslte/phy/utils/mat.h"
+
+
+bool zf_solver = false;
+bool mmse_solver = false;
+bool verbose = false;
+
+double elapsed_us(struct timeval *ts_start, struct timeval *ts_end) {
+  if (ts_end->tv_usec > ts_start->tv_usec) {
+    return ((double) ts_end->tv_sec - (double) ts_start->tv_sec) * 1000000 +
+           (double) ts_end->tv_usec - (double) ts_start->tv_usec;
+  } else {
+    return ((double) ts_end->tv_sec - (double) ts_start->tv_sec - 1) * 1000000 +
+           ((double) ts_end->tv_usec + 1000000) - (double) ts_start->tv_usec;
+  }
+}
+
+#define NOF_REPETITIONS 1000
+#define RUN_TEST(FUNCTION) /*TYPE NAME (void)*/ { \
+  int i;\
+  struct timeval start, end;\
+  gettimeofday(&start, NULL); \
+  bool ret = true; \
+  for (i = 0; i < NOF_REPETITIONS; i++) {ret &= FUNCTION ();}\
+  gettimeofday(&end, NULL);\
+  if (verbose) printf("%32s: %s ... %6.2f us/call\n", #FUNCTION, (ret)?"Pass":"Fail", \
+                      elapsed_us(&start, &end)/NOF_REPETITIONS);\
+  passed &= ret;\
+}
+
+void usage(char *prog) {
+  printf("Usage: %s [mzvh]\n", prog);
+  printf("\t-m Test Minimum Mean Squared Error (MMSE) solver\n");
+  printf("\t-z Test Zero Forcing (ZF) solver\n");
+  printf("\t-v Verbose\n");
+  printf("\t-h Show this message\n");
+}
+
+void parse_args(int argc, char **argv) {
+  int opt;
+  while ((opt = getopt(argc, argv, "mzvh")) != -1) {
+    switch (opt) {
+      case 'm':
+        mmse_solver = true;
+        break;
+      case 'z':
+        zf_solver = true;
+        break;
+      case 'v':
+        verbose = true;
+        break;
+      case 'h':
+      default:
+        usage(argv[0]);
+        exit(-1);
+    }
+  }
+}
+
+bool test_zf_solver_gen(void) {
+  cf_t x0, x1, cf_error0, cf_error1;
+  float error;
+
+  cf_t x0_gold = RANDOM_CF();
+  cf_t x1_gold = RANDOM_CF();
+  cf_t h00 = RANDOM_CF();
+  cf_t h01 = RANDOM_CF();
+  cf_t h10 = RANDOM_CF();
+  cf_t h11 = (1 - h01 * h10) / h00;
+  cf_t y0 = x0_gold * h00 + x1_gold * h01;
+  cf_t y1 = x0_gold * h10 + x1_gold * h11;
+
+  srslte_mat_2x2_zf_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 1.0f);
+
+  cf_error0 = x0 - x0_gold;
+  cf_error1 = x1 - x1_gold;
+  error = crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+          crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-6);
+}
+
+bool test_mmse_solver_gen(void) {
+  cf_t x0, x1, cf_error0, cf_error1;
+  float error;
+
+  cf_t x0_gold = RANDOM_CF();
+  cf_t x1_gold = RANDOM_CF();
+  cf_t h00 = RANDOM_CF();
+  cf_t h01 = RANDOM_CF();
+  cf_t h10 = RANDOM_CF();
+  cf_t h11 = (1 - h01 * h10) / h00;
+  cf_t y0 = x0_gold * h00 + x1_gold * h01;
+  cf_t y1 = x0_gold * h10 + x1_gold * h11;
+
+  srslte_mat_2x2_mmse_gen(y0, y1, h00, h01, h10, h11, &x0, &x1, 0.0f, 1.0f);
+
+  cf_error0 = x0 - x0_gold;
+  cf_error1 = x1 - x1_gold;
+  error = crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+          crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-6);
+}
+
+#ifdef LV_HAVE_SSE
+
+bool test_zf_solver_sse(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m128 _y0 = _mm_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m128 _y1 = _mm_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m128 _h00 = _mm_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m128 _h01 = _mm_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m128 _h10 = _mm_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m128 _h11 = _mm_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m128 _x0, _x1;
+
+  srslte_mat_2x2_zf_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+
+
+  __attribute__((aligned(128))) cf_t x0[2];
+  __attribute__((aligned(128))) cf_t x1[2];
+
+  _mm_store_ps((float *) x0, _x0);
+  _mm_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+bool test_mmse_solver_sse(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m128 _y0 = _mm_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m128 _y1 = _mm_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m128 _h00 = _mm_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m128 _h01 = _mm_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m128 _h10 = _mm_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m128 _h11 = _mm_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m128 _x0, _x1;
+
+  srslte_mat_2x2_mmse_sse(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+
+
+  __attribute__((aligned(128))) cf_t x0[2];
+  __attribute__((aligned(128))) cf_t x1[2];
+
+  _mm_store_ps((float *) x0, _x0);
+  _mm_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+
+bool test_zf_solver_avx(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m256 _y0 = _mm256_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2),
+                             cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m256 _y1 = _mm256_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2),
+                             cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m256 _h00 = _mm256_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2),
+                              cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m256 _h01 = _mm256_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2),
+                              cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m256 _h10 = _mm256_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2),
+                              cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m256 _h11 = _mm256_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2),
+                              cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m256 _x0, _x1;
+
+  srslte_mat_2x2_zf_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 1.0f);
+
+
+  __attribute__((aligned(256))) cf_t x0[4];
+  __attribute__((aligned(256))) cf_t x1[4];
+
+  _mm256_store_ps((float *) x0, _x0);
+  _mm256_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+bool test_mmse_solver_avx(void) {
+  cf_t cf_error0, cf_error1;
+  float error = 0.0f;
+
+  cf_t x0_gold_1 = RANDOM_CF();
+  cf_t x1_gold_1 = RANDOM_CF();
+  cf_t h00_1 = RANDOM_CF();
+  cf_t h01_1 = RANDOM_CF();
+  cf_t h10_1 = RANDOM_CF();
+  cf_t h11_1 = (1 - h01_1 * h10_1) / h00_1;
+  cf_t y0_1 = x0_gold_1 * h00_1 + x1_gold_1 * h01_1;
+  cf_t y1_1 = x0_gold_1 * h10_1 + x1_gold_1 * h11_1;
+
+  cf_t x0_gold_2 = RANDOM_CF();
+  cf_t x1_gold_2 = RANDOM_CF();
+  cf_t h00_2 = RANDOM_CF();
+  cf_t h01_2 = RANDOM_CF();
+  cf_t h10_2 = RANDOM_CF();
+  cf_t h11_2 = (1 - h01_2 * h10_2) / h00_2;
+  cf_t y0_2 = x0_gold_2 * h00_2 + x1_gold_2 * h01_2;
+  cf_t y1_2 = x0_gold_2 * h10_2 + x1_gold_2 * h11_2;
+
+  __m256 _y0 = _mm256_set_ps(cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2),
+                             cimagf(y0_1), crealf(y0_1), cimagf(y0_2), crealf(y0_2));
+  __m256 _y1 = _mm256_set_ps(cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2),
+                             cimagf(y1_1), crealf(y1_1), cimagf(y1_2), crealf(y1_2));
+
+  __m256 _h00 = _mm256_set_ps(cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2),
+                              cimagf(h00_1), crealf(h00_1), cimagf(h00_2), crealf(h00_2));
+  __m256 _h01 = _mm256_set_ps(cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2),
+                              cimagf(h01_1), crealf(h01_1), cimagf(h01_2), crealf(h01_2));
+  __m256 _h10 = _mm256_set_ps(cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2),
+                              cimagf(h10_1), crealf(h10_1), cimagf(h10_2), crealf(h10_2));
+  __m256 _h11 = _mm256_set_ps(cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2),
+                              cimagf(h11_1), crealf(h11_1), cimagf(h11_2), crealf(h11_2));
+
+  __m256 _x0, _x1;
+
+  srslte_mat_2x2_mmse_avx(_y0, _y1, _h00, _h01, _h10, _h11, &_x0, &_x1, 0.0f, 1.0f);
+
+
+  __attribute__((aligned(256))) cf_t x0[4];
+  __attribute__((aligned(256))) cf_t x1[4];
+
+  _mm256_store_ps((float *) x0, _x0);
+  _mm256_store_ps((float *) x1, _x1);
+
+  cf_error0 = x0[1] - x0_gold_1;
+  cf_error1 = x1[1] - x1_gold_1;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  cf_error0 = x0[0] - x0_gold_2;
+  cf_error1 = x1[0] - x1_gold_2;
+  error += crealf(cf_error0) * crealf(cf_error0) + cimagf(cf_error0) * cimagf(cf_error0) +
+           crealf(cf_error1) * crealf(cf_error1) + cimagf(cf_error1) * cimagf(cf_error1);
+
+  return (error < 1e-3);
+}
+
+#endif /* LV_HAVE_AVX */
+
+
+int main(int argc, char **argv) {
+  bool passed = true;
+  int ret = 0;
+
+  parse_args(argc, argv);
+
+  if (zf_solver) {
+    RUN_TEST(test_zf_solver_gen);
+
+#ifdef LV_HAVE_SSE
+    RUN_TEST(test_zf_solver_sse);
+#endif /* LV_HAVE_SSE */
+
+#ifdef LV_HAVE_AVX
+    RUN_TEST(test_zf_solver_avx);
+#endif /* LV_HAVE_AVX */
+  }
+
+  if (mmse_solver) {
+    RUN_TEST(test_mmse_solver_gen);
+
+#ifdef LV_HAVE_SSE
+    RUN_TEST(test_mmse_solver_sse);
+#endif /* LV_HAVE_SSE */
+
+
+#ifdef LV_HAVE_AVX
+    RUN_TEST(test_mmse_solver_avx);
+#endif /* LV_HAVE_AVX */
+  }
+
+  printf("%s!\n", (passed) ? "Ok" : "Failed");
+
+  if (!passed) {
+    exit(ret);
+  }
+
+  exit(ret);
+}
diff --git a/lib/src/phy/utils/vector.c b/lib/src/phy/utils/vector.c
index b76c87645..d78f5d707 100644
--- a/lib/src/phy/utils/vector.c
+++ b/lib/src/phy/utils/vector.c
@@ -35,6 +35,16 @@
 #include "srslte/phy/utils/vector_simd.h"
 #include "srslte/phy/utils/bit.h"
 
+
+#ifdef LV_HAVE_SSE
+#include <smmintrin.h>
+#endif
+
+#ifdef LV_HAVE_AVX
+#include <immintrin.h>
+#endif
+
+
 #ifdef HAVE_VOLK
 #include "volk/volk.h"
 #endif
@@ -852,3 +862,24 @@ void srslte_vec_quant_suc(int16_t *in, uint8_t *out, float gain, int16_t offset,
   }
 }
 
+void srs_vec_cf_cpy(cf_t *dst, cf_t *src, int len) {
+  int i = 0;
+
+#ifdef LV_HAVE_AVX
+    for (; i < len - 3; i += 4) {
+      _mm256_store_ps((float *) &dst[i], _mm256_load_ps((float *) &src[i]));
+    }
+#endif /* LV_HAVE_AVX */
+#ifdef LV_HAVE_SSE
+    for (; i < len - 1; i += 2) {
+      _mm_store_ps((float *) &dst[i], _mm_load_ps((float *) &src[i]));
+    }
+  for (; i < len; i++) {
+    ((__m64*) dst)[i] = ((__m64*) src)[i];
+  }
+#else
+  for (; i < len; i++) {
+    dst[i] = src[i];
+  }
+#endif /* LV_HAVE_SSE */
+}
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index 97a66dd82..930c52d83 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -169,6 +169,8 @@ bool radio::has_rssi()
 
 bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
 {
+  void *iq_samples[4] = {(void *) zeros, (void *) zeros, (void *) zeros, (void *) zeros};
+
   if (!tx_adv_negative) {
     srslte_timestamp_sub(&tx_time, 0, tx_adv_sec);
   } else {
@@ -177,11 +179,11 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   
   if (is_start_of_burst) {
     if (burst_preamble_samples != 0) {
-      srslte_timestamp_t tx_time_pad; 
+      srslte_timestamp_t tx_time_pad;
       srslte_timestamp_copy(&tx_time_pad, &tx_time);
       srslte_timestamp_sub(&tx_time_pad, 0, burst_preamble_time_rounded); 
       save_trace(1, &tx_time_pad);
-      srslte_rf_send_timed2(&rf_device, zeros, burst_preamble_samples, tx_time_pad.full_secs, tx_time_pad.frac_secs, true, false);
+      srslte_rf_send_timed_multi(&rf_device, iq_samples, burst_preamble_samples, tx_time_pad.full_secs, tx_time_pad.frac_secs, true, true, false);
       is_start_of_burst = false; 
     }        
   }
@@ -191,7 +193,8 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   srslte_timestamp_add(&end_of_burst_time, 0, (double) nof_samples/cur_tx_srate); 
   
   save_trace(0, &tx_time);
-  int ret = srslte_rf_send_timed2(&rf_device, buffer, nof_samples+offset, tx_time.full_secs, tx_time.frac_secs, is_start_of_burst, false);
+  iq_samples[0] = buffer;
+  int ret = srslte_rf_send_timed_multi(&rf_device, (void**) iq_samples, nof_samples+offset, tx_time.full_secs, tx_time.frac_secs, true, is_start_of_burst, false);
   offset = 0; 
   is_start_of_burst = false; 
   if (ret > 0) {
diff --git a/srsenb/hdr/mac/mac_metrics.h b/srsenb/hdr/mac/mac_metrics.h
index 4e3452ae1..35a65f8ae 100644
--- a/srsenb/hdr/mac/mac_metrics.h
+++ b/srsenb/hdr/mac/mac_metrics.h
@@ -43,6 +43,7 @@ struct mac_metrics_t
   int rx_brate;
   int ul_buffer;
   int dl_buffer;
+  float dl_cqi;
   float phr; 
 };
 
diff --git a/srsenb/hdr/mac/scheduler_ue.h b/srsenb/hdr/mac/scheduler_ue.h
index b95e5dda2..b59461140 100644
--- a/srsenb/hdr/mac/scheduler_ue.h
+++ b/srsenb/hdr/mac/scheduler_ue.h
@@ -40,7 +40,10 @@ class sched_ue {
 public: 
   
   // used by sched_metric
-  uint32_t ue_idx;   
+  uint32_t ue_idx;
+
+  bool has_pusch;
+  bool has_pucch;
   
   typedef struct {
     uint32_t cce_start[4][6];
diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index b98b07f9d..5805e5472 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -48,8 +48,9 @@ public:
     log_h = NULL; 
     rnti  = 0; 
     pcap  = NULL;
-    nof_failures = 0; 
-    phr_counter = 0; 
+    nof_failures   = 0;
+    phr_counter    = 0;
+    dl_cqi_counter = 0;
     is_phy_added = false; 
     for (int i=0;i<NOF_HARQ_PROCESSES;i++) {
       pending_buffers[i] = NULL; 
@@ -91,8 +92,10 @@ public:
   void metrics_read(srsenb::mac_metrics_t* metrics);
   void metrics_rx(bool crc, uint32_t tbs);
   void metrics_tx(bool crc, uint32_t tbs);
-  
-  
+  void metrics_phr(float phr);
+  void metrics_dl_cqi(uint32_t dl_cqi);
+
+
   bool is_phy_added;
 
 private: 
@@ -100,12 +103,11 @@ private:
   void allocate_sdu(srslte::sch_pdu *pdu, uint32_t lcid, uint32_t sdu_len);   
   bool process_ce(srslte::sch_subh *subh); 
   void allocate_ce(srslte::sch_pdu *pdu, uint32_t lcid);
-  
-  void metrics_phr(float phr); 
-  uint32_t phr_counter;
 
   std::vector<uint32_t> lc_groups[4];
-  
+
+  uint32_t phr_counter;
+  uint32_t dl_cqi_counter;
   mac_metrics_t metrics;
   
   srslte::mac_pcap* pcap;
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index fb2c3110e..63d189c20 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -329,6 +329,7 @@ int mac::cqi_info(uint32_t tti, uint16_t rnti, uint32_t cqi_value)
 
   if (ue_db.count(rnti)) {         
     scheduler.dl_cqi_info(tti, rnti, cqi_value);
+    ue_db[rnti]->metrics_dl_cqi(cqi_value);
   } else {
     Error("User rnti=0x%x not found\n", rnti);
     return -1;
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 12a7ea1cc..73dbc2d41 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -691,7 +691,10 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
   for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
     sched_ue *user = (sched_ue*) &iter->second;
     uint16_t rnti  = (uint16_t) iter->first; 
-    
+
+    user->has_pusch = false;
+    user->has_pucch = false;
+
     ul_harq_proc *h = user->get_ul_harq(current_tti);
   
     /* Indicate PHICH acknowledgment if needed */
@@ -717,9 +720,13 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
     uint32_t prb_idx[2] = {0, 0}; 
     uint32_t L = 0; 
     if (user->get_pucch_sched(current_tti, prb_idx, &L)) {
-      for (int i=0;i<2;i++) {
-        ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], L};
-        ul_metric->update_allocation(pucch);        
+      user->has_pucch = true;
+      // allocate PUCCH if no PUSCH for user
+      if (!user->has_pusch) {
+        for (int i=0;i<2;i++) {
+          ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], L};
+          ul_metric->update_allocation(pucch);
+        }
       }
     }
   }
diff --git a/srsenb/src/mac/scheduler_harq.cc b/srsenb/src/mac/scheduler_harq.cc
index 009872153..a6ae70d19 100644
--- a/srsenb/src/mac/scheduler_harq.cc
+++ b/srsenb/src/mac/scheduler_harq.cc
@@ -100,7 +100,7 @@ void harq_proc::set_ack(bool ack_)
   ack = ack_;   
   ack_received = true;
   log_h->debug("ACK=%d received pid=%d, n_rtx=%d, max_retx=%d\n", ack_, id, n_rtx, max_retx);
-  if (n_rtx >= max_retx) {
+  if (n_rtx + 1 >= max_retx) {
     Warning("SCHED: discarting TB pid=%d, tti=%d, maximum number of retx exceeded (%d)\n", id, tti, max_retx);
     active = false;      
   }
diff --git a/srsenb/src/mac/scheduler_metric.cc b/srsenb/src/mac/scheduler_metric.cc
index 2a9a4432b..309eed45a 100644
--- a/srsenb/src/mac/scheduler_metric.cc
+++ b/srsenb/src/mac/scheduler_metric.cc
@@ -214,7 +214,8 @@ void ul_metric_rr::new_tti(std::map<uint16_t,sched_ue> &ue_db, uint32_t nof_rb_,
   for(std::map<uint16_t, sched_ue>::iterator iter=ue_db.begin(); iter!=ue_db.end(); ++iter) {
     sched_ue *user      = (sched_ue*) &iter->second;
     if (user->get_pending_ul_new_data(current_tti) || !user->get_ul_harq(current_tti)->is_empty()) {
-      user->ue_idx = nof_users_with_data;
+      user->ue_idx    = nof_users_with_data;
+      user->has_pusch = true;
       nof_users_with_data++; 
     }
   }
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 21aa61b19..9a258a1a3 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -257,7 +257,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
       if (L) {
         *L = 1; 
       }
-      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
+      Debug("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
       return true; 
     }
   }
@@ -271,7 +271,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
     if (L) {
       *L = 1; 
     }
-    Info("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
+    Debug("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
     return true; 
   }
   // Finally check Format2 (periodic CQI)
@@ -284,7 +284,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
     if(L) {
       *L = 2;
     }
-    Info("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
+    Debug("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
          rnti, prb_idx[0], prb_idx[1], cfg.cqi_pucch, cfg.cqi_idx);
     return true;
   }
@@ -813,6 +813,11 @@ int sched_ue::alloc_tbs(uint32_t nof_prb,
   uint32_t max_mcs = is_ul?max_mcs_ul:max_mcs_dl;
   uint32_t max_Qm  = is_ul?4:6; // Allow 16-QAM in PUSCH Only
 
+  // TODO: Compute real spectral efficiency based on PUSCH-UCI configuration
+  if (has_pucch) {
+    cqi-=2;
+  }
+
   int tbs = cqi_to_tbs(cqi, nof_prb, nof_re, max_mcs, max_Qm, &sel_mcs)/8;
   
   /* If less bytes are requested, lower the MCS */
diff --git a/srsenb/src/mac/ue.cc b/srsenb/src/mac/ue.cc
index 0b9776918..14944b6d4 100644
--- a/srsenb/src/mac/ue.cc
+++ b/srsenb/src/mac/ue.cc
@@ -396,14 +396,20 @@ void ue::metrics_read(mac_metrics_t* metrics_)
   metrics.dl_buffer = sched->get_dl_buffer(rnti);
   
   memcpy(metrics_, &metrics, sizeof(mac_metrics_t));
-  
-  phr_counter = 0; 
+
+  phr_counter = 0;
+  dl_cqi_counter = 0;
   bzero(&metrics, sizeof(mac_metrics_t));  
 }
 
 void ue::metrics_phr(float phr) {
   metrics.phr = SRSLTE_VEC_CMA(phr, metrics.phr, phr_counter);
-  phr_counter++; 
+  phr_counter++;
+}
+
+void ue::metrics_dl_cqi(uint32_t dl_cqi) {
+  metrics.dl_cqi = SRSLTE_VEC_CMA((float) dl_cqi, metrics.dl_cqi, dl_cqi_counter);
+  dl_cqi_counter++;
 }
 
 void ue::metrics_rx(bool crc, uint32_t tbs)
diff --git a/srsenb/src/main.cc b/srsenb/src/main.cc
index 1e337b3ba..528878388 100644
--- a/srsenb/src/main.cc
+++ b/srsenb/src/main.cc
@@ -373,6 +373,7 @@ int main(int argc, char *argv[])
   pthread_cancel(input);
   metrics.stop();
   enb->stop();
+  enb->cleanup();
   cout << "---  exiting  ---" << endl;
   exit(0);
 }
diff --git a/srsenb/src/metrics_stdout.cc b/srsenb/src/metrics_stdout.cc
index dc6b6d575..6c294b96d 100644
--- a/srsenb/src/metrics_stdout.cc
+++ b/srsenb/src/metrics_stdout.cc
@@ -108,8 +108,8 @@ void metrics_stdout::print_metrics()
   {
     n_reports = 0;
     cout << endl;
-    cout << "------DL-------------------UL----------------" << endl;
-    cout << "rnti   mcs   brate   bler  snr  phr  turbo  mcs  brate   bler" << endl;
+    cout << "------DL-------------------------UL-------------------------------" << endl;
+    cout << "rnti   cqi   mcs  brate   bler   snr  phr   mcs  brate   bler   bsr" << endl;
   }
   if (metrics.rrc.n_ues > 0) {
     
@@ -122,6 +122,7 @@ void metrics_stdout::print_metrics()
       }
     
       cout << std::hex << metrics.mac[i].rnti << " ";
+      cout << float_to_string(metrics.mac[i].dl_cqi, 2);
       cout << float_to_string(metrics.phy[i].dl.mcs, 2);
       if (metrics.mac[i].tx_brate > 0 && metrics_report_period) {
         cout << float_to_eng_string((float) metrics.mac[i].tx_brate/metrics_report_period, 2);
@@ -135,7 +136,6 @@ void metrics_stdout::print_metrics()
       }
       cout << float_to_string(metrics.phy[i].ul.sinr, 2);
       cout << float_to_string(metrics.mac[i].phr, 2);
-      cout << float_to_string(metrics.phy[i].ul.turbo_iters, 2);
       cout << float_to_string(metrics.phy[i].ul.mcs, 2);
       if (metrics.mac[i].rx_brate > 0 && metrics_report_period) {
         cout << float_to_eng_string((float) metrics.mac[i].rx_brate/metrics_report_period, 2);
@@ -147,6 +147,7 @@ void metrics_stdout::print_metrics()
       } else {
         cout << float_to_string(0, 2) << "%";
       }
+      cout << float_to_eng_string(metrics.mac[i].ul_buffer, 2);
       cout << endl;
     }
   } else {
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 9b31d6a43..a18a96405 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -644,7 +644,7 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
       if (LOG_THIS(rnti)) { 
         uint8_t x = 0;
         uint8_t *ptr = grants[i].data;
-        uint32_t len = phy_grant.mcs.tbs/8;
+        uint32_t len = phy_grant.mcs[0].tbs / (uint32_t) 8;
         if (!ptr) {          
           ptr = &x;
           len = 1; 
@@ -652,17 +652,18 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
         log_h->info_hex(ptr, len,
                              "PDSCH: rnti=0x%x, l_crb=%2d, %s, harq=%d, tbs=%d, mcs=%d, rv=%d, tti_tx=%d\n", 
                              rnti, phy_grant.nof_prb, grant_str, grants[i].grant.harq_process, 
-                             phy_grant.mcs.tbs/8, phy_grant.mcs.idx, grants[i].grant.rv_idx, tti_tx);
+                             phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
-      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, grants[i].softbuffer, rnti, grants[i].grant.rv_idx, sf_idx, 
-                                  grants[i].data)) 
+      srslte_softbuffer_tx_t *sb[SRSLTE_MAX_CODEWORDS] = {grants[i].softbuffer, NULL};
+      uint8_t *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
+      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, grants[i].grant.rv_idx, sf_idx, d))
       {
         fprintf(stderr, "Error putting PDSCH %d\n",i);
         return SRSLTE_ERROR; 
       }
 
       // Save metrics stats 
-      ue_db[rnti].metrics_dl(phy_grant.mcs.idx);
+      ue_db[rnti].metrics_dl(phy_grant.mcs[0].idx);
     }
   }
   return SRSLTE_SUCCESS; 
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 809ecd151..963551150 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -84,20 +84,20 @@ public:
         harq_pid = grant.pid%N;
       }
       if (grant.rnti_type == SRSLTE_RNTI_TEMP && last_temporal_crnti != grant.rnti) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
         Info("Set NDI=1 for Temp-RNTI DL grant\n");
         last_temporal_crnti = grant.rnti;
       }
       if (grant.rnti_type == SRSLTE_RNTI_USER && proc[harq_pid].is_sps()) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
         Info("Set NDI=1 for C-RNTI DL grant\n");
       }
       proc[harq_pid].new_grant_dl(grant, action);
     } else {
       /* This is for SPS scheduling */
       uint32_t harq_pid = get_harq_sps_pid(grant.tti)%N;
-      if (grant.ndi) {
-        grant.ndi = false; 
+      if (grant.ndi[0]) {
+        grant.ndi[0] = false;
         proc[harq_pid].new_grant_dl(grant, action);
       } else {
         if (grant.is_sps_release) {
@@ -117,12 +117,12 @@ public:
   }
 
 
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
   {
     if (rnti_type == SRSLTE_RNTI_SI) {
-      proc[N].tb_decoded(ack);
+      proc[N].tb_decoded(ack, 0);
     } else {
-      proc[harq_pid%N].tb_decoded(ack);
+      proc[harq_pid%N].tb_decoded(ack, tb_idx);
     }
   }
 
@@ -137,200 +137,238 @@ public:
 
   void start_pcap(srslte::mac_pcap* pcap_) { pcap = pcap_; }
 
-  int  get_current_tbs(uint32_t harq_pid) { return proc[harq_pid%N].get_current_tbs(); }
+  int  get_current_tbs(uint32_t harq_pid, uint32_t tb_idx) { return proc[harq_pid%N].get_current_tbs(tb_idx); }
 
   void set_si_window_start(int si_window_start_) { si_window_start = si_window_start_; }
 
   float get_average_retx() { return average_retx; }
 
-private:  
+private:
   class dl_harq_process {
   public:
-    dl_harq_process()
-    {
-      is_initiated = false; 
-      ack = false; 
-      bzero(&cur_grant, sizeof(Tgrant));
+    dl_harq_process() : subproc(SRSLTE_MAX_TB) {
+
     }
 
-    bool init(uint32_t pid_, dl_harq_entity *parent)
-    {
-      if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
-        Error("Error initiating soft buffer\n");
-        return false; 
-      } else {
-        pid = pid_;
-        is_initiated = true; 
-        harq_entity = parent; 
-        log_h = harq_entity->log_h; 
-        return true;
+    bool init(uint32_t pid_, dl_harq_entity *parent) {
+      bool ret = true;
+
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        ret &= subproc[tb].init(pid_, parent, tb);
       }
+      return ret;
     }
 
-    void reset()
-    {
-      ack = false; 
-      payload_buffer_ptr = NULL; 
-      bzero(&cur_grant, sizeof(Tgrant));
-      if (is_initiated) {
-        srslte_softbuffer_rx_reset(&softbuffer);
+    void reset(void) {
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        subproc[tb].reset();
       }
     }
-    
-    void new_grant_dl(Tgrant grant, Taction *action)
-    {
-      // Compute RV for BCCH when not specified in PDCCH format
-      if (pid == HARQ_BCCH_PID && grant.rv == -1) {
-        uint32_t k; 
-        if ((grant.tti/10)%2 == 0 && grant.tti%10 == 5) { // This is SIB1, k is different
-          k = (grant.tti/20)%4; 
-          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-        } else if (grant.rv == -1) {      
-          k = (grant.tti-harq_entity->si_window_start)%4; 
-          grant.rv = ((uint32_t) ceilf((float)1.5*k))%4;
-        }
-      }
-      calc_is_new_transmission(grant);
-      if (is_new_transmission) {
-        ack = false; 
-        srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes*8);
-        n_retx = 0; 
+
+    void new_grant_dl(Tgrant grant, Taction *action) {
+      for (uint32_t tb = 0; tb < grant.phy_grant.dl.nof_tb; tb++) {
+        subproc[tb].new_grant_dl(grant, action);
       }
-      
-      // Save grant 
-      grant.last_ndi = cur_grant.ndi; 
-      grant.last_tti = cur_grant.tti; 
-      memcpy(&cur_grant, &grant, sizeof(Tgrant));
-      
-      // Fill action structure 
-      bzero(action, sizeof(Taction));
-      action->default_ack = ack; 
-      action->generate_ack = true; 
-      action->decode_enabled = false; 
-      
-      // If data has not yet been successfully decoded
-      if (ack == false) {
-        
-        // Instruct the PHY To combine the received data and attempt to decode it
-        payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid, cur_grant.n_bytes);
-        action->payload_ptr = payload_buffer_ptr;
-        if (!action->payload_ptr) {
-          action->decode_enabled = false; 
-          Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes);
-          return;       
-        }    
-        action->decode_enabled = true;     
-        action->rv = cur_grant.rv; 
-        action->rnti = cur_grant.rnti; 
-        action->softbuffer = &softbuffer;     
-        memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
-        n_retx++; 
-        
-      } else {
-        Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+    }
+
+    int get_current_tbs(uint32_t tb_idx) { return subproc[tb_idx].get_current_tbs(); }
+
+    bool is_sps() { return false; }
+
+    void tb_decoded(bool ack_, uint32_t tb_idx) {
+      subproc[tb_idx].tb_decoded(ack_);
+    }
+
+  private:
+    class dl_tb_process {
+    public:
+      dl_tb_process(void) {
+        is_initiated = false;
+        ack = false;
+        bzero(&cur_grant, sizeof(Tgrant));
       }
-        
-      if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
-        // Do not generate ACK
-        Debug("Not generating ACK\n");
-        action->generate_ack = false;    
-      } else {
-        if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && ack == false) {
-          // Postpone ACK after contention resolution is resolved
-          action->generate_ack_callback = harq_entity->generate_ack_callback; 
-          action->generate_ack_callback_arg = harq_entity->demux_unit;
-          Debug("ACK pending contention resolution\n");
+
+      bool init(uint32_t pid_, dl_harq_entity *parent, uint32_t tb_idx) {
+        tid = tb_idx;
+        if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
+          Error("Error initiating soft buffer\n");
+          return false;
         } else {
-          Debug("Generating ACK\n");
+          pid = pid_;
+          is_initiated = true;
+          harq_entity = parent;
+          log_h = harq_entity->log_h;
+          return true;
         }
       }
-    }
-    
-    void tb_decoded(bool ack_)
-    {
-      ack = ack_;
-      if (ack == true) {
-        if (pid == HARQ_BCCH_PID) {
-          if (harq_entity->pcap) {
-            harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes, ack, cur_grant.tti);
+
+      void reset(void) {
+        ack = false;
+        payload_buffer_ptr = NULL;
+        bzero(&cur_grant, sizeof(Tgrant));
+        if (is_initiated) {
+          srslte_softbuffer_rx_reset(&softbuffer);
+        }
+      }
+
+      void new_grant_dl(Tgrant grant, Taction *action) {
+        // Compute RV for BCCH when not specified in PDCCH format
+        if (pid == HARQ_BCCH_PID && grant.rv[tid] == -1) {
+          uint32_t k;
+          if ((grant.tti / 10) % 2 == 0 && grant.tti % 10 == 5) { // This is SIB1, k is different
+            k = (grant.tti / 20) % 4;
+            grant.rv[tid] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+          } else if (grant.rv[tid] == -1) {
+            k = (grant.tti - harq_entity->si_window_start) % 4;
+            grant.rv[tid] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
           }
-          Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes);
-          harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-        } else {      
-          if (harq_entity->pcap) {
-            harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes, cur_grant.rnti, ack, cur_grant.tti);            
+        }
+        calc_is_new_transmission(grant);
+        if (is_new_transmission) {
+          ack = false;
+          srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes[tid] * 8);
+          n_retx = 0;
+        }
+
+        // Save grant
+        grant.last_ndi[tid] = cur_grant.ndi[tid];
+        grant.last_tti = cur_grant.tti;
+        memcpy(&cur_grant, &grant, sizeof(Tgrant));
+
+        // Fill action structure
+        bzero(action, sizeof(Taction));
+        action->default_ack = ack;
+        action->generate_ack = true;
+        action->decode_enabled = false;
+
+        // If data has not yet been successfully decoded
+        if (!ack) {
+
+          // Instruct the PHY To combine the received data and attempt to decode it
+          payload_buffer_ptr = harq_entity->demux_unit->request_buffer(pid * SRSLTE_MAX_TB + tid,
+                                                                       cur_grant.n_bytes[tid]);
+          action->payload_ptr[tid] = payload_buffer_ptr;
+          if (!action->payload_ptr) {
+            action->decode_enabled = false;
+            Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes[tid]);
+            return;
           }
-          if (ack) {
-            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n", cur_grant.n_bytes);
-              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes);
-            } else {
-              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes);
-              harq_entity->demux_unit->push_pdu(pid, payload_buffer_ptr, cur_grant.n_bytes, cur_grant.tti);
-
-              // Compute average number of retransmissions per packet 
-              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx, harq_entity->nof_pkts++); 
-            }
+          action->decode_enabled = true;
+          action->rv[tid] = cur_grant.rv[tid];
+          action->rnti = cur_grant.rnti;
+          action->softbuffers[tid] = &softbuffer;
+          memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
+          n_retx++;
+
+        } else {
+          Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+        }
+
+        if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
+          // Do not generate ACK
+          Debug("Not generating ACK\n");
+          action->generate_ack = false;
+        } else {
+          if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP && !ack) {
+            // Postpone ACK after contention resolution is resolved
+            action->generate_ack_callback = harq_entity->generate_ack_callback;
+            action->generate_ack_callback_arg = harq_entity->demux_unit;
+            Debug("ACK pending contention resolution\n");
+          } else {
+            Debug("Generating ACK\n");
           }
         }
-      } else {
-        harq_entity->demux_unit->deallocate(payload_buffer_ptr);
       }
-      
-      Info("DL %d:  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n", 
-           pid, is_new_transmission?"newTX":"reTX ", 
-           cur_grant.n_bytes, cur_grant.rv, ack?"OK":"KO", 
-           cur_grant.ndi, cur_grant.last_ndi, cur_grant.tti, cur_grant.last_tti);
-      
-      if (ack && pid == HARQ_BCCH_PID) {
-        reset();
-      }
-    }
 
-    bool is_sps() { return false; }
+      void tb_decoded(bool ack_) {
+        ack = ack_;
+        if (ack) {
+          if (pid == HARQ_BCCH_PID) {
+            if (harq_entity->pcap) {
+              harq_entity->pcap->write_dl_sirnti(payload_buffer_ptr, cur_grant.n_bytes[tid], ack, cur_grant.tti);
+            }
+            Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (BCCH)\n", cur_grant.n_bytes[tid]);
+            harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                              cur_grant.tti);
+          } else {
+            if (harq_entity->pcap) {
+              harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid], cur_grant.rnti, ack,
+                                                cur_grant.tti);
+            }
+            if (ack) {
+              if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
+                      cur_grant.n_bytes[tid]);
+                harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
+              } else {
+                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
+                harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                                  cur_grant.tti);
 
-    int get_current_tbs() { return cur_grant.n_bytes*8; }
-    
-  private: 
-    bool calc_is_new_transmission(Tgrant grant)
-    {
-      bool is_new_tb = true; 
-      if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes == cur_grant.n_bytes)) ||
-          pid == HARQ_BCCH_PID) 
-      {
-        is_new_tb = false; 
+                // Compute average number of retransmissions per packet
+                harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
+                                                           harq_entity->nof_pkts++);
+              }
+            }
+          }
+        } else {
+          harq_entity->demux_unit->deallocate(payload_buffer_ptr);
+        }
+
+        Info("DL %d (TB %d):  %s tbs=%d, rv=%d, ack=%s, ndi=%d (%d), tti=%d (%d)\n",
+             pid, tid, is_new_transmission ? "newTX" : "reTX ",
+             cur_grant.n_bytes[tid], cur_grant.rv[tid], ack ? "OK" : "KO",
+             cur_grant.ndi[tid], cur_grant.last_ndi[tid], cur_grant.tti, cur_grant.last_tti);
+
+        if (ack && pid == HARQ_BCCH_PID) {
+          reset();
+        }
       }
-      
-      if ((grant.ndi != cur_grant.ndi && !is_new_tb) || // NDI toggled for same TB
-          is_new_tb                                  || // is new TB
-          (pid == HARQ_BCCH_PID && grant.rv == 0))      // Broadcast PID and 1st TX (RV=0)
-      {
-        is_new_transmission = true; 
-        Debug("Set HARQ for new transmission\n");
-      } else {
-        is_new_transmission = false; 
-        Debug("Set HARQ for retransmission\n");
+
+      int get_current_tbs(void) { return cur_grant.n_bytes[tid] * 8; }
+
+    private:
+      bool calc_is_new_transmission(Tgrant grant) {
+        bool is_new_tb = true;
+        if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes[tid] == cur_grant.n_bytes[tid])) ||
+            pid == HARQ_BCCH_PID) {
+          is_new_tb = false;
+        }
+
+        if ((grant.ndi[tid] != cur_grant.ndi[tid] && !is_new_tb) || // NDI toggled for same TB
+            is_new_tb || // is new TB
+            (pid == HARQ_BCCH_PID && grant.rv[tid] == 0))      // Broadcast PID and 1st TX (RV=0)
+        {
+          is_new_transmission = true;
+          Debug("Set HARQ for new transmission\n");
+        } else {
+          is_new_transmission = false;
+          Debug("Set HARQ for retransmission\n");
+        }
+
+        return is_new_transmission;
       }
 
-      return is_new_transmission;
-    }
+      bool is_initiated;
+      dl_harq_entity *harq_entity;
+      srslte::log *log_h;
 
-    bool            is_initiated; 
-    dl_harq_entity *harq_entity; 
-    srslte::log    *log_h;
-    
-    bool            is_new_transmission; 
-    
-    uint32_t        pid;    
-    uint8_t        *payload_buffer_ptr; 
-    bool            ack;
-    
-    uint32_t        n_retx; 
-    
-    Tgrant                         cur_grant;
-    srslte_softbuffer_rx_t         softbuffer; 
+      bool is_new_transmission;
+
+      uint32_t pid;     /* HARQ Proccess ID   */
+      uint32_t tid;     /* Transport block ID */
+      uint8_t *payload_buffer_ptr;
+      bool ack;
+
+      uint32_t n_retx;
+
+      Tgrant cur_grant;
+      srslte_softbuffer_rx_t softbuffer;
+    };
+
+    /* Transport blocks */
+    std::vector<dl_tb_process> subproc;
   };
-  
   // Private members of dl_harq_entity
   
   static bool      generate_ack_callback(void *arg)
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index 835a5614e..51bb467f2 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -65,7 +65,7 @@ public:
   void new_grant_ul_ack(mac_grant_t grant, bool ack, tb_action_ul_t *action);
   void harq_recv(uint32_t tti, bool ack, tb_action_ul_t *action);
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action);
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid);
   void bch_decoded_ok(uint8_t *payload, uint32_t len);
   void pch_decoded_ok(uint32_t len);    
   void tti_clock(uint32_t tti);
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 2ef1ed2b6..952b9f002 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -113,12 +113,12 @@ public:
       grant.rnti_type == SRSLTE_RNTI_RAR)
     {
       if (grant.rnti_type == SRSLTE_RNTI_USER && proc[pidof(grant.tti)].is_sps()) {
-        grant.ndi = true;
+        grant.ndi[0] = true;
       }
       run_tti(grant.tti, &grant, action);
     } else if (grant.rnti_type == SRSLTE_RNTI_SPS) {
-      if (grant.ndi) {
-        grant.ndi = proc[pidof(grant.tti)].get_ndi();
+      if (grant.ndi[0]) {
+        grant.ndi[0] = proc[pidof(grant.tti)].get_ndi();
         run_tti(grant.tti, &grant, action);
       } else {
         Info("Not implemented\n");
@@ -208,7 +208,7 @@ private:
 
       // Receive and route HARQ feedbacks
       if (grant) {
-        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi != get_ndi()) ||
+        if ((!(grant->rnti_type == SRSLTE_RNTI_TEMP) && grant->ndi[0] != get_ndi()) ||
           (grant->rnti_type == SRSLTE_RNTI_USER && !has_grant())             ||
           grant->is_from_rar)
         {
@@ -216,8 +216,8 @@ private:
 
           // Uplink grant in a RAR
           if (grant->is_from_rar) {
-            Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes);
-            pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes);
+            Debug("Getting Msg3 buffer payload, grant size=%d bytes\n", grant->n_bytes[0]);
+            pdu_ptr  = harq_entity->mux_unit->msg3_get(payload_buffer, grant->n_bytes[0]);
             if (pdu_ptr) {
               generate_new_tx(tti_tx, true, grant, action);
             } else {
@@ -227,7 +227,7 @@ private:
             // Normal UL grant
           } else {
             // Request a MAC PDU from the Multiplexing & Assemble Unit
-            pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes, tti_tx, pid);
+            pdu_ptr = harq_entity->mux_unit->pdu_get(payload_buffer, grant->n_bytes[0], tti_tx, pid);
             if (pdu_ptr) {
               generate_new_tx(tti_tx, false, grant, action);
             } else {
@@ -258,7 +258,7 @@ private:
         if (grant->is_from_rar) {
           grant->rnti = harq_entity->rntis->temp_rnti;
         }
-        harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes, grant->rnti, get_nof_retx(), tti_tx);
+        harq_entity->pcap->write_ul_crnti(pdu_ptr, grant->n_bytes[0], grant->rnti, get_nof_retx(), tti_tx);
       }
     }
 
@@ -285,7 +285,7 @@ private:
     bool is_sps() { return false; }
     uint32_t last_tx_tti() { return tti_last_tx; }
     uint32_t get_nof_retx() { return current_tx_nb; }
-    int get_current_tbs() { return cur_grant.n_bytes*8; }
+    int get_current_tbs() { return cur_grant.n_bytes[0]*8; }
    
   private: 
     Tgrant                      cur_grant;
@@ -321,16 +321,16 @@ private:
       if (grant) {
         // HARQ entity requests an adaptive transmission
         if (grant->rv) {
-          current_irv = irv_of_rv[grant->rv%4];
+          current_irv = irv_of_rv[grant->rv[0]%4];
         }
         memcpy(&cur_grant, grant, sizeof(Tgrant));
         harq_feedback = false;
         Info("UL %d:  Adaptive retx=%d, RV=%d, TBS=%d\n",
-             pid, current_tx_nb, get_rv(), grant->n_bytes);
+             pid, current_tx_nb, get_rv(), grant->n_bytes[0]);
         generate_tx(tti_tx, action);
       } else {
         Info("UL %d:  Non-Adaptive retx=%d, RV=%d, TBS=%d\n",
-             pid, current_tx_nb, get_rv(), cur_grant.n_bytes);
+             pid, current_tx_nb, get_rv(), cur_grant.n_bytes[0]);
         // HARQ entity requests a non-adaptive transmission
         if (!harq_feedback) {
           generate_tx(tti_tx, action);
@@ -358,7 +358,7 @@ private:
         current_irv = 0;
         is_msg3 = is_msg3_;
         Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
-             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes, cur_grant.rnti);
+             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0], cur_grant.rnti);
         generate_tx(tti_tx, action);
       }
     }
@@ -370,10 +370,10 @@ private:
       current_tx_nb++;
       action->expect_ack = true;
       action->rnti = is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti;
-      action->rv = cur_grant.rv>0?cur_grant.rv:get_rv();
-      action->softbuffer = &softbuffer;
+      action->rv[0] = cur_grant.rv[0]>0?cur_grant.rv[0]:get_rv();
+      action->softbuffers = &softbuffer;
       action->tx_enabled = true;
-      action->payload_ptr = pdu_ptr;
+      action->payload_ptr[0] = pdu_ptr;
       memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
 
       current_irv = (current_irv+1)%4;
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index bb669408e..d81c4c2f6 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -78,8 +78,15 @@ private:
   /* ... for DL */
   bool decode_pdcch_ul(mac_interface_phy::mac_grant_t *grant);
   bool decode_pdcch_dl(mac_interface_phy::mac_grant_t *grant);
-  bool decode_phich(bool *ack); 
-  bool decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload, srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t pid);
+  bool decode_phich(bool *ack);
+
+  int decode_pdsch(srslte_ra_dl_grant_t *grant,
+                   uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                   int rv[SRSLTE_MAX_CODEWORDS],
+                   uint16_t rnti,
+                   uint32_t pid,
+                   bool acks[SRSLTE_MAX_CODEWORDS]);
 
   /* ... for UL */
   void encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, uint32_t current_tx_nb, srslte_softbuffer_tx_t *softbuffer, 
@@ -90,7 +97,7 @@ private:
   void set_uci_sr();
   void set_uci_periodic_cqi();
   void set_uci_aperiodic_cqi();
-  void set_uci_ack(bool ack);
+  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb);
   bool srs_is_ready_to_send();
   float set_power(float tx_power);
   void setup_tx_gain();
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 999be5de8..12054567f 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -44,7 +44,7 @@ namespace srsue {
 mac::mac() : ttisync(10240), 
              timers_db((uint32_t) NOF_MAC_TIMERS),
              mux_unit(MAC_NOF_HARQ_PROC),
-             demux_unit(MAC_NOF_HARQ_PROC),
+             demux_unit(SRSLTE_MAX_TB*MAC_NOF_HARQ_PROC),
              pdu_process_thread(&demux_unit)
 {
   started = false;  
@@ -240,17 +240,17 @@ void mac::pch_decoded_ok(uint32_t len)
   }
 }
 
-void mac::tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
+void mac::tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid)
 {
   if (rnti_type == SRSLTE_RNTI_RAR) {
     if (ack) {
       ra_procedure.tb_decoded_ok();
     }
   } else {
-    dl_harq.tb_decoded(ack, rnti_type, harq_pid);
+    dl_harq.tb_decoded(ack, tb_idx, rnti_type, harq_pid);
     if (ack) {
       pdu_process_thread.notify();
-      metrics.rx_brate += dl_harq.get_current_tbs(harq_pid);
+      metrics.rx_brate += dl_harq.get_current_tbs(harq_pid, tb_idx);
     } else {
       metrics.rx_errors++;
     }
@@ -268,12 +268,12 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
     action->generate_ack = false; 
     action->decode_enabled = true; 
     srslte_softbuffer_rx_reset_cb(&pch_softbuffer, 1);
-    action->payload_ptr = pch_payload_buffer;
-    action->softbuffer  = &pch_softbuffer;
+    action->payload_ptr[0] = pch_payload_buffer;
+    action->softbuffers[0]  = &pch_softbuffer;
     action->rnti = grant.rnti;
-    action->rv   = grant.rv; 
-    if (grant.n_bytes > pch_payload_buffer_sz) {
-      Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes, pch_payload_buffer_sz);
+    action->rv[0]   = grant.rv[0];
+    if (grant.n_bytes[0] > pch_payload_buffer_sz) {
+      Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes[0], pch_payload_buffer_sz);
       action->decode_enabled = false; 
     }
   } else {
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 273531f66..f9903e2eb 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -269,23 +269,23 @@ void ra_proc::step_pdcch_setup() {
 
 void ra_proc::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::tb_action_dl_t* action) 
 {
-  if (grant.n_bytes < MAX_RAR_PDU_LEN) {
+  if (grant.n_bytes[0] < MAX_RAR_PDU_LEN) {
     rDebug("DL grant found RA-RNTI=%d\n", ra_rnti);        
     action->decode_enabled = true; 
     action->default_ack = false; 
     action->generate_ack = false; 
-    action->payload_ptr = rar_pdu_buffer; 
+    action->payload_ptr[0] = rar_pdu_buffer;
     action->rnti = grant.rnti; 
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
-    action->rv = grant.rv;
-    action->softbuffer = &softbuffer_rar;
-    rar_grant_nbytes = grant.n_bytes;
+    action->rv[0] = grant.rv[0];
+    action->softbuffers[0] = &softbuffer_rar;
+    rar_grant_nbytes = grant.n_bytes[0];
     rar_grant_tti    = grant.tti; 
-    if (action->rv == 0) {
+    if (action->rv[0] == 0) {
       srslte_softbuffer_rx_reset(&softbuffer_rar);
     }
   } else {
-    rError("Received RAR grant exceeds buffer length (%d>%d)\n", grant.n_bytes, MAX_RAR_PDU_LEN);
+    rError("Received RAR grant exceeds buffer length (%d>%d)\n", grant.n_bytes[0], MAX_RAR_PDU_LEN);
     action->decode_enabled = false; 
     state = RESPONSE_ERROR;
   }
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 55389059b..7ff787a38 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -108,7 +108,7 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
     srslte_ue_sync_start_agc(&cs.ue_sync, callback_set_rx_gain, last_gain);
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl_measure, SRSLTE_MAX_PRB, nof_rx_antennas)) {
+  if (srslte_ue_dl_init(&ue_dl_measure, SRSLTE_MAX_PRB, nof_rx_antennas)) {
     Error("Initiating ue_dl_measure\n");
     return;
   }
@@ -358,7 +358,7 @@ int phch_recv::cell_meas_rsrp() {
 
   int sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
   if (sync_res == 1) {
-    if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
+    if (srslte_ue_dl_decode_fft_estimate(&ue_dl_measure, sf_buffer, sf_idx, &cfi)) {
       log_h->error("SYNC:  Measuring RSRP: Estimating channel\n");
       return -1;
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index ecbbc8768..1437f7fce 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -40,9 +40,8 @@
 #ifdef ENABLE_GUI
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
-#include <srslte/srslte.h>
-#include <srslte/phy/phch/pdsch_cfg.h>
-#include <srslte/phy/phch/ra.h>
+#include "srslte/srslte.h"
+#include "srslte/interfaces/ue_interfaces.h"
 
 void init_plots(srsue::phch_worker *worker);
 pthread_t plot_thread; 
@@ -118,7 +117,7 @@ bool phch_worker::init(uint32_t max_prb)
     }
   }
 
-  if (srslte_ue_dl_init_multi(&ue_dl, max_prb, phy->args->nof_rx_ant)) {
+  if (srslte_ue_dl_init(&ue_dl, max_prb, phy->args->nof_rx_ant)) {
     Error("Initiating UE DL\n");
     return false;
   }
@@ -206,8 +205,8 @@ void phch_worker::work_imp()
   reset_uci();
 
   bool dl_grant_available = false; 
-  bool ul_grant_available = false; 
-  bool dl_ack = false;
+  bool ul_grant_available = false;
+  bool dl_ack[SRSLTE_MAX_CODEWORDS] = {false};
 
   mac_interface_phy::mac_grant_t    dl_mac_grant;
   mac_interface_phy::tb_action_dl_t dl_action; 
@@ -228,22 +227,30 @@ void phch_worker::work_imp()
     if(dl_grant_available) {
       /* Send grant to MAC and get action for this TB */
       phy->mac->new_grant_dl(dl_mac_grant, &dl_action);
-      
+
+      /* Set DL ACKs to default */
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+        dl_ack[tb] = dl_action.default_ack;
+      }
+
       /* Decode PDSCH if instructed to do so */
-      dl_ack = dl_action.default_ack; 
       if (dl_action.decode_enabled) {
-        dl_ack = decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr, 
-                              dl_action.softbuffer, dl_action.rv, dl_action.rnti, 
-                              dl_mac_grant.pid);              
+        decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr,
+                      dl_action.softbuffers, dl_action.rv, dl_action.rnti,
+                      dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-        phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
-        dl_ack = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-        Debug("Calling generate ACK callback returned=%d\n", dl_ack);
+
+        // NOTE: Currently hard-coded to 1st TB only
+        for (uint32_t tb = 0; tb < 1; tb++) {
+          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+          dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
+          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+        }
       }
-      Debug("dl_ack=%d, generate_ack=%d\n", dl_ack, dl_action.generate_ack);
+      Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);
       if (dl_action.generate_ack) {
-        set_uci_ack(dl_ack);
+        set_uci_ack(dl_ack, dl_mac_grant.phy_grant.dl.nof_tb);
       }
     }
   }
@@ -286,8 +293,8 @@ void phch_worker::work_imp()
   /* Transmit PUSCH, PUCCH or SRS */
   bool signal_ready = false; 
   if (ul_action.tx_enabled) {
-    encode_pusch(&ul_action.phy_grant.ul, ul_action.payload_ptr, ul_action.current_tx_nb, 
-                 ul_action.softbuffer, ul_action.rv, ul_action.rnti, ul_mac_grant.is_from_rar);          
+    encode_pusch(&ul_action.phy_grant.ul, ul_action.payload_ptr[0], ul_action.current_tx_nb,
+                 &ul_action.softbuffers[0], ul_action.rv[0], ul_action.rnti, ul_mac_grant.is_from_rar);
     signal_ready = true; 
     if (ul_action.expect_ack) {
       phy->set_pending_ack(tti + 8, ue_ul.pusch_cfg.grant.n_prb_tilde[0], ul_action.phy_grant.ul.ncs_dmrs);
@@ -307,9 +314,11 @@ void phch_worker::work_imp()
   
   if (dl_action.decode_enabled && !dl_action.generate_ack_callback) {
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
-      phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes);
+      phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
-      phy->mac->tb_decoded(dl_ack, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      for (uint32_t tb = 0; tb < dl_mac_grant.phy_grant.dl.nof_tb; tb++) {
+        phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      }
     }
   }
 
@@ -349,7 +358,7 @@ bool phch_worker::extract_fft_and_pdcch_llr() {
       srslte_chest_dl_set_noise_alg(&ue_dl.chest, SRSLTE_NOISE_ALG_PSS);      
     }
   
-    if (srslte_ue_dl_decode_fft_estimate_multi(&ue_dl, signal_buffer, tti%10, &cfi) < 0) {
+    if (srslte_ue_dl_decode_fft_estimate(&ue_dl, signal_buffer, tti%10, &cfi) < 0) {
       Error("Getting PDCCH FFT estimate\n");
       return false; 
     }        
@@ -398,7 +407,8 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     
     Debug("Looking for RNTI=0x%x\n", dl_rnti);
     
-    if (srslte_ue_dl_find_dl_dci_type(&ue_dl, cfi, tti%10, dl_rnti, type, &dci_msg) != 1) {
+    if (srslte_ue_dl_find_dl_dci_type(&ue_dl, phy->config->dedicated.antenna_info_explicit_value.tx_mode, cfi, tti%10,
+                                      dl_rnti, type, &dci_msg) != 1) {
       return false; 
     }
     
@@ -408,13 +418,16 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     }
 
     /* Fill MAC grant structure */
-    grant->ndi = dci_unpacked.ndi;
+    grant->ndi[0] = dci_unpacked.ndi;
+    grant->ndi[1] = dci_unpacked.ndi_1;
     grant->pid = dci_unpacked.harq_process;
-    grant->n_bytes = grant->phy_grant.dl.mcs.tbs/8;
-    grant->tti = tti; 
-    grant->rv  = dci_unpacked.rv_idx;
-    grant->rnti = dl_rnti; 
-    grant->rnti_type = type; 
+    grant->n_bytes[0] = grant->phy_grant.dl.mcs[0].tbs / (uint32_t) 8;
+    grant->n_bytes[1] = grant->phy_grant.dl.mcs[1].tbs / (uint32_t) 8;
+    grant->tti = tti;
+    grant->rv[0] = dci_unpacked.rv_idx;
+    grant->rv[1] = dci_unpacked.rv_idx_1;
+    grant->rnti = dl_rnti;
+    grant->rnti_type = type;
     grant->last_tti = 0;
     
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
@@ -433,18 +446,77 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
   }
 }
 
-bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload, 
-                               srslte_softbuffer_rx_t* softbuffer, int rv, uint16_t rnti, uint32_t harq_pid)
-{
+int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSLTE_MAX_CODEWORDS],
+                                     srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS],
+                                     int rv[SRSLTE_MAX_CODEWORDS],
+                                     uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
   char timestr[64];
+  bool valid_config = true;
   timestr[0]='\0';
-  
+  srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+  int ret = SRSLTE_SUCCESS;
+
+  for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
+    if (rv[tb] < 0 || rv[tb] > 3) {
+      valid_config = false;
+      Error("Wrong RV (%d) for TB index %d", rv[tb], tb);
+    }
+  }
+
+  switch(phy->config->dedicated.antenna_info_explicit_value.tx_mode) {
+    /* Implemented Tx Modes */
+    case LIBLTE_RRC_TRANSMISSION_MODE_1:
+      mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_2:
+      if (cell.nof_ports > 1) {
+        mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      } else {
+        mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+      }
+      break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_3:
+      if (grant->nof_tb == 1) {
+        mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
+      } else if (grant->nof_tb == 2) {
+        mimo_type = SRSLTE_MIMO_TYPE_CDD;
+      } else {
+        Error("Wrong number of transport blocks (%d) for TM3\n", grant->nof_tb);
+        valid_config = false;
+      }
+      break;
+    case LIBLTE_RRC_TRANSMISSION_MODE_4:
+      if (grant->nof_tb == 1) {
+        mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+      } else if (grant->nof_tb == 2) {
+        mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
+      } else {
+        Error("Wrong number of transport blocks (%d) for TM4\n", grant->nof_tb);
+        valid_config = false;
+      }
+    break;
+
+    /* Not implemented cases */
+    case LIBLTE_RRC_TRANSMISSION_MODE_5:
+    case LIBLTE_RRC_TRANSMISSION_MODE_6:
+    case LIBLTE_RRC_TRANSMISSION_MODE_7:
+    case LIBLTE_RRC_TRANSMISSION_MODE_8:
+      Error("Not implemented Tx mode (%d)\n", phy->config->dedicated.antenna_info_explicit_value.tx_mode);
+      break;
+
+    /* Error cases */
+    case LIBLTE_RRC_TRANSMISSION_MODE_N_ITEMS:
+    default:
+      Error("Wrong Tx mode (%d)\n", phy->config->dedicated.antenna_info_explicit_value.tx_mode);
+      valid_config = false;
+  }
+
   Debug("DL Buffer TTI %d: Decoding PDSCH\n", tti);
 
   /* Setup PDSCH configuration for this CFI, SFIDX and RVIDX */
-  if (rv >= 0 && rv <= 3) {
-    if (!srslte_ue_dl_cfg_grant(&ue_dl, grant, cfi, tti%10, rv)) {
-      if (ue_dl.pdsch_cfg.grant.mcs.mod > 0 && ue_dl.pdsch_cfg.grant.mcs.tbs >= 0) {
+  if (valid_config) {
+    if (!srslte_ue_dl_cfg_grant(&ue_dl, grant, cfi, tti%10, rv, mimo_type)) {
+      if (ue_dl.pdsch_cfg.grant.mcs[0].mod > 0 && ue_dl.pdsch_cfg.grant.mcs[0].tbs >= 0) {
         
         float noise_estimate = srslte_chest_dl_get_noise_estimate(&ue_dl.chest);
         
@@ -454,7 +526,7 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
         
         /* Set decoder iterations */
         if (phy->args->pdsch_max_its > 0) {
-          srslte_sch_set_max_noi(&ue_dl.pdsch.dl_sch, phy->args->pdsch_max_its);
+          srslte_pdsch_set_max_noi(&ue_dl.pdsch, phy->args->pdsch_max_its);
         }
 
 
@@ -462,37 +534,36 @@ bool phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload,
         struct timeval t[3];
         gettimeofday(&t[1], NULL);
   #endif
-
-        bool ack = srslte_pdsch_decode_multi(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffer, ue_dl.sf_symbols_m,
-                                      ue_dl.ce_m, noise_estimate, rnti, payload) == 0;
+        ret = srslte_pdsch_decode(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
+                                  ue_dl.ce_m, noise_estimate, rnti, payload, acks);
+        if (ret) {
+          Error("Decoding PDSCH");
+        }
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
         get_time_interval(t);
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info("PDSCH: l_crb=%2d, harq=%d, tbs=%d, mcs=%d, rv=%d, crc=%s, snr=%.1f dB, n_iter=%d%s\n",
-              grant->nof_prb, harq_pid,
-              grant->mcs.tbs/8, grant->mcs.idx, rv, 
-              ack?"OK":"KO", 
-              10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)), 
-              srslte_pdsch_last_noi(&ue_dl.pdsch),
-              timestr);
+        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
+             grant->nof_prb, harq_pid,
+             grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
+             rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
+             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+             srslte_pdsch_last_noi(&ue_dl.pdsch),
+             timestr);
 
         // Store metrics
-        dl_metrics.mcs    = grant->mcs.idx;
-        
-        return ack; 
+        dl_metrics.mcs    = grant->mcs[0].idx;
       } else {
         Warning("Received grant for TBS=0\n");
       }
     } else {
-      Error("Error configuring DL grant\n"); 
+      Error("Error configuring DL grant\n");
+      ret = SRSLTE_ERROR;
     }
-  } else {
-    Error("Error RV is not set or is invalid (%d)\n", rv);
   }
-  return true; 
+  return ret;
 }
 
 bool phch_worker::decode_phich(bool *ack)
@@ -589,12 +660,12 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
   }
   
   if (ret) {    
-    grant->ndi = dci_unpacked.ndi;
+    grant->ndi[0] = dci_unpacked.ndi;
     grant->pid = 0; // This is computed by MAC from TTI 
-    grant->n_bytes = grant->phy_grant.ul.mcs.tbs/8;
+    grant->n_bytes[0] = grant->phy_grant.ul.mcs.tbs / (uint32_t) 8;
     grant->tti = tti; 
     grant->rnti = ul_rnti; 
-    grant->rv = dci_unpacked.rv_idx;
+    grant->rv[0] = dci_unpacked.rv_idx;
     if (SRSLTE_VERBOSE_ISINFO()) {
       srslte_ra_pusch_fprint(stdout, &dci_unpacked, cell.nof_prb);
     }
@@ -610,11 +681,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-void phch_worker::set_uci_ack(bool ack)
-{
-  uci_data.uci_ack = ack?1:0;
-  uci_data.uci_ack_len = 1; 
-}
+  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
+    if (nof_tb > 0) {
+      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    }
+
+    if (nof_tb > 1) {
+      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    }
+
+    if (nof_tb > 2) {
+      Error("Number of transport blocks is not supported");
+    }
+
+    uci_data.uci_ack_len = nof_tb;
+
+  }
 
 void phch_worker::set_uci_sr()
 {
@@ -992,8 +1074,9 @@ int phch_worker::read_ce_abs(float *ce_abs) {
 
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
 {
-  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits.nof_re*sizeof(cf_t));
-  return ue_dl.pdsch_cfg.nbits.nof_re; 
+
+  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  return ue_dl.pdsch_cfg.nbits[0].nof_re;
 }
 
 
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index 1e4665f80..42e04c495 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -158,8 +158,8 @@ int rar_unpack(uint8_t *buffer, rar_msg_t *msg)
 srsue::phy my_phy;
 bool bch_decoded = false; 
 
-uint8_t payload[10240]; 
-uint8_t payload_bits[10240]; 
+uint8_t payload[SRSLTE_MAX_TB][10240];
+uint8_t payload_bits[SRSLTE_MAX_TB][10240];
 const uint8_t conn_request_msg[] = {0x20, 0x06, 0x1F, 0x5C, 0x2C, 0x04, 0xB2, 0xAC, 0xF6, 0x00, 0x00, 0x00};
 
 enum mac_state {RA, RAR, CONNREQUEST, CONNSETUP} state = RA; 
@@ -168,7 +168,7 @@ uint32_t preamble_idx = 0;
 rar_msg_t rar_msg;
 
 uint32_t nof_rtx_connsetup = 0; 
-uint32_t rv_value[4] = {0, 2, 3, 1}; 
+uint32_t rv_value[4] = {0, 2, 3, 1};
 
 void config_phy() {
   srsue::phy_interface_rrc::phy_cfg_t config; 
@@ -194,8 +194,8 @@ void config_phy() {
   my_phy.configure_prach_params();
 }
 
-srslte_softbuffer_rx_t softbuffer_rx; 
-srslte_softbuffer_tx_t softbuffer_tx; 
+srslte_softbuffer_rx_t softbuffers_rx[SRSLTE_MAX_TB];
+srslte_softbuffer_tx_t softbuffers_tx[SRSLTE_MAX_TB];
 
 uint16_t temp_c_rnti; 
 
@@ -218,7 +218,7 @@ public:
   
   bool rar_rnti_set;
 
-  void pch_decoded_ok(uint32_t len) {} 
+  void pch_decoded_ok(uint32_t len) {}
 
   
   void tti_clock(uint32_t tti) {
@@ -233,19 +233,21 @@ public:
   
   void new_grant_ul(mac_grant_t grant, tb_action_ul_t *action) {
     printf("New grant UL\n");
-    memcpy(payload, conn_request_msg, grant.n_bytes*sizeof(uint8_t));
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+      memcpy(payload[tb], conn_request_msg, grant.n_bytes[tb]*sizeof(uint8_t));
+      action->rv[tb] = rv_value[nof_rtx_connsetup%4];
+      action->payload_ptr[tb] = payload[tb];
+      if (action->rv[tb] == 0) {
+        srslte_softbuffer_tx_reset(&softbuffers_tx[tb]);
+      }
+    }
     action->current_tx_nb = nof_rtx_connsetup;
-    action->rv = rv_value[nof_rtx_connsetup%4];
-    action->softbuffer = &softbuffer_tx;     
+    action->softbuffers = softbuffers_tx;
     action->rnti = temp_c_rnti; 
     action->expect_ack = (nof_rtx_connsetup < 5)?true:false;
-    action->payload_ptr = payload;
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     memcpy(&last_grant, &grant, sizeof(mac_grant_t));
     action->tx_enabled = true; 
-    if (action->rv == 0) {
-      srslte_softbuffer_tx_reset(&softbuffer_tx);      
-    }
     my_phy.pdcch_dl_search(SRSLTE_RNTI_USER, temp_c_rnti);
   }
   
@@ -258,17 +260,20 @@ public:
     if (!ack) {
       nof_rtx_connsetup++;
       action->current_tx_nb = nof_rtx_connsetup;
-      action->rv = rv_value[nof_rtx_connsetup%4];
-      action->softbuffer = &softbuffer_tx;     
+      action->softbuffers = softbuffers_tx;
       action->rnti = temp_c_rnti; 
       action->expect_ack = true; 
       memcpy(&action->phy_grant, &last_grant.phy_grant, sizeof(srslte_phy_grant_t));
-      action->tx_enabled = true; 
-      if (action->rv == 0) {
-        srslte_softbuffer_tx_reset(&softbuffer_tx);      
+      action->tx_enabled = true;
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+        action->rv[tb] = rv_value[nof_rtx_connsetup%4];
+        if (action->rv[tb] == 0) {
+          srslte_softbuffer_tx_reset(&softbuffers_tx[tb]);
+        }
+        printf("Retransmission %d (TB %d), rv=%d\n", nof_rtx_connsetup, tb, action->rv[tb]);
+
       }
-      printf("Retransmission %d, rv=%d\n", nof_rtx_connsetup, action->rv);
-    } 
+    }
   }
 
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) {
@@ -279,24 +284,25 @@ public:
     } else {
       action->generate_ack = true; 
     }
-    action->payload_ptr = payload; 
-    action->rnti = grant.rnti; 
+    action->rnti = grant.rnti;
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     memcpy(&last_grant, &grant, sizeof(mac_grant_t));
-    action->rv = grant.rv;
-    action->softbuffer = &softbuffer_rx;
-    
-    if (action->rv == 0) {
-      srslte_softbuffer_rx_reset(&softbuffer_rx);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb ++) {
+      action->softbuffers[tb] = &softbuffers_rx[tb];
+      action->rv[tb] = grant.rv[tb];
+      action->payload_ptr[tb] = payload[tb];
+      if (action->rv[tb] == 0) {
+        srslte_softbuffer_rx_reset(&softbuffers_rx[tb]);
+      }
     }
   }
   
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
     if (ack) {
       if (rnti_type == SRSLTE_RNTI_RAR) {
         my_phy.pdcch_dl_search_reset();
-        srslte_bit_unpack_vector(payload, payload_bits, last_grant.n_bytes*8);
-        rar_unpack(payload_bits, &rar_msg);
+        srslte_bit_unpack_vector(payload[tb_idx], payload_bits[tb_idx], last_grant.n_bytes[tb_idx]*8);
+        rar_unpack(payload_bits[tb_idx], &rar_msg);
         if (rar_msg.RAPID == preamble_idx) {
 
           printf("Received RAR at TTI: %d\n", last_grant.tti);
@@ -304,7 +310,7 @@ public:
           
           temp_c_rnti = rar_msg.temp_c_rnti; 
           
-          if (last_grant.n_bytes*8 > 20 + SRSLTE_RAR_GRANT_LEN) {
+          if (last_grant.n_bytes[0]*8 > 20 + SRSLTE_RAR_GRANT_LEN) {
             uint8_t rar_grant[SRSLTE_RAR_GRANT_LEN];
             memcpy(rar_grant, &payload_bits[20], sizeof(uint8_t)*SRSLTE_RAR_GRANT_LEN);
             my_phy.set_rar_grant(last_grant.tti, rar_grant);          
@@ -323,9 +329,11 @@ public:
     printf("BCH decoded\n");
     bch_decoded = true; 
     srslte_cell_t cell; 
-    my_phy.get_current_cell(&cell); 
-    srslte_softbuffer_rx_init(&softbuffer_rx, cell.nof_prb);
-    srslte_softbuffer_tx_init(&softbuffer_tx, cell.nof_prb);
+    my_phy.get_current_cell(&cell);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      srslte_softbuffer_rx_init(&softbuffers_rx[tb], cell.nof_prb);
+      srslte_softbuffer_tx_init(&softbuffers_tx[tb], cell.nof_prb);
+    }
   }
    
 private: 
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index c2207d88f..90f8b5b43 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -87,8 +87,8 @@ bool bch_decoded = false;
 uint32_t total_pkts=0;
 uint32_t total_dci=0;
 uint32_t total_oks=0;
-uint8_t payload[1024]; 
-srslte_softbuffer_rx_t softbuffer; 
+uint8_t payload[SRSLTE_MAX_TB][1024];
+srslte_softbuffer_rx_t softbuffers[SRSLTE_MAX_TB];
 
 class rrc_dummy : public srsue::rrc_interface_phy
 {
@@ -118,20 +118,23 @@ public:
 
     action->decode_enabled = true; 
     action->default_ack = false; 
-    action->generate_ack = false; 
-    action->payload_ptr = payload; 
-    memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
-    action->rv = ((uint32_t) ceilf((float)3*((my_phy.tti_to_SFN(grant.tti)/2)%4)/2))%4;
-    action->softbuffer = &softbuffer;
-    action->rnti = grant.rnti; 
-    if (action->rv == 0) {
-      srslte_softbuffer_rx_reset(&softbuffer);
+    action->generate_ack = false;
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      action->payload_ptr[tb] = payload[tb];
+      action->rv[tb] = ((uint32_t) ceilf((float) 3 * ((my_phy.tti_to_SFN(grant.tti) / 2) % 4) / 2)) % 4;
+      if (action->rv == 0) {
+        srslte_softbuffer_rx_reset(&softbuffers[tb]);
+      }
+      action->softbuffers[0] = &softbuffers[tb];
     }
+    memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
+
+    action->rnti = grant.rnti;
   }
   
   
   
-  void tb_decoded(bool ack, srslte_rnti_type_t rnti, uint32_t harq_pid) {
+  void tb_decoded(bool ack, uint32_t tb_idx, srslte_rnti_type_t rnti_type, uint32_t harq_pid) {
     if (ack) {
       total_oks++;     
     }
@@ -144,7 +147,9 @@ public:
     bch_decoded = true; 
     srslte_cell_t cell; 
     my_phy.get_current_cell(&cell); 
-    srslte_softbuffer_rx_init(&softbuffer, cell.nof_prb);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+      srslte_softbuffer_rx_init(&softbuffers[tb], cell.nof_prb);
+    }
   }
   void tti_clock(uint32_t tti) {
     
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 532bb9849..8c9ba7d3d 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -26,7 +26,7 @@ ul_freq = 2565000000
 tx_gain = 80
 rx_gain = 60
 
-nof_rx_ant = 2
+#nof_rx_ant = 1
 #device_name = auto
 #device_args = auto
 #time_adv_nsamples = auto

From 91a8a291009a427b33054957ebd20929f745d69b Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 16:51:44 +0200
Subject: [PATCH 144/269] fixed memory issues

---
 lib/src/phy/enb/enb_ul.c       |  4 ++--
 srsenb/hdr/phy/phch_common.h   |  1 +
 srsenb/src/mac/mac.cc          | 10 ----------
 srsenb/src/mac/scheduler.cc    |  2 ++
 srsenb/src/phy/phch_common.cc  |  2 +-
 srsenb/src/phy/phch_worker.cc  | 13 ++++++++++++-
 srsenb/src/phy/prach_worker.cc |  5 +++--
 srsenb/src/upper/rrc.cc        |  8 ++++----
 srsenb/src/upper/s1ap.cc       |  1 +
 9 files changed, 26 insertions(+), 20 deletions(-)

diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index 9d773d6d5..c399d715c 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -60,7 +60,7 @@ int srslte_enb_ul_init(srslte_enb_ul_t *q, srslte_cell_t cell,
       memcpy(&q->hopping_cfg, hopping_cfg, sizeof(srslte_pusch_hopping_cfg_t));
     } 
     
-    q->users = calloc(sizeof(srslte_enb_ul_user_t*), SRSLTE_SIRNTI);
+    q->users = calloc(sizeof(srslte_enb_ul_user_t*), (1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean_exit;
@@ -161,7 +161,7 @@ void srslte_enb_ul_free(srslte_enb_ul_t *q)
 int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, uint16_t rnti)
 {
   if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_enb_ul_user_t));
+    q->users[rnti] = calloc(1, sizeof(srslte_enb_ul_user_t));
     
     if (srslte_pucch_set_crnti(&q->pucch, rnti)) {
       fprintf(stderr, "Error setting PUCCH rnti\n");
diff --git a/srsenb/hdr/phy/phch_common.h b/srsenb/hdr/phy/phch_common.h
index ae396ae0e..00a59d969 100644
--- a/srsenb/hdr/phy/phch_common.h
+++ b/srsenb/hdr/phy/phch_common.h
@@ -54,6 +54,7 @@ public:
  
   
   phch_common(uint32_t max_mutex_) : tx_mutex(max_mutex_) {
+    nof_mutex = 0;
     max_mutex = max_mutex_; 
     params.max_prach_offset_us = 20; 
   }
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 63d189c20..a1e9fb21b 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -113,16 +113,6 @@ void mac::reset()
   /* Setup scheduler */
   scheduler.reset();
   
-  /* Setup SI-RNTI in PHY */
-  phy_h->add_rnti(SRSLTE_SIRNTI);
-
-  /* Setup P-RNTI in PHY */
-  phy_h->add_rnti(SRSLTE_PRNTI);
-
-  /* Setup RA-RNTI in PHY */
-  for (int i=0;i<10;i++) {
-    phy_h->add_rnti(1+i);
-  }    
 }
 
 uint32_t mac::get_unique_id()
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 73dbc2d41..325970b83 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -20,6 +20,7 @@ namespace srsenb {
  *******************************************************/
 sched::sched()
 {
+  current_tti = 0;
   log_h = NULL; 
   pthread_mutex_init(&mutex, NULL);
   reset();
@@ -39,6 +40,7 @@ void sched::init(rrc_interface_mac *rrc_, srslte::log* log)
 
 int sched::reset()
 {
+  bzero(pending_msg3, sizeof(pending_msg3_t)*10);
   bzero(pending_rar, sizeof(sched_rar_t)*SCHED_MAX_PENDING_RAR);
   bzero(pending_sibs, sizeof(sched_sib_t)*MAX_SIBS); 
   ue_db.clear();
diff --git a/srsenb/src/phy/phch_common.cc b/srsenb/src/phy/phch_common.cc
index bfb0d1b0d..e4d91581e 100644
--- a/srsenb/src/phy/phch_common.cc
+++ b/srsenb/src/phy/phch_common.cc
@@ -60,7 +60,7 @@ bool phch_common::init(srslte_cell_t *cell_, srslte::radio* radio_h_, mac_interf
 
   is_first_of_burst = true; 
   is_first_tx = true; 
-  for (uint32_t i=0;i<nof_mutex;i++) {
+  for (uint32_t i=0;i<max_mutex;i++) {
     pthread_mutex_init(&tx_mutex[i], NULL);
   }
   reset(); 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index d86bce9c9..73aaa0f54 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -110,7 +110,18 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     fprintf(stderr, "Error initiating ENB DL\n");
     return;
   }
-  
+
+  /* Setup SI-RNTI in PHY */
+  add_rnti(SRSLTE_SIRNTI);
+
+  /* Setup P-RNTI in PHY */
+  add_rnti(SRSLTE_PRNTI);
+
+  /* Setup RA-RNTI in PHY */
+  for (int i=0;i<10;i++) {
+    add_rnti(1+i);
+  }
+
   srslte_pucch_set_threshold(&enb_ul.pucch, 0.8);
   srslte_sch_set_max_noi(&enb_ul.pusch.ul_sch, phy->params.pusch_max_its);
   srslte_enb_dl_set_amp(&enb_dl, phy->params.tx_amplitude);
diff --git a/srsenb/src/phy/prach_worker.cc b/srsenb/src/phy/prach_worker.cc
index 218f7cc21..15b42c374 100644
--- a/srsenb/src/phy/prach_worker.cc
+++ b/srsenb/src/phy/prach_worker.cc
@@ -56,8 +56,9 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
   }
   
   start(priority);
-  initiated = true; 
-  
+  initiated = true;
+
+  sf_cnt = 0;
   pending_tti   = 0; 
   processed_tti = 0; 
   return 0; 
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 03ea65f2c..44b3d9a89 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -61,7 +61,8 @@ void rrc::init(rrc_cfg_t *cfg_,
  
   pthread_mutex_init(&user_mutex, NULL);
   pthread_mutex_init(&paging_mutex, NULL);
-  
+
+  act_monitor.start(RRC_THREAD_PRIO);
   bzero(&sr_sched, sizeof(sr_sched_t));
   
   start(RRC_THREAD_PRIO);
@@ -69,9 +70,8 @@ void rrc::init(rrc_cfg_t *cfg_,
 
 rrc::activity_monitor::activity_monitor(rrc* parent_) 
 {
-  running = true; 
-  parent = parent_; 
-  start(RRC_THREAD_PRIO);
+  running = true;
+  parent = parent_;
 }
 
 void rrc::activity_monitor::stop()
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index eba9186e7..44a564d09 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -347,6 +347,7 @@ bool s1ap::setup_s1()
   uint16_t                    tmp16;
   srslte::byte_buffer_t       msg;
   LIBLTE_S1AP_S1AP_PDU_STRUCT pdu;
+  bzero(&pdu, sizeof(LIBLTE_S1AP_S1AP_PDU_STRUCT));
 
   pdu.choice_type = LIBLTE_S1AP_S1AP_PDU_CHOICE_INITIATINGMESSAGE;
 

From a8224e8caeca60c5e100cdc43a55c72a00a026f0 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 17:50:20 +0200
Subject: [PATCH 145/269] Removed redundant condition

---
 srsue/hdr/mac/dl_harq.h | 26 ++++++++++++--------------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index a9949cc3a..eecf14401 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -297,20 +297,18 @@ private:
               harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid], cur_grant.rnti, ack,
                                                 cur_grant.tti);
             }
-            if (ack) {
-              if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
-                      cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
-              } else {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
-                                                  cur_grant.tti);
-
-                // Compute average number of retransmissions per packet
-                harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
-                                                           harq_entity->nof_pkts++);
-              }
+            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
+                    cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
+            } else {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                                cur_grant.tti);
+
+              // Compute average number of retransmissions per packet
+              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
+                                                         harq_entity->nof_pkts++);
             }
           }
         } else {

From 224a54a315e550efbf12eff6e0a68e7664d7ef2d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 17:51:56 +0200
Subject: [PATCH 146/269] Solved bug for plotting srsue contellation and
 removed trace of tb_cw_swap

---
 srsue/src/phy/phch_worker.cc | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 8f9726a46..9fd37251c 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -411,12 +411,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     grant->last_tti = 0;
     grant->tb_en[0] = dci_unpacked.tb_en[0];
     grant->tb_en[1] = dci_unpacked.tb_en[1];
-    grant->tb_cw_swap = dci_unpacked.tb_cw_swap;
-
-    if (grant->tb_cw_swap) {
-      Info("tb_cw_swap = true\n");
-      printf("tb_cw_swap = true\n");
-    }
+    grant->tb_cw_swap = dci_unpacked.tb_cw_swap; // FIXME: tb_cw_swap not supported
 
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
@@ -1056,7 +1051,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
   bzero(ce_abs, sizeof(float)*sz);
   int g = (sz - 12*cell.nof_prb)/2;
   for (i = 0; i < 12*cell.nof_prb; i++) {
-    ce_abs[g+i] = 20 * log10(cabs(ue_dl.ce[0][i]));
+    ce_abs[g+i] = 20 * log10f(cabsf(ue_dl.ce_m[0][0][i]));
     if (isinf(ce_abs[g+i])) {
       ce_abs[g+i] = -80;
     }
@@ -1067,7 +1062,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
 {
 
-  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  memcpy(pdsch_d, ue_dl.pdsch.d[0], ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
   return ue_dl.pdsch_cfg.nbits[0].nof_re;
 }
 

From 22f194182c6726ce7a00143443d7e24669d9954a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 5 Sep 2017 16:51:44 +0200
Subject: [PATCH 147/269] fixed memory leaks

---
 lib/src/phy/enb/enb_ul.c       |  8 +++---
 lib/src/phy/phch/pdsch.c       |  2 +-
 lib/src/phy/phch/pucch.c       |  2 +-
 lib/src/phy/phch/pusch.c       |  2 +-
 lib/src/phy/rf/rf_uhd_imp.c    | 10 +++++--
 lib/src/phy/utils/cexptab.c    |  2 +-
 srsenb/hdr/enb.h               | 43 +++++++++++++++++-----------
 srsenb/hdr/mac/scheduler.h     |  3 +-
 srsenb/hdr/mac/ue.h            |  2 +-
 srsenb/hdr/phy/phch_common.h   |  1 +
 srsenb/hdr/phy/phch_worker.h   |  7 +++--
 srsenb/src/enb.cc              | 10 +++++--
 srsenb/src/mac/mac.cc          | 16 ++++-------
 srsenb/src/mac/scheduler.cc    |  8 ++++++
 srsenb/src/phy/phch_common.cc  |  2 +-
 srsenb/src/phy/phch_worker.cc  | 36 ++++++++++++++++++++++--
 srsenb/src/phy/phy.cc          |  3 ++
 srsenb/src/phy/prach_worker.cc | 10 +++++--
 srsenb/src/upper/rrc.cc        | 51 +++++++++++++++++++++-------------
 srsenb/src/upper/s1ap.cc       |  1 +
 srsue/hdr/mac/dl_harq.h        |  6 ++++
 srsue/hdr/mac/proc_ra.h        |  3 ++
 srsue/hdr/mac/ul_harq.h        | 11 ++++++++
 srsue/src/mac/mac.cc           |  2 ++
 srsue/src/mac/proc_ra.cc       |  4 +++
 srsue/src/phy/phch_recv.cc     |  1 +
 26 files changed, 176 insertions(+), 70 deletions(-)

diff --git a/lib/src/phy/enb/enb_ul.c b/lib/src/phy/enb/enb_ul.c
index f756c021f..db05d44ea 100644
--- a/lib/src/phy/enb/enb_ul.c
+++ b/lib/src/phy/enb/enb_ul.c
@@ -50,7 +50,7 @@ int srslte_enb_ul_init(srslte_enb_ul_t *q,
     
     bzero(q, sizeof(srslte_enb_ul_t));
     
-    q->users = calloc(sizeof(srslte_enb_ul_user_t*), SRSLTE_SIRNTI);
+    q->users = calloc(sizeof(srslte_enb_ul_user_t*), (1+SRSLTE_SIRNTI));
     if (!q->users) {
       perror("malloc");
       goto clean_exit;
@@ -110,7 +110,7 @@ void srslte_enb_ul_free(srslte_enb_ul_t *q)
   if (q) {
     
     if (q->users) {
-      for (int i=0;i<SRSLTE_SIRNTI;i++) {
+      for (int i=0;i<=SRSLTE_SIRNTI;i++) {
         if (q->users[i]) {
           free(q->users[i]);
         }
@@ -198,8 +198,8 @@ int srslte_enb_ul_set_cell(srslte_enb_ul_t *q, srslte_cell_t cell,
 int srslte_enb_ul_add_rnti(srslte_enb_ul_t *q, uint16_t rnti)
 {
   if (!q->users[rnti]) {
-    q->users[rnti] = malloc(sizeof(srslte_enb_ul_user_t));
-    
+    q->users[rnti] = calloc(1, sizeof(srslte_enb_ul_user_t));
+
     if (srslte_pucch_set_crnti(&q->pucch, rnti)) {
       fprintf(stderr, "Error setting PUCCH rnti\n");
       return -1;
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index c29d048da..6bacf99cf 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -320,7 +320,7 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
     if (q->is_ue) {
       srslte_pdsch_free_rnti(q, 0);
     } else {
-      for (uint16_t u=0;u<SRSLTE_SIRNTI;u++) {
+      for (int u=0;u<=SRSLTE_SIRNTI;u++) {
         if (q->users[u]) {
           srslte_pdsch_free_rnti(q, u);
         }
diff --git a/lib/src/phy/phch/pucch.c b/lib/src/phy/phch/pucch.c
index 6b271d02e..498d34f3a 100644
--- a/lib/src/phy/phch/pucch.c
+++ b/lib/src/phy/phch/pucch.c
@@ -451,7 +451,7 @@ clean_exit:
 
 void srslte_pucch_free(srslte_pucch_t *q) {
   if (q->users) {
-    for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {      
+    for (int rnti=0;rnti<=SRSLTE_SIRNTI;rnti++) {
       srslte_pucch_clear_rnti(q, rnti);
     }
     free(q->users);
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index 24e86e9a3..ee01484d8 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -295,7 +295,7 @@ void srslte_pusch_free(srslte_pusch_t *q) {
     if (q->is_ue) {
       srslte_pusch_free_rnti(q, 0);
     } else {
-      for (int rnti=0;rnti<SRSLTE_SIRNTI;rnti++) {
+      for (int rnti=0;rnti<=SRSLTE_SIRNTI;rnti++) {
         srslte_pusch_free_rnti(q, rnti);
       }
     }
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 2505fc02b..31194aa71 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -117,6 +117,7 @@ static void* async_thread(void *h) {
       return NULL; 
     }
   }
+  uhd_async_metadata_free(&md);
   return NULL; 
 }
 
@@ -457,14 +458,17 @@ int rf_uhd_close(void *h)
   uhd_rx_metadata_free(&handler->rx_md_first);
   uhd_rx_metadata_free(&handler->rx_md);
   uhd_meta_range_free(&handler->rx_gain_range);
-  uhd_tx_streamer_free(&handler->tx_stream);
-  uhd_rx_streamer_free(&handler->rx_stream);
   if (handler->has_rssi) {
     uhd_sensor_value_free(&handler->rssi_value);
   }
   handler->async_thread_running = false; 
-  pthread_join(handler->async_thread, NULL); 
+  pthread_join(handler->async_thread, NULL);
+
+  uhd_tx_streamer_free(&handler->tx_stream);
+  uhd_rx_streamer_free(&handler->rx_stream);
   uhd_usrp_free(&handler->usrp);
+
+  free(handler);
   
   /** Something else to close the USRP?? */
   return 0;
diff --git a/lib/src/phy/utils/cexptab.c b/lib/src/phy/utils/cexptab.c
index 47ff8ef96..0f9232e85 100644
--- a/lib/src/phy/utils/cexptab.c
+++ b/lib/src/phy/utils/cexptab.c
@@ -36,7 +36,7 @@ int srslte_cexptab_init(srslte_cexptab_t *h, uint32_t size) {
   uint32_t i;
 
   h->size = size;
-  h->tab = malloc(sizeof(cf_t) * size);
+  h->tab = malloc(sizeof(cf_t) * (1+size));
   if (h->tab) {
     for (i = 0; i < size; i++) {
       h->tab[i] = cexpf(_Complex_I * 2 * M_PI * (float) i / size);
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index b28d12ab4..3ab1dddb9 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -138,42 +138,53 @@ typedef struct {
   Main UE class
 *******************************************************************************/
 
+#define LOG_STDOUT
+
 class enb
-    :public enb_metrics_interface
-{
+    :public enb_metrics_interface {
 public:
-  static enb* get_instance(void);
+  static enb *get_instance(void);
+
   static void cleanup(void);
 
   bool init(all_args_t *args_);
+
   void stop();
+
   void start_plot();
-  
+
   static void rf_msg(srslte_rf_error_t error);
+
   void handle_rf_msg(srslte_rf_error_t error);
 
   // eNodeB metrics interface
   bool get_metrics(enb_metrics_t &m);
 
   void pregenerate_signals(bool enable);
-  
+
 
 private:
   static enb *instance;
+
   enb();
+
   virtual ~enb();
 
-  srslte::radio      radio;
-  srsenb::phy        phy;
-  srsenb::mac        mac;
-  srslte::mac_pcap   mac_pcap;
-  srsenb::rlc        rlc;
-  srsenb::pdcp       pdcp;
-  srsenb::rrc        rrc;
-  srsenb::gtpu       gtpu;
-  srsenb::s1ap       s1ap;
-
-  srslte::logger_file logger;
+  srslte::radio radio;
+  srsenb::phy phy;
+  srsenb::mac mac;
+  srslte::mac_pcap mac_pcap;
+  srsenb::rlc rlc;
+  srsenb::pdcp pdcp;
+  srsenb::rrc rrc;
+  srsenb::gtpu gtpu;
+  srsenb::s1ap s1ap;
+
+#ifdef LOG_STDOUT
+    srslte::logger_stdout logger;
+#else
+    srslte::logger_file logger;
+#endif
   srslte::log_filter  rf_log;
   std::vector<void*>  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsenb/hdr/mac/scheduler.h b/srsenb/hdr/mac/scheduler.h
index 66f8998c1..c70ee4247 100644
--- a/srsenb/hdr/mac/scheduler.h
+++ b/srsenb/hdr/mac/scheduler.h
@@ -80,7 +80,8 @@ public:
    ************************************************************/
   
   sched(); 
-  
+  ~sched();
+
   void init(rrc_interface_mac *rrc, srslte::log *log);
   void set_metric(metric_dl *dl_metric, metric_ul *ul_metric);
   int cell_cfg(cell_cfg_t *cell_cfg); 
diff --git a/srsenb/hdr/mac/ue.h b/srsenb/hdr/mac/ue.h
index 5805e5472..b879d040b 100644
--- a/srsenb/hdr/mac/ue.h
+++ b/srsenb/hdr/mac/ue.h
@@ -38,7 +38,7 @@
 
 namespace srsenb {
   
-class ue : public srslte::read_pdu_interface, 
+class ue : public srslte::read_pdu_interface,
            public srslte::pdu_queue::process_callback
 {
 public:
diff --git a/srsenb/hdr/phy/phch_common.h b/srsenb/hdr/phy/phch_common.h
index ae396ae0e..00a59d969 100644
--- a/srsenb/hdr/phy/phch_common.h
+++ b/srsenb/hdr/phy/phch_common.h
@@ -54,6 +54,7 @@ public:
  
   
   phch_common(uint32_t max_mutex_) : tx_mutex(max_mutex_) {
+    nof_mutex = 0;
     max_mutex = max_mutex_; 
     params.max_prach_offset_us = 20; 
   }
diff --git a/srsenb/hdr/phy/phch_worker.h b/srsenb/hdr/phy/phch_worker.h
index 2f0f80f41..906e8b9d0 100644
--- a/srsenb/hdr/phy/phch_worker.h
+++ b/srsenb/hdr/phy/phch_worker.h
@@ -42,7 +42,8 @@ public:
   
   phch_worker();
   void  init(phch_common *phy, srslte::log *log_h);
-  void  reset(); 
+  void  stop();
+  void  reset();
   
   cf_t *get_buffer_rx();
   void set_time(uint32_t tti, uint32_t tx_mutex_cnt, srslte_timestamp_t tx_time);
@@ -83,7 +84,9 @@ private:
   /* Common objects */  
   srslte::log    *log_h; 
   phch_common    *phy;
-  bool           initiated; 
+  bool           initiated;
+  bool           running;
+
   cf_t          *signal_buffer_rx; 
   cf_t          *signal_buffer_tx; 
   uint32_t       tti_rx, tti_tx, tti_sched_ul, sf_rx, sf_tx, sf_sched_ul, tx_mutex_cnt;
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index 1bd7240a2..e2c1c9a29 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -45,6 +45,7 @@ enb* enb::get_instance(void)
 void enb::cleanup(void)
 {
   srslte_dft_exit();
+  srslte::byte_buffer_pool::cleanup();
   boost::mutex::scoped_lock lock(enb_instance_mutex);
   if(NULL != instance) {
       delete instance;
@@ -61,14 +62,15 @@ enb::enb()
 
 enb::~enb()
 {
-  srslte::byte_buffer_pool::cleanup();
 }
 
 bool enb::init(all_args_t *args_)
 {
   args     = args_;
 
-  logger.init(args->log.filename);
+#ifndef LOG_STDOUT
+    logger.init(args->log.filename);
+#endif
   rf_log.init("RF  ", &logger);
   
   // Create array of pointers to phy_logs 
@@ -87,7 +89,9 @@ bool enb::init(all_args_t *args_)
   s1ap_log.init("S1AP", &logger);
 
   // Init logs
-  logger.log("\n\n");
+#ifndef LOG_STDOUT
+    logger.log("\n\n");
+#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     ((srslte::log_filter*) phy_log[i])->set_level(level(args->log.phy_level));
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 63d189c20..284cef3e7 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -83,12 +83,16 @@ bool mac::init(mac_args_t *args_, srslte_cell_t *cell_, phy_interface_mac *phy,
     reset();
 
     started = true; 
-  }    
+  }
+
   return started; 
 }
 
 void mac::stop()
 {
+  for (uint32_t i=0;i<ue_db.size();i++) {
+    delete ue_db[i];
+  }
   for (int i=0;i<NOF_BCCH_DLSCH_MSG;i++) {
     srslte_softbuffer_tx_free(&bcch_softbuffer_tx[i]);
   }  
@@ -113,16 +117,6 @@ void mac::reset()
   /* Setup scheduler */
   scheduler.reset();
   
-  /* Setup SI-RNTI in PHY */
-  phy_h->add_rnti(SRSLTE_SIRNTI);
-
-  /* Setup P-RNTI in PHY */
-  phy_h->add_rnti(SRSLTE_PRNTI);
-
-  /* Setup RA-RNTI in PHY */
-  for (int i=0;i<10;i++) {
-    phy_h->add_rnti(1+i);
-  }    
 }
 
 uint32_t mac::get_unique_id()
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 73dbc2d41..79cf3f476 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -20,11 +20,18 @@ namespace srsenb {
  *******************************************************/
 sched::sched()
 {
+  current_tti = 0;
   log_h = NULL; 
   pthread_mutex_init(&mutex, NULL);
   reset();
 }
 
+sched::~sched()
+{
+  srslte_regs_free(&regs);
+  pthread_mutex_destroy(&mutex);
+}
+
 void sched::init(rrc_interface_mac *rrc_, srslte::log* log)
 {
   sched_cfg.pdsch_max_mcs = 28; 
@@ -39,6 +46,7 @@ void sched::init(rrc_interface_mac *rrc_, srslte::log* log)
 
 int sched::reset()
 {
+  bzero(pending_msg3, sizeof(pending_msg3_t)*10);
   bzero(pending_rar, sizeof(sched_rar_t)*SCHED_MAX_PENDING_RAR);
   bzero(pending_sibs, sizeof(sched_sib_t)*MAX_SIBS); 
   ue_db.clear();
diff --git a/srsenb/src/phy/phch_common.cc b/srsenb/src/phy/phch_common.cc
index bfb0d1b0d..e4d91581e 100644
--- a/srsenb/src/phy/phch_common.cc
+++ b/srsenb/src/phy/phch_common.cc
@@ -60,7 +60,7 @@ bool phch_common::init(srslte_cell_t *cell_, srslte::radio* radio_h_, mac_interf
 
   is_first_of_burst = true; 
   is_first_tx = true; 
-  for (uint32_t i=0;i<nof_mutex;i++) {
+  for (uint32_t i=0;i<max_mutex;i++) {
     pthread_mutex_init(&tx_mutex[i], NULL);
   }
   reset(); 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index a18a96405..03c1fd56e 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -118,6 +118,17 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
     return;
   }
   
+  /* Setup SI-RNTI in PHY */
+  add_rnti(SRSLTE_SIRNTI);
+
+  /* Setup P-RNTI in PHY */
+  add_rnti(SRSLTE_PRNTI);
+
+  /* Setup RA-RNTI in PHY */
+  for (int i=0;i<10;i++) {
+    add_rnti(1+i);
+  }
+
   srslte_pucch_set_threshold(&enb_ul.pucch, 0.8);
   srslte_sch_set_max_noi(&enb_ul.pusch.ul_sch, phy->params.pusch_max_its);
   srslte_enb_dl_set_amp(&enb_dl, phy->params.tx_amplitude);
@@ -125,12 +136,29 @@ void phch_worker::init(phch_common* phy_, srslte::log *log_h_)
   Info("Worker %d configured cell %d PRB\n", get_id(), phy->cell.nof_prb);
   
   initiated = true; 
-  
+  running   = true;
+
 #ifdef DEBUG_WRITE_FILE
   f = fopen("test.dat", "w");
 #endif
 }
 
+void phch_worker::stop()
+{
+  running = false;
+  pthread_mutex_lock(&mutex);
+
+  srslte_enb_dl_free(&enb_dl);
+  srslte_enb_ul_free(&enb_ul);
+  if (signal_buffer_rx) {
+    free(signal_buffer_rx);
+  }
+  if (signal_buffer_tx) {
+    free(signal_buffer_tx);
+  }
+  pthread_mutex_unlock(&mutex);
+  pthread_mutex_destroy(&mutex);
+}
 void phch_worker::reset() 
 {
   initiated  = false; 
@@ -234,7 +262,11 @@ void phch_worker::rem_rnti(uint16_t rnti)
 
 void phch_worker::work_imp()
 {
-  uint32_t sf_ack; 
+  uint32_t sf_ack;
+
+  if (!running) {
+    return;
+  }
   
   pthread_mutex_lock(&mutex); 
   
diff --git a/srsenb/src/phy/phy.cc b/srsenb/src/phy/phy.cc
index bc4f3173e..c0d0a73c9 100644
--- a/srsenb/src/phy/phy.cc
+++ b/srsenb/src/phy/phy.cc
@@ -134,6 +134,9 @@ void phy::stop()
 {  
   tx_rx.stop();  
   workers_common.stop();
+  for (uint32_t i=0;i<nof_workers;i++) {
+    workers[i].stop();
+  }
   workers_pool.stop();
   prach.stop();
 }
diff --git a/srsenb/src/phy/prach_worker.cc b/srsenb/src/phy/prach_worker.cc
index cc924bf0b..b4bd7da3f 100644
--- a/srsenb/src/phy/prach_worker.cc
+++ b/srsenb/src/phy/prach_worker.cc
@@ -57,8 +57,9 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
   }
   
   start(priority);
-  initiated = true; 
-  
+  initiated = true;
+
+  sf_cnt = 0;
   pending_tti   = 0; 
   processed_tti = 0; 
   return 0; 
@@ -66,6 +67,11 @@ int prach_worker::init(srslte_cell_t *cell_, srslte_prach_cfg_t *prach_cfg_, mac
 
 void prach_worker::stop()
 {
+  srslte_prach_free(&prach);
+
+  if (signal_buffer_rx) {
+    free(signal_buffer_rx);
+  }
   pthread_mutex_lock(&mutex);
   processed_tti = 99999; 
   running = false; 
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 03ea65f2c..f803500f2 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -26,6 +26,7 @@
 
 #include <srslte/interfaces/sched_interface.h>
 #include <srslte/asn1/liblte_rrc.h>
+#include <upper/rrc.h>
 #include "srslte/asn1/liblte_mme.h"
 #include "upper/rrc.h"
 
@@ -61,7 +62,8 @@ void rrc::init(rrc_cfg_t *cfg_,
  
   pthread_mutex_init(&user_mutex, NULL);
   pthread_mutex_init(&paging_mutex, NULL);
-  
+
+  act_monitor.start(RRC_THREAD_PRIO);
   bzero(&sr_sched, sizeof(sr_sched_t));
   
   start(RRC_THREAD_PRIO);
@@ -69,9 +71,8 @@ void rrc::init(rrc_cfg_t *cfg_,
 
 rrc::activity_monitor::activity_monitor(rrc* parent_) 
 {
-  running = true; 
-  parent = parent_; 
-  start(RRC_THREAD_PRIO);
+  running = true;
+  parent = parent_;
 }
 
 void rrc::activity_monitor::stop()
@@ -149,6 +150,7 @@ uint32_t rrc::generate_sibs()
     srslte_bit_pack_vector(bitbuffer.msg, sib_buffer[i].msg, bitbuffer.N_bits);
     sib_buffer[i].N_bytes = (bitbuffer.N_bits-1)/8+1;
   }
+  free(msg);
   return nof_messages; 
 }
 
@@ -604,23 +606,32 @@ void rrc::run_thread()
     if (p.pdu) {
       rrc_log->info_hex(p.pdu->msg, p.pdu->N_bytes, "Rx %s PDU", rb_id_text[p.lcid]);
     }
-    switch(p.lcid)
-    {
-    case RB_ID_SRB0:
-      parse_ul_ccch(p.rnti, p.pdu);
-      break;
-    case RB_ID_SRB1:
-    case RB_ID_SRB2:
-      parse_ul_dcch(p.rnti, p.lcid, p.pdu);
-      break;
-    case LCID_REM_USER:
-      usleep(10000);
-      rem_user(p.rnti);
-      break;
-    default:
-      rrc_log->error("Rx PDU with invalid bearer id: %s", p.lcid);
-      break;
+    pthread_mutex_lock(&user_mutex);
+    if (users.count(p.rnti) == 1) {
+      switch(p.lcid)
+      {
+        case RB_ID_SRB0:
+          parse_ul_ccch(p.rnti, p.pdu);
+          break;
+        case RB_ID_SRB1:
+        case RB_ID_SRB2:
+          parse_ul_dcch(p.rnti, p.lcid, p.pdu);
+          break;
+        case LCID_REM_USER:
+          pthread_mutex_unlock(&user_mutex);
+          usleep(10000);
+          rem_user(p.rnti);
+          pthread_mutex_lock(&user_mutex);
+          break;
+        default:
+          rrc_log->error("Rx PDU with invalid bearer id: %s", p.lcid);
+          break;
+      }
+    } else {
+      printf("Discarting rnti=0x%xn", p.rnti);
+      rrc_log->warning("Discarting PDU for removed rnti=0x%x\n", p.rnti);
     }
+    pthread_mutex_unlock(&user_mutex);
   }
 }
 void rrc::activity_monitor::run_thread()
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index eba9186e7..44a564d09 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -347,6 +347,7 @@ bool s1ap::setup_s1()
   uint16_t                    tmp16;
   srslte::byte_buffer_t       msg;
   LIBLTE_S1AP_S1AP_PDU_STRUCT pdu;
+  bzero(&pdu, sizeof(LIBLTE_S1AP_S1AP_PDU_STRUCT));
 
   pdu.choice_type = LIBLTE_S1AP_S1AP_PDU_CHOICE_INITIATINGMESSAGE;
 
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 963551150..ba5888a30 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -188,6 +188,12 @@ private:
         bzero(&cur_grant, sizeof(Tgrant));
       }
 
+      ~dl_tb_process() {
+        if (is_initiated) {
+          srslte_softbuffer_rx_free(&softbuffer);
+        }
+      }
+
       bool init(uint32_t pid_, dl_harq_entity *parent, uint32_t tb_idx) {
         tid = tb_idx;
         if (srslte_softbuffer_rx_init(&softbuffer, 110)) {
diff --git a/srsue/hdr/mac/proc_ra.h b/srsue/hdr/mac/proc_ra.h
index 6863371c0..c1bdf3644 100644
--- a/srsue/hdr/mac/proc_ra.h
+++ b/srsue/hdr/mac/proc_ra.h
@@ -71,6 +71,9 @@ class ra_proc : public srslte::timer_callback
       rar_grant_tti             = 0; 
       msg3_flushed              = false; 
     };
+
+    ~ra_proc();
+
     void init(phy_interface_mac *phy_h, 
               rrc_interface_mac *rrc_, 
               srslte::log *log_h, 
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 952b9f002..9ce3b37ca 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -163,9 +163,20 @@ private:
       is_initiated = false;
       is_grant_configured = false;
       tti_last_tx = 0;
+      payload_buffer = NULL;
       bzero(&cur_grant, sizeof(Tgrant));
     }
 
+    ~ul_harq_process()
+    {
+      if (is_initiated) {
+        if (payload_buffer) {
+          free(payload_buffer);
+        }
+        srslte_softbuffer_tx_free(&softbuffer);
+      }
+    }
+
     bool init(uint32_t pid_, ul_harq_entity *parent)
     {
       if (srslte_softbuffer_tx_init(&softbuffer, 110)) {
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 12054567f..f28e43553 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -86,6 +86,8 @@ bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac
 
 void mac::stop()
 {
+  srslte_softbuffer_rx_free(&pch_softbuffer);
+
   started = false;   
   ttisync.increase();
   upper_timers_thread.thread_cancel();
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index f9903e2eb..28a2b47e3 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -73,6 +73,10 @@ void ra_proc::init(phy_interface_mac* phy_h_,
   reset();
 }
 
+ra_proc::~ra_proc() {
+  srslte_softbuffer_rx_free(&softbuffer_rar);
+}
+
 void ra_proc::reset() {
   state = IDLE;
   msg3_transmitted = false;
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 7ff787a38..7607164d3 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -145,6 +145,7 @@ phch_recv::~phch_recv() {
   }
   srslte_ue_sync_free(&ue_sync);
   srslte_ue_dl_free(&ue_dl_measure);
+  srslte_ue_mib_free(&ue_mib);
   srslte_ue_mib_sync_free(&ue_mib_sync);
   srslte_ue_cellsearch_free(&cs);
 }

From b59e4d08cadc66b1ca5607677733cc2e0f826df2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 12:43:50 +0200
Subject: [PATCH 148/269] fixed incorrect initialization of PUSCH estimator

---
 lib/include/srslte/phy/enb/enb_dl.h  |  8 +++++---
 lib/src/phy/ch_estimation/chest_ul.c |  5 +++--
 lib/src/phy/dft/dft_fftw.c           | 21 +++++++++++++++++----
 lib/src/phy/enb/enb_dl.c             |  6 +++---
 srsenb/hdr/enb.h                     |  2 +-
 srsenb/src/phy/phch_worker.cc        | 19 ++++++++++++-------
 6 files changed, 41 insertions(+), 20 deletions(-)

diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index 62062cca8..f465d81e2 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -157,16 +157,18 @@ SRSLTE_API int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q,
                                        srslte_ra_dl_grant_t *grant, 
                                        srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
                                        uint16_t rnti,
-                                       uint32_t rv_idx, 
+                                       int rv_idx[SRSLTE_MAX_CODEWORDS],
                                        uint32_t sf_idx, 
-                                       uint8_t *data[SRSLTE_MAX_CODEWORDS]);
+                                       uint8_t *data[SRSLTE_MAX_CODEWORDS],
+                                       srslte_mimo_type_t mimo_type,
+                                       uint32_t pmi);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_dl(srslte_enb_dl_t *q, 
                                           srslte_ra_dl_dci_t *grant, 
                                           srslte_dci_format_t format, 
                                           srslte_dci_location_t location,
                                           uint16_t rnti, 
-                                          uint32_t sf_idx); 
+                                          uint32_t sf_idx);
 
 SRSLTE_API int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, 
                                           srslte_ra_ul_dci_t *grant, 
diff --git a/lib/src/phy/ch_estimation/chest_ul.c b/lib/src/phy/ch_estimation/chest_ul.c
index f1d4f495a..ac4831cc9 100644
--- a/lib/src/phy/ch_estimation/chest_ul.c
+++ b/lib/src/phy/ch_estimation/chest_ul.c
@@ -45,6 +45,7 @@
 #define NOF_REFS_SYM    (q->cell.nof_prb*SRSLTE_NRE)
 #define NOF_REFS_SF     (NOF_REFS_SYM*2) // 2 reference symbols per subframe
 
+#define MAX_REFS_SYM    (max_prb*SRSLTE_NRE)
 #define MAX_REFS_SF     (max_prb*SRSLTE_NRE*2) // 2 reference symbols per subframe
 
 /** 3GPP LTE Downlink channel estimator and equalizer. 
@@ -98,7 +99,7 @@ int srslte_chest_ul_init(srslte_chest_ul_t *q, uint32_t max_prb)
       goto clean_exit;
     }
     
-    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, MAX_REFS_SF)) {
+    if (srslte_interp_linear_vector_init(&q->srslte_interp_linvec, MAX_REFS_SYM)) {
       fprintf(stderr, "Error initializing vector interpolator\n");
       goto clean_exit; 
     }
@@ -165,7 +166,7 @@ int srslte_chest_ul_set_cell(srslte_chest_ul_t *q, srslte_cell_t cell)
         return SRSLTE_ERROR;
       }
 
-      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, NOF_REFS_SF)) {
+      if (srslte_interp_linear_vector_resize(&q->srslte_interp_linvec, NOF_REFS_SYM)) {
         fprintf(stderr, "Error initializing vector interpolator\n");
         return SRSLTE_ERROR;
       }
diff --git a/lib/src/phy/dft/dft_fftw.c b/lib/src/phy/dft/dft_fftw.c
index db0f38f33..b4a627742 100644
--- a/lib/src/phy/dft/dft_fftw.c
+++ b/lib/src/phy/dft/dft_fftw.c
@@ -39,14 +39,27 @@
 
 #define FFTW_WISDOM_FILE ".fftw_wisdom"
 
+#ifdef FFTW_WISDOM_FILE
+#define FFTW_TYPE FFTW_MEASURE
+#else
+#define FFTW_TYPE 0
+#endif
+
+
 void srslte_dft_load() {
+#ifdef FFTW_WISDOM_FILE
   fftwf_import_wisdom_from_filename(FFTW_WISDOM_FILE);
+#else
+  printf("Warning: FFTW Wisdom file not defined\n");
+#endif
 }
 
 void srslte_dft_exit() {
+#ifdef FFTW_WISDOM_FILE
   if (!fftwf_export_wisdom_to_filename(FFTW_WISDOM_FILE)) {
     fprintf(stderr, "Error saving FFTW wisdom to file %s\n", FFTW_WISDOM_FILE);
   }
+#endif
 }
 
 int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
@@ -86,7 +99,7 @@ int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
     fftwf_destroy_plan(plan->p);
     plan->p = NULL;
   }
-  plan->p = fftwf_plan_dft_1d(new_dft_points, plan->in, plan->out, sign, 0U);
+  plan->p = fftwf_plan_dft_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
@@ -97,7 +110,7 @@ int srslte_dft_replan_c(srslte_dft_plan_t *plan, const int new_dft_points) {
 int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
-  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
+  plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
@@ -120,7 +133,7 @@ int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
     fftwf_destroy_plan(plan->p);
     plan->p = NULL;
   }
-  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
+  plan->p = fftwf_plan_r2r_1d(new_dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
@@ -131,7 +144,7 @@ int srslte_dft_replan_r(srslte_dft_plan_t *plan, const int new_dft_points) {
 int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
   allocate(plan,sizeof(float),sizeof(float), dft_points);
   int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
-  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_MEASURE);
+  plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, FFTW_TYPE);
   if (!plan->p) {
     return -1;
   }
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index de9ef3e22..fb9fe7d87 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -320,11 +320,11 @@ int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q, srslte_ra_ul_dci_t *grant,
 }
 
 int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srslte_softbuffer_tx_t *softbuffer[SRSLTE_MAX_CODEWORDS],
-                            uint16_t rnti, uint32_t rv_idx, uint32_t sf_idx, 
-                            uint8_t *data[SRSLTE_MAX_CODEWORDS])
+                            uint16_t rnti, int rv_idx[SRSLTE_MAX_CODEWORDS], uint32_t sf_idx,
+                            uint8_t *data[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type, uint32_t pmi)
 {  
   /* Configure pdsch_cfg parameters */
-  if (srslte_pdsch_cfg(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx)) {
+  if (srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, q->cfi, sf_idx, rv_idx, mimo_type, pmi)) {
     fprintf(stderr, "Error configuring PDSCH\n");
     return SRSLTE_ERROR;
   }
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index 3ab1dddb9..8b847302e 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -138,7 +138,7 @@ typedef struct {
   Main UE class
 *******************************************************************************/
 
-#define LOG_STDOUT
+//#define LOG_STDOUT
 
 class enb
     :public enb_metrics_interface {
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index 03c1fd56e..ab7df91af 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -50,6 +50,9 @@ using namespace std;
 #include "srsgui/srsgui.h"
 #include <semaphore.h>
 #include <srslte/phy/phch/ra.h>
+#include <srslte/srslte.h>
+#include <srslte/phy/phch/pdsch.h>
+#include <srslte/phy/common/sequence.h>
 
 void init_plots(srsenb::phch_worker *worker);
 pthread_t plot_thread; 
@@ -267,8 +270,8 @@ void phch_worker::work_imp()
   if (!running) {
     return;
   }
-  
-  pthread_mutex_lock(&mutex); 
+
+  pthread_mutex_lock(&mutex);
   
   mac_interface_phy::ul_sched_t *ul_grants = phy->ul_grants;
   mac_interface_phy::dl_sched_t *dl_grants = phy->dl_grants; 
@@ -283,10 +286,10 @@ void phch_worker::work_imp()
     ue_db[rnti].has_grant_tti = -1; 
   }
 
-  // Process UL signal 
+  // Process UL signal
   srslte_enb_ul_fft(&enb_ul, signal_buffer_rx);
 
-  // Decode pending UL grants for the tti they were scheduled 
+  // Decode pending UL grants for the tti they were scheduled
   decode_pusch(ul_grants[sf_rx].sched_grants, ul_grants[sf_rx].nof_grants, sf_rx);
   
   // Decode remaining PUCCH ACKs not associated with PUSCH transmission and SR signals
@@ -355,7 +358,7 @@ void phch_worker::work_imp()
   }
 #endif
 
-unlock: 
+unlock:
   pthread_mutex_unlock(&mutex); 
 
 }
@@ -687,8 +690,10 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
                              phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
       srslte_softbuffer_tx_t *sb[SRSLTE_MAX_CODEWORDS] = {grants[i].softbuffer, NULL};
-      uint8_t *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
-      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, grants[i].grant.rv_idx, sf_idx, d))
+      uint8_t                 *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
+      int                     rv[SRSLTE_MAX_CODEWORDS] = {grants[i].grant.rv_idx, 0};
+
+      if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, rv, sf_idx, d, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0))
       {
         fprintf(stderr, "Error putting PDSCH %d\n",i);
         return SRSLTE_ERROR; 

From 14a901c807e76c4871db9ab21143345d5ec4ba9f Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 13:03:55 +0200
Subject: [PATCH 149/269] compute sequences using static memory

---
 lib/src/phy/common/sequence.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/lib/src/phy/common/sequence.c b/lib/src/phy/common/sequence.c
index d7f702149..0ecf9f159 100644
--- a/lib/src/phy/common/sequence.c
+++ b/lib/src/phy/common/sequence.c
@@ -38,12 +38,14 @@
 
 #define MAX_SEQ_LEN  (128*1024)
 
+#define static_memory
+
 /*
  * Pseudo Random Sequence generation.
  * It follows the 3GPP Release 8 (LTE) 36.211
  * Section 7.2
  */
-#ifdef static
+#ifdef static_memory
 static uint8_t x1[Nc+MAX_SEQ_LEN+31];
 static uint8_t x2[Nc+MAX_SEQ_LEN+31];
 
@@ -78,6 +80,8 @@ int srslte_sequence_set_LTE_pr(srslte_sequence_t *q, uint32_t len, uint32_t seed
     q->c[n] = (x1[n + Nc] + x2[n + Nc]) & 0x1;
   }
   pthread_mutex_unlock(&mutex);
+
+  return 0;
 }
 
 #else

From 37754f992c8579e5bc26dd392bf5641b62c70040 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 6 Sep 2017 14:45:34 +0200
Subject: [PATCH 150/269] Removed bug in dl_harq for multiple TB

---
 srsue/hdr/mac/dl_harq.h | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index eecf14401..dec16086b 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -166,6 +166,13 @@ private:
     }
 
     void new_grant_dl(Tgrant grant, Taction *action) {
+      /* Fill action structure */
+      bzero(action, sizeof(Taction));
+      action->default_ack = false;
+      action->generate_ack = true;
+      action->decode_enabled = false;
+
+      /* For each subprocess... */
       for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
         if (grant.tb_en[tb]) {
           subproc[tb].new_grant_dl(grant, action);
@@ -237,12 +244,6 @@ private:
         grant.last_tti = cur_grant.tti;
         memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
-        // Fill action structure
-        bzero(action, sizeof(Taction));
-        action->default_ack = ack;
-        action->generate_ack = true;
-        action->decode_enabled = false;
-
         // If data has not yet been successfully decoded
         if (!ack) {
 
@@ -264,6 +265,7 @@ private:
 
         } else {
           Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+          action->phy_grant.dl.tb_en[tid] = false;
         }
 
         if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {

From b771c35371596b725650d8ab8846bb8a8048aafb Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 15:05:57 +0200
Subject: [PATCH 151/269] add freq_offset option to ue.conf

---
 lib/include/srslte/radio/radio.h |  3 ++-
 lib/src/radio/radio.cc           |  8 ++++++--
 srsue/hdr/ue_base.h              |  1 +
 srsue/src/main.cc                |  3 +--
 srsue/src/phy/phch_recv.cc       | 14 +++++++++++---
 srsue/src/ue.cc                  |  2 +-
 srsue/ue.conf.example            |  8 ++++----
 7 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index 849c1c0e9..f04fee98d 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -98,6 +98,7 @@ namespace srslte {
       void set_tx_rx_gain_offset(float offset); 
       double set_rx_gain_th(float gain);
 
+      void set_freq_offset(float freq);
       void set_tx_freq(float freq);
       void set_rx_freq(float freq);
 
@@ -158,7 +159,7 @@ namespace srslte {
       const static double blade_default_tx_adv_samples     = 27;
       const static double blade_default_tx_adv_offset_sec  = 1e-6; 
       
-      float tx_freq, rx_freq; 
+      float tx_freq, rx_freq, freq_offset;
       
       trace<uint32_t> tr_local_time;
       trace<uint32_t> tr_usrp_time;
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index 930c52d83..b43e837cd 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -250,9 +250,13 @@ void radio::save_trace(uint32_t is_eob, srslte_timestamp_t *tx_time) {
   }
 }
 
+void radio::set_freq_offset(float freq) {
+  freq_offset = freq;
+}
+
 void radio::set_rx_freq(float freq)
 {
-  rx_freq = srslte_rf_set_rx_freq(&rf_device, freq);
+  rx_freq = srslte_rf_set_rx_freq(&rf_device, freq+freq_offset);
 }
 
 void radio::set_rx_gain(float gain)
@@ -277,7 +281,7 @@ void radio::set_rx_srate(float srate)
 
 void radio::set_tx_freq(float freq)
 {
-  tx_freq = srslte_rf_set_tx_freq(&rf_device, freq);  
+  tx_freq = srslte_rf_set_tx_freq(&rf_device, freq+freq_offset);
 }
 
 void radio::set_tx_gain(float gain)
diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index 7765d43bd..b105b2088 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -55,6 +55,7 @@ typedef struct {
   uint32_t      dl_earfcn;
   float         dl_freq;
   float         ul_freq;
+  float         freq_offset;
   float         rx_gain;
   float         tx_gain;
   uint32_t      nof_rx_ant;
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index d2a75604d..f095a60c4 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -62,8 +62,7 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
   bpo::options_description common("Configuration options");
   common.add_options()
     ("rf.dl_earfcn", bpo::value<uint32_t>(&args->rf.dl_earfcn)->default_value(3400), "Downlink EARFCN")
-    ("rf.dl_freq", bpo::value<float>(&args->rf.dl_freq)->default_value(2680000000), "(optional) Downlink centre frequency")
-    ("rf.ul_freq", bpo::value<float>(&args->rf.ul_freq)->default_value(2560000000), "(optional) Uplink centre frequency")
+    ("rf.freq_offset", bpo::value<float>(&args->rf.freq_offset)->default_value(0), "(optional) Frequency offset")
     ("rf.rx_gain", bpo::value<float>(&args->rf.rx_gain)->default_value(-1), "Front-end receiver gain")
     ("rf.tx_gain", bpo::value<float>(&args->rf.tx_gain)->default_value(-1), "Front-end transmitter gain")
     ("rf.nof_rx_ant", bpo::value<uint32_t>(&args->rf.nof_rx_ant)->default_value(1), "Number of RX antennas")
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 7607164d3..b31d4e8ce 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -235,15 +235,19 @@ bool phch_recv::cell_search(int force_N_id_2) {
   bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
 
   if (srate_mode != SRATE_FIND) {
+    printf("set rx rate\n");
     srate_mode = SRATE_FIND;
     radio_h->set_rx_srate(1.92e6);
   }
+  printf("start rx\n");
   radio_h->start_rx();
 
   /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
   uint32_t max_peak_cell = 0;
   int ret = SRSLTE_ERROR;
 
+  Info("Searching for cell...\n");
+
   if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
     ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
     max_peak_cell = force_N_id_2;
@@ -253,13 +257,14 @@ bool phch_recv::cell_search(int force_N_id_2) {
 
   last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
+  printf("stop rx\n");
   radio_h->stop_rx();
 
   if (ret < 0) {
     Error("Error decoding MIB: Error searching PSS\n");
     return false;
   } else if (ret == 0) {
-    Error("Error decoding MIB: Could not find any PSS in this frequency\n");
+    Info("Could not find any cell in this frequency\n");
     return false;
   }
 
@@ -404,6 +409,7 @@ bool phch_recv::stop_sync() {
 
 void phch_recv::cell_search_inc()
 {
+  printf("cell search inc\n");
   cur_earfcn_index++;
   Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
   if (cur_earfcn_index >= 0) {
@@ -494,8 +500,9 @@ bool phch_recv::set_frequency()
     log_h->console("Searching cell in DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
 
-    radio_h->set_rx_freq(dl_freq-4000);
-    radio_h->set_tx_freq(ul_freq-4000);
+    printf("set frequency\n");
+    radio_h->set_rx_freq(dl_freq);
+    radio_h->set_tx_freq(ul_freq);
     ul_dl_factor = ul_freq / dl_freq;
 
     srslte_ue_sync_reset(&ue_sync);
@@ -549,6 +556,7 @@ void phch_recv::run_thread() {
 
           Info("SYNC:  Cell found. Synchronizing...\n");
         } else {
+          printf("no trobat in progress=%d\n", cell_search_in_progress);
           if (cell_search_in_progress) {
             cell_search_inc();
           }
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 135bb0591..675137e31 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -32,7 +32,6 @@
 #include <string>
 #include <algorithm>
 #include <iterator>
-#include <ue.h>
 
 using namespace srslte;
 
@@ -157,6 +156,7 @@ bool ue::init(all_args_t *args_)
   }
 
   radio.register_error_handler(rf_msg);
+  radio.set_freq_offset(args->rf.freq_offset);
 
   mac.init(&phy, &rlc, &rrc, &mac_log);
   rlc.init(&pdcp, &rrc, this, &rlc_log, &mac, 0 /* RB_ID_SRB0 */);
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 8c9ba7d3d..c281a34fd 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -3,8 +3,8 @@
 #####################################################################
 # RF configuration
 #
-# dl_freq: Downlink centre frequency (Hz).
-# ul_freq: Uplink centre frequency (Hz).
+# dl_earfcn: Downlink EARFCN code.
+# freq_offset: Uplink and Downlink optional frequency offset (in Hz)
 # tx_gain: Transmit gain (dB). 
 # rx_gain: Optional receive gain (dB). If disabled, AGC if enabled
 #
@@ -21,8 +21,8 @@
 #                     Default "auto". B210 USRP: 400 us, bladeRF: 0 us. 
 #####################################################################
 [rf]
-dl_freq = 2685000000
-ul_freq = 2565000000
+dl_earfcn = 3400
+freq_offset = 0
 tx_gain = 80
 rx_gain = 60
 

From 1ab106c127f2dc2c7f3c5f20bb9b51c07e69c4e1 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 6 Sep 2017 15:25:12 +0200
Subject: [PATCH 152/269] Improved robustness of PDSCH decoder

---
 lib/src/phy/phch/pdsch.c     | 27 +++++++++++++++++++++------
 srsue/src/phy/phch_worker.cc | 16 +++++++++-------
 2 files changed, 30 insertions(+), 13 deletions(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index b04b21ea3..fecf50a13 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -537,12 +537,13 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
 
 static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
-                                               uint32_t codeword_idx) {
+                                               uint32_t codeword_idx, bool *ack) {
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
   srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
   uint32_t rv = cfg->rv[codeword_idx];
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (nbits->nof_bits) {
+  if (softbuffer && data && ack) {
     INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
          nbits->nof_re, nbits->nof_bits, rv);
@@ -566,10 +567,20 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *pdsch, srslte_pdsch_cfg_
       srslte_sequence_free(&seq);
     }
 
-    return srslte_dlsch_decode2(&pdsch->dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
+    /* Return  */
+    ret = srslte_dlsch_decode2(&pdsch->dl_sch, cfg, softbuffer, pdsch->e[codeword_idx], data, codeword_idx);
+
+    if (ret == SRSLTE_SUCCESS) {
+      *ack = true;
+    } else if (ret == SRSLTE_ERROR) {
+      *ack = false;
+      ret = SRSLTE_SUCCESS;
+    }
+  } else {
+    ERROR("Detected NULL pointer");
   }
 
-  return SRSLTE_SUCCESS;
+  return ret;
 }
 
 /** Decodes the PDSCH from the received symbols
@@ -643,8 +654,12 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
     // Codeword decoding
     for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
       if (cfg->grant.tb_en[tb]) {
-        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
-        acks[tb] = (ret == SRSLTE_SUCCESS);
+        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb, &acks[tb]);
+
+        /* Check if there has been any execution error */
+        if (ret) {
+          return ret;
+        }
       }
     }
 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 9fd37251c..6e77b3a3e 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -216,8 +216,6 @@ void phch_worker::work_imp()
                       dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-
-        // NOTE: Currently hard-coded to 1st TB only
         for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
           if (dl_mac_grant.tb_en[tb]) {
             phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
@@ -520,7 +518,7 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         ret = srslte_pdsch_decode(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
                                   ue_dl.ce_m, noise_estimate, rnti, payload, acks);
         if (ret) {
-          Error("Decoding PDSCH");
+          Error("ERROR: Decoding PDSCH\n");
         }
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
@@ -673,12 +671,14 @@ void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_
   uint32_t nof_tb = 0;
   if (tb_en[0]) {
     uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    nof_tb++;
+    nof_tb = 1;
+  } else {
+    uci_data.uci_ack = 1;
   }
 
   if (tb_en[1]) {
     uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    nof_tb++;
+    nof_tb = 2;
   }
 
   uci_data.uci_ack_len = nof_tb;
@@ -867,10 +867,12 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s, sr=%s, cfo=%.1f Hz%s\n", 
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
-       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",uci_data.scheduling_request?"yes":"no", 
+       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+       uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+       uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   
   

From 31cdd734eeec29345fc53052c39cc6c8182d0a1a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 15:44:18 +0200
Subject: [PATCH 153/269] remove get functions from set in uhd

---
 lib/src/phy/rf/rf_uhd_imp.c | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 31194aa71..8daf2b9f0 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -492,8 +492,7 @@ double rf_uhd_set_rx_srate(void *h, double freq)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_rate(handler->usrp, freq, i);
   }
-  uhd_usrp_get_rx_rate(handler->usrp, 0, &freq);
-  return freq; 
+  return freq;
 }
 
 double rf_uhd_set_tx_srate(void *h, double freq)
@@ -502,7 +501,6 @@ double rf_uhd_set_tx_srate(void *h, double freq)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_rate(handler->usrp, freq, i);
   }
-  uhd_usrp_get_tx_rate(handler->usrp, 0, &freq);
   handler->tx_rate = freq;
   return freq; 
 }
@@ -513,7 +511,6 @@ double rf_uhd_set_rx_gain(void *h, double gain)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_gain(handler->usrp, gain, i, "");
   }
-  uhd_usrp_get_rx_gain(handler->usrp, 0, "", &gain);
   return gain;
 }
 
@@ -523,7 +520,6 @@ double rf_uhd_set_tx_gain(void *h, double gain)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_gain(handler->usrp, gain, i, "");
   }
-  uhd_usrp_get_tx_gain(handler->usrp, 0, "", &gain);
   return gain;
 }
 
@@ -555,7 +551,6 @@ double rf_uhd_set_rx_freq(void *h, double freq)
   for (int i=0;i<handler->nof_rx_channels;i++) {
     uhd_usrp_set_rx_freq(handler->usrp, &tune_request, i, &tune_result);
   }
-  uhd_usrp_get_rx_freq(handler->usrp, 0, &freq);
   return freq;
 }
 
@@ -571,7 +566,6 @@ double rf_uhd_set_tx_freq(void *h, double freq)
   for (int i=0;i<handler->nof_tx_channels;i++) {
     uhd_usrp_set_tx_freq(handler->usrp, &tune_request, i, &tune_result);
   }
-  uhd_usrp_get_tx_freq(handler->usrp, 0, &freq);
   return freq;
 }
 

From f1bacd009a3bbfb6cc9690c27f1601f272b5c77c Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 16:28:26 +0200
Subject: [PATCH 154/269] Reduced time to sync to cell

---
 lib/src/phy/rf/rf_uhd_imp.c |  2 +-
 srsue/hdr/phy/phch_recv.h   |  2 +-
 srsue/hdr/ue.h              |  8 +++++--
 srsue/hdr/upper/rrc.h       |  8 ++++---
 srsue/src/phy/phch_recv.cc  | 46 ++++++++++++++++++++-----------------
 srsue/src/ue.cc             |  6 ++++-
 srsue/src/upper/rrc.cc      | 13 ++++-------
 7 files changed, 47 insertions(+), 38 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 8daf2b9f0..16fb2e07e 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -227,7 +227,7 @@ int rf_uhd_start_rx_stream(void *h)
         .stream_now = false
   };
   uhd_usrp_get_time_now(handler->usrp, 0, &stream_cmd.time_spec_full_secs, &stream_cmd.time_spec_frac_secs);
-  stream_cmd.time_spec_frac_secs += 0.5; 
+  stream_cmd.time_spec_frac_secs += 0.1;
   if (stream_cmd.time_spec_frac_secs > 1) {
     stream_cmd.time_spec_frac_secs -= 1;
     stream_cmd.time_spec_full_secs += 1; 
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 5bf9142d0..dad4890aa 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -149,7 +149,7 @@ private:
   srslte_ue_dl_t ue_dl_measure;
 
 
-  const static int RSRP_MEASURE_NOF_FRAMES = 20;
+  const static int RSRP_MEASURE_NOF_FRAMES = 5;
 
   int    cell_sync_sfn();
   int    cell_meas_rsrp();
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 406d014d0..11565ec00 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -57,6 +57,7 @@
 
 namespace srsue {
 
+//#define LOG_STDOUT
 
 /*******************************************************************************
   Main UE class
@@ -99,8 +100,11 @@ private:
   srslte::gw         gw;
   srsue::usim        usim;
 
-  srslte::logger_file logger;
-  //srslte::logger_stdout logger;
+#ifdef LOG_STDOUT
+    srslte::logger_stdout logger;
+#else
+    srslte::logger_file logger;
+#endif
   srslte::log_filter  rf_log;
   srslte::log_filter  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index bafbdc03c..81514b559 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -100,14 +100,16 @@ private:
   uint8_t transaction_id;
   bool drb_up;
 
+  // timeouts in ms
+
   uint32_t connecting_timeout;
-  static const uint32_t RRC_CONNECTING_TIMEOUT = 100;
+  static const uint32_t RRC_CONNECTING_TIMEOUT = 1000;
 
   uint32_t plmn_select_timeout;
-  static const uint32_t RRC_PLMN_SELECT_TIMEOUT = 1000;
+  static const uint32_t RRC_PLMN_SELECT_TIMEOUT = 10000;
 
   uint32_t select_cell_timeout;
-  static const uint32_t RRC_SELECT_CELL_TIMEOUT = 500;
+  static const uint32_t RRC_SELECT_CELL_TIMEOUT = 2000;
 
   uint8_t k_rrc_enc[32];
   uint8_t k_rrc_int[32];
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index b31d4e8ce..fd38b8cf6 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -93,13 +93,13 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
   }
 
 
-  if (srslte_ue_cellsearch_init_multi(&cs, SRSLTE_DEFAULT_MAX_FRAMES_PSS, radio_recv_wrapper_cs, nof_rx_antennas,
+  if (srslte_ue_cellsearch_init_multi(&cs, 5, radio_recv_wrapper_cs, nof_rx_antennas,
                                       radio_h)) {
     Error("Initiating UE cell search\n");
     return;
   }
 
-  srslte_ue_cellsearch_set_nof_valid_frames(&cs, SRSLTE_DEFAULT_NOF_VALID_PSS_FRAMES);
+  srslte_ue_cellsearch_set_nof_valid_frames(&cs, 2);
 
   // Set options defined in expert section
   set_ue_sync_opts(&cs.ue_sync);
@@ -211,6 +211,8 @@ bool phch_recv::set_cell() {
     return false;
   }
 
+  worker_com->set_cell(cell);
+
   for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
     if (!((phch_worker *) workers_pool->get_worker(i))->set_cell(cell)) {
       Error("Setting cell: initiating PHCH worker\n");
@@ -235,11 +237,9 @@ bool phch_recv::cell_search(int force_N_id_2) {
   bzero(found_cells, 3 * sizeof(srslte_ue_cellsearch_result_t));
 
   if (srate_mode != SRATE_FIND) {
-    printf("set rx rate\n");
     srate_mode = SRATE_FIND;
     radio_h->set_rx_srate(1.92e6);
   }
-  printf("start rx\n");
   radio_h->start_rx();
 
   /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
@@ -247,6 +247,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
   int ret = SRSLTE_ERROR;
 
   Info("Searching for cell...\n");
+  printf("."); fflush(stdout);
 
   if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
     ret = srslte_ue_cellsearch_scan_N_id_2(&cs, force_N_id_2, &found_cells[force_N_id_2]);
@@ -257,22 +258,24 @@ bool phch_recv::cell_search(int force_N_id_2) {
 
   last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
-  printf("stop rx\n");
-  radio_h->stop_rx();
-
   if (ret < 0) {
+    radio_h->stop_rx();
     Error("Error decoding MIB: Error searching PSS\n");
     return false;
   } else if (ret == 0) {
+    radio_h->stop_rx();
     Info("Could not find any cell in this frequency\n");
     return false;
   }
-
   // Save result
   cell.id = found_cells[max_peak_cell].cell_id;
   cell.cp = found_cells[max_peak_cell].cp;
   cellsearch_cfo = found_cells[max_peak_cell].cfo;
 
+  printf("\n");
+  Info("SYNC:  PSS/SSS detected: PCI=%d, CFO=%.1f KHz, CP=%s\n",
+       cell.id, cellsearch_cfo/1000, srslte_cp_string(cell.cp));
+
   if (srslte_ue_mib_sync_set_cell(&ue_mib_sync, cell.id, cell.cp)) {
     Error("Setting UE MIB cell\n");
     return false;
@@ -290,7 +293,6 @@ bool phch_recv::cell_search(int force_N_id_2) {
 
   /* Find and decode MIB */
   int sfn_offset;
-  radio_h->start_rx();
   ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
                                   40,
                                   bch_payload, &cell.nof_ports, &sfn_offset);
@@ -301,14 +303,18 @@ bool phch_recv::cell_search(int force_N_id_2) {
   srslte_ue_sync_reset(&ue_sync);
   srslte_ue_sync_set_cfo(&ue_sync, cellsearch_cfo);
 
-  Info("Setting ue_sync cfo=%f KHz\n", cellsearch_cfo/1000);
-
   if (ret == 1) {
     srslte_pbch_mib_unpack(bch_payload, &cell, NULL);
-    worker_com->set_cell(cell);
+
+    fprintf(stdout, "Found Cell:  PCI=%d, PRB=%d, Ports=%d, CFO=%.1f KHz\n",
+         cell.id, cell.nof_prb, cell.nof_ports, cellsearch_cfo/1000);
+
+    Info("SYNC:  MIB Decoded: PCI=%d, PRB=%d, Ports=%d, CFO=%.1f KHz\n",
+         cell.id, cell.nof_prb, cell.nof_ports, cellsearch_cfo/1000);
+
     return true;
   } else {
-    Warning("Error decoding MIB: Error decoding PBCH\n");
+    Warning("Found PSS but could not decode PBCH\n");
     return false;
   }
 }
@@ -329,7 +335,7 @@ int phch_recv::cell_sync_sfn(void) {
   if (ret == 1) {
     if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
       int sfn_offset = 0;
-      Info("SYNC:  Decoding MIB...\n");
+      Info("SYNC:  Trying to decode MIB...\n");
       int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
       if (n < 0) {
         Error("SYNC:  Error decoding MIB while synchronising SFN");
@@ -388,6 +394,7 @@ void phch_recv::resync_sfn() {
   radio_h->stop_rx();
   radio_h->start_rx();
   srslte_ue_mib_reset(&ue_mib);
+  Info("SYNC:  Starting SFN synchronization\n");
   sync_sfn_cnt = 0;
   phy_state = CELL_SELECT;
 }
@@ -397,7 +404,7 @@ void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
 }
 
 bool phch_recv::stop_sync() {
-  Info("SYNC:   Going to IDLE\n");
+  Info("SYNC:  Going to IDLE\n");
   phy_state = IDLE;
   int cnt=0;
   while(!is_in_idle && cnt<100) {
@@ -409,7 +416,6 @@ bool phch_recv::stop_sync() {
 
 void phch_recv::cell_search_inc()
 {
-  printf("cell search inc\n");
   cur_earfcn_index++;
   Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
   if (cur_earfcn_index >= 0) {
@@ -500,7 +506,6 @@ bool phch_recv::set_frequency()
     log_h->console("Searching cell in DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
 
-    printf("set frequency\n");
     radio_h->set_rx_freq(dl_freq);
     radio_h->set_tx_freq(ul_freq);
     ul_dl_factor = ul_freq / dl_freq;
@@ -518,12 +523,13 @@ void phch_recv::set_sampling_rate()
 {
   float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
 
+  Info("Setting sampling rate %.1f MHz\n", srate/1000000);
+
   if (30720 % ((int) srate / 1000) == 0) {
     radio_h->set_master_clock_rate(30.72e6);
   } else {
     radio_h->set_master_clock_rate(23.04e6);
   }
-
   srate_mode = SRATE_CAMP;
   radio_h->set_rx_srate(srate);
   radio_h->set_tx_srate(srate);
@@ -554,9 +560,7 @@ void phch_recv::run_thread() {
             resync_sfn();
           }
 
-          Info("SYNC:  Cell found. Synchronizing...\n");
         } else {
-          printf("no trobat in progress=%d\n", cell_search_in_progress);
           if (cell_search_in_progress) {
             cell_search_inc();
           }
@@ -636,7 +640,7 @@ void phch_recv::run_thread() {
             srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
             worker->set_tx_time(tx_time);
 
-            Debug("Settting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+            Debug("Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
             worker->set_tti(tti, tx_mutex_cnt);
             tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 675137e31..55e0c7d92 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -52,7 +52,9 @@ bool ue::init(all_args_t *args_)
 {
   args     = args_;
 
-  logger.init(args->log.filename);
+#ifndef LOG_STDOUT
+    logger.init(args->log.filename);
+#endif
   rf_log.init("RF  ", &logger);
   phy_log.init("PHY ", &logger, true);
   mac_log.init("MAC ", &logger, true);
@@ -64,7 +66,9 @@ bool ue::init(all_args_t *args_)
   usim_log.init("USIM", &logger);
 
   // Init logs
+#ifndef LOG_STDOUT
   logger.log("\n\n");
+#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   phy_log.set_level(level(args->log.phy_level));
   mac_log.set_level(level(args->log.mac_level));
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 1b24d05c1..cb7c117a3 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -249,14 +249,9 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
   si_acquire_state = SI_ACQUIRE_SIB1;
 
-  rrc_log->info("Found Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
+  rrc_log->info("New Cell: PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
                 cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
                 cell.earfcn, cell.rsrp);
-
-  rrc_log->console("Found Cell:  PCI=%d, PRB=%d, Ports=%d, EARFCN=%d, RSRP=%.1f dBm\n",
-                cell.phy_cell.id, cell.phy_cell.nof_prb, cell.phy_cell.nof_ports,
-                cell.earfcn, cell.rsrp);
-
 }
 
 // Detection of physical layer problems (5.3.11.1)
@@ -426,7 +421,7 @@ void rrc::run_thread() {
     switch(state) {
       case RRC_STATE_IDLE:
         if (nas->is_attached()) {
-          usleep(100000);
+          usleep(10000);
           rrc_log->info("RRC IDLE: NAS is attached, re-selecting cell...\n");
           plmn_select(selected_plmn_id);
         }
@@ -436,7 +431,7 @@ void rrc::run_thread() {
         if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
           rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
           sleep(1);
-          rrc_log->console("RRC PLMN Search: timeout expired. Searching again\n");
+          rrc_log->console("\nRRC PLMN Search: timeout expired. Searching again\n");
           plmn_select_timeout = 0;
           phy->cell_search_start();
         }
@@ -481,7 +476,7 @@ void rrc::run_thread() {
       default:
         break;
     }
-    usleep(10000);
+    usleep(1000);
   }
 }
 

From 394d8f166a83c67d9565acc293318908f5712139 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 18:05:07 +0200
Subject: [PATCH 155/269] Improved initial CFO estimation for PRACH. Fixed bug
 in PDCCH after changing cell

---
 lib/include/srslte/phy/sync/sync.h |  2 +
 lib/src/phy/sync/sync.c            | 34 ++++++++++----
 lib/src/phy/ue/ue_dl.c             |  3 ++
 lib/src/phy/ue/ue_mib.c            |  2 -
 srsue/hdr/phy/phch_recv.h          |  2 +-
 srsue/src/phy/phch_recv.cc         | 72 +++++++++++++++---------------
 srsue/src/phy/phch_worker.cc       | 26 +++++------
 7 files changed, 82 insertions(+), 59 deletions(-)

diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index a11c43c59..610de2d43 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -75,6 +75,8 @@ typedef struct SRSLTE_API {
   uint32_t frame_size;
   uint32_t max_offset;
   bool enable_cfo_corr;
+  bool mean_cfo2_isunset;
+  bool mean_cfo_isunset;
   float mean_cfo;
   float mean_cfo2; 
   int cfo_i;
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index 99d58a46f..2ec4bfc78 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -29,6 +29,7 @@
 #include <complex.h>
 #include <math.h>
 #include <srslte/srslte.h>
+#include <srslte/phy/sync/sync.h>
 
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/common/phy_common.h"
@@ -79,6 +80,8 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     q->max_offset = max_offset;
     q->sss_alg = SSS_FULL;
     q->max_frame_size = frame_size;
+    q->mean_cfo_isunset = true;
+    q->mean_cfo2_isunset = true;
 
     q->enable_cfo_corr = true; 
     if (srslte_cfo_init(&q->cfocorr, q->frame_size)) {
@@ -287,7 +290,10 @@ float srslte_sync_get_cfo(srslte_sync_t *q) {
 }
 
 void srslte_sync_set_cfo(srslte_sync_t *q, float cfo) {
-  q->mean_cfo = cfo;
+  q->mean_cfo  = cfo;
+  q->mean_cfo2 = cfo;
+  q->mean_cfo2_isunset = false;
+  q->mean_cfo_isunset  = false;
 }
 
 void srslte_sync_set_cfo_i(srslte_sync_t *q, int cfo_i) {
@@ -515,11 +521,16 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
     cf_t *input_cfo = input; 
 
     if (q->enable_cfo_corr) {
-      float cfo = cfo_estimate(q, input); 
-      
-      /* compute exponential moving average CFO */
-      q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
-      
+      float cfo = cfo_estimate(q, input);
+
+      if (q->mean_cfo_isunset) {
+        q->mean_cfo = cfo;
+        q->mean_cfo_isunset = false;
+      } else {
+        /* compute exponential moving average CFO */
+        q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
+      }
+
       /* Correct CFO with the averaged CFO estimation */
       srslte_cfo_correct(&q->cfocorr2, input, q->temp, -q->mean_cfo / q->fft_size);
       
@@ -574,7 +585,12 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
   
       if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
         float cfo2   = srslte_pss_synch_cfo_compute(&q->pss, &input[find_offset + peak_pos - q->fft_size]);
-        q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
+        if (q->mean_cfo2_isunset) {
+          q->mean_cfo2 = cfo2;
+          q->mean_cfo2_isunset = true;
+        } else {
+          q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
+        }
         
         ret = SRSLTE_SYNC_FOUND;
       } else {
@@ -596,7 +612,9 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
 }
 
 void srslte_sync_reset(srslte_sync_t *q) {
-  q->M_ext_avg = 0; 
+  q->mean_cfo2_isunset = true;
+  q->mean_cfo_isunset  = true;
+  q->M_ext_avg  = 0;
   q->M_norm_avg = 0; 
   srslte_pss_synch_reset(&q->pss);
 }
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 1c56f5ae1..95e670642 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -27,6 +27,7 @@
 #include "srslte/phy/ue/ue_dl.h"
 
 #include <string.h>
+#include <srslte/srslte.h>
 
 
 #define CURRENT_FFTSIZE   srslte_symbol_sz(q->cell.nof_prb)
@@ -228,6 +229,7 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
         fprintf(stderr, "Error creating PDSCH object\n");
         return SRSLTE_ERROR;
       }
+      q->current_rnti = 0;
     }
     ret = SRSLTE_SUCCESS;
   } else {
@@ -242,6 +244,7 @@ int srslte_ue_dl_set_cell(srslte_ue_dl_t *q, srslte_cell_t cell)
  * For the connection procedure, use srslte_pusch_encode_rnti() or srslte_pusch_decode_rnti() functions 
  */
 void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
+
   srslte_pdsch_set_rnti(&q->pdsch, rnti);
   
   // Compute UE-specific and Common search space for this RNTI
diff --git a/lib/src/phy/ue/ue_mib.c b/lib/src/phy/ue/ue_mib.c
index 7f68b9f23..49ab30657 100644
--- a/lib/src/phy/ue/ue_mib.c
+++ b/lib/src/phy/ue/ue_mib.c
@@ -149,8 +149,6 @@ int srslte_ue_mib_decode(srslte_ue_mib_t * q, cf_t *input,
   int ret = SRSLTE_SUCCESS;
   cf_t *ce_slot1[SRSLTE_MAX_PORTS]; 
 
-
-
   /* Run FFT for the slot symbols */
   srslte_ofdm_rx_sf(&q->fft, input, q->sf_symbols);
             
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index dad4890aa..784d99abc 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -140,7 +140,7 @@ private:
   uint32_t      current_earfcn;
 
   uint32_t      sync_sfn_cnt;
-  const static uint32_t SYNC_SFN_TIMEOUT = 100;
+  const static uint32_t SYNC_SFN_TIMEOUT = 200;
   float ul_dl_factor;
   int cur_earfcn_index;
   bool cell_search_in_progress;
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index fd38b8cf6..61eda8562 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -95,7 +95,7 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
 
   if (srslte_ue_cellsearch_init_multi(&cs, 5, radio_recv_wrapper_cs, nof_rx_antennas,
                                       radio_h)) {
-    Error("Initiating UE cell search\n");
+    Error("SYNC:  Initiating UE cell search\n");
     return;
   }
 
@@ -109,22 +109,22 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
   }
 
   if (srslte_ue_dl_init(&ue_dl_measure, SRSLTE_MAX_PRB, nof_rx_antennas)) {
-    Error("Initiating ue_dl_measure\n");
+    Error("SYNC:  Initiating ue_dl_measure\n");
     return;
   }
 
   if (srslte_ue_mib_init(&ue_mib, SRSLTE_MAX_PRB)) {
-    Error("Initiating UE MIB decoder\n");
+    Error("SYNC:  Initiating UE MIB decoder\n");
     return;
   }
 
   if (srslte_ue_sync_init_multi(&ue_sync, SRSLTE_MAX_PRB, false, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating ue_sync\n");
+    Error("SYNC:  Initiating ue_sync\n");
     return;
   }
 
   if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
-    Error("Initiating UE MIB synchronization\n");
+    Error("SYNC:  Initiating UE MIB synchronization\n");
     return;
   }
 
@@ -186,7 +186,7 @@ void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
   } else if (!worker_com->args->sss_algorithm.compare("full")) {
     sss_alg = SSS_FULL;
   } else {
-    Warning("Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
+    Warning("SYNC:  Invalid SSS algorithm %s. Using 'full'\n", worker_com->args->sss_algorithm.c_str());
   }
   srslte_sync_set_sss_algorithm(&q->strack, (sss_alg_t) sss_alg);
   srslte_sync_set_sss_algorithm(&q->sfind, (sss_alg_t) sss_alg);
@@ -195,11 +195,11 @@ void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
 bool phch_recv::set_cell() {
   cell_is_set = false;
   if (srslte_ue_mib_set_cell(&ue_mib, cell)) {
-    Error("Setting cell: initiating ue_mib\n");
+    Error("SYNC:  Setting cell: initiating ue_mib\n");
     return false;
   }
   if (srslte_ue_sync_set_cell(&ue_sync, cell)) {
-    Error("Setting cell: initiating ue_sync");
+    Error("SYNC:  Setting cell: initiating ue_sync");
     return false;
   }
 
@@ -207,7 +207,7 @@ bool phch_recv::set_cell() {
   set_ue_sync_opts(&ue_sync);
 
   if (srslte_ue_dl_set_cell(&ue_dl_measure, cell)) {
-    Error("Setting cell: initiating ue_dl_measure\n");
+    Error("SYNC:  Setting cell: initiating ue_dl_measure\n");
     return false;
   }
 
@@ -215,7 +215,7 @@ bool phch_recv::set_cell() {
 
   for (uint32_t i = 0; i < workers_pool->get_nof_workers(); i++) {
     if (!((phch_worker *) workers_pool->get_worker(i))->set_cell(cell)) {
-      Error("Setting cell: initiating PHCH worker\n");
+      Error("SYNC:  Setting cell: initiating PHCH worker\n");
       return false;
     }
   }
@@ -246,7 +246,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
   uint32_t max_peak_cell = 0;
   int ret = SRSLTE_ERROR;
 
-  Info("Searching for cell...\n");
+  Info("SYNC:  Searching for cell...\n");
   printf("."); fflush(stdout);
 
   if (force_N_id_2 >= 0 && force_N_id_2 < 3) {
@@ -260,11 +260,11 @@ bool phch_recv::cell_search(int force_N_id_2) {
 
   if (ret < 0) {
     radio_h->stop_rx();
-    Error("Error decoding MIB: Error searching PSS\n");
+    Error("SYNC:  Error decoding MIB: Error searching PSS\n");
     return false;
   } else if (ret == 0) {
     radio_h->stop_rx();
-    Info("Could not find any cell in this frequency\n");
+    Info("SYNC:  Could not find any cell in this frequency\n");
     return false;
   }
   // Save result
@@ -277,7 +277,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
        cell.id, cellsearch_cfo/1000, srslte_cp_string(cell.cp));
 
   if (srslte_ue_mib_sync_set_cell(&ue_mib_sync, cell.id, cell.cp)) {
-    Error("Setting UE MIB cell\n");
+    Error("SYNC:  Setting UE MIB cell\n");
     return false;
   }
 
@@ -314,7 +314,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
 
     return true;
   } else {
-    Warning("Found PSS but could not decode PBCH\n");
+    Warning("SYNC:  Found PSS but could not decode PBCH\n");
     return false;
   }
 }
@@ -328,14 +328,15 @@ int phch_recv::cell_sync_sfn(void) {
   srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
   ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
   if (ret < 0) {
-    Error("Error calling ue_sync_get_buffer");
+    Error("SYNC:  Error calling ue_sync_get_buffer");
     return -1;
   }
 
   if (ret == 1) {
     if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
       int sfn_offset = 0;
-      Info("SYNC:  Trying to decode MIB...\n");
+      Info("SYNC:  Trying to decode MIB... SNR=%.1f dB\n",
+           10*log10(srslte_chest_dl_get_snr(&ue_mib.chest)));
       int n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
       if (n < 0) {
         Error("SYNC:  Error decoding MIB while synchronising SFN");
@@ -375,10 +376,11 @@ int phch_recv::cell_meas_rsrp() {
       return -1;
     }
     float rsrp   = srslte_chest_dl_get_rsrp(&ue_dl_measure.chest);
+    float snr    = srslte_chest_dl_get_snr(&ue_dl_measure.chest);
     measure_rsrp = SRSLTE_VEC_CMA(rsrp, measure_rsrp, measure_cnt);
     measure_cnt++;
-    log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm\n",
-                measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000));
+    log_h->info("SYNC:  Measuring RSRP %d/%d, sf_idx=%d, RSRP=%.1f dBm, SNR=%.1f dB\n",
+                measure_cnt, RSRP_MEASURE_NOF_FRAMES, sf_idx, 10 * log10(rsrp / 1000), 10*log10(snr));
     if (measure_cnt >= RSRP_MEASURE_NOF_FRAMES) {
       return 1;
     }
@@ -433,7 +435,7 @@ void phch_recv::cell_search_next() {
   if (cell_search_in_progress) {
     cell_search_in_progress = false;
     if (!stop_sync()) {
-      log_h->warning("Couldn't stop sync\n");
+      log_h->warning("SYNC:  Couldn't stop PHY\n");
     }
     cell_search_inc();
     phy_state = CELL_SEARCH;
@@ -446,9 +448,9 @@ void phch_recv::cell_search_start() {
     cell_search_in_progress = true;
     cur_earfcn_index = -1;
     cell_search_next();
-    log_h->info("Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+    log_h->info("SYNC:  Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
   } else {
-    log_h->info("Empty EARFCN list. Stopping cell search...\n");
+    log_h->info("SYNC:  Empty EARFCN list. Stopping cell search...\n");
     log_h->console("Empty EARFCN list. Stopping cell search...\n");
   }
 }
@@ -500,7 +502,7 @@ bool phch_recv::set_frequency()
   float dl_freq = 1e6*srslte_band_fd(current_earfcn);
   float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(current_earfcn));
   if (dl_freq > 0 && ul_freq > 0) {
-    log_h->info("Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
+    log_h->info("SYNC:  Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
 
     log_h->console("Searching cell in DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
@@ -514,7 +516,7 @@ bool phch_recv::set_frequency()
 
     return true;
   } else {
-    log_h->error("Cell Search: Invalid EARFCN=%d\n", current_earfcn);
+    log_h->error("SYNC:  Cell Search: Invalid EARFCN=%d\n", current_earfcn);
     return false;
   }
 }
@@ -523,7 +525,7 @@ void phch_recv::set_sampling_rate()
 {
   float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
 
-  Info("Setting sampling rate %.1f MHz\n", srate/1000000);
+  Info("SYNC:  Setting sampling rate %.2f MHz\n", srate/1000000);
 
   if (30720 % ((int) srate / 1000) == 0) {
     radio_h->set_master_clock_rate(30.72e6);
@@ -545,13 +547,13 @@ void phch_recv::run_thread() {
   while (running) {
     if (phy_state != IDLE) {
       is_in_idle = false;
-      Debug("SYNC state=%d\n", phy_state);
+      Debug("SYNC:  state=%d\n", phy_state);
     }
     switch (phy_state) {
       case CELL_SEARCH:
         if (cell_search() && cell_search_in_progress) {
           if (!srslte_cell_isvalid(&cell)) {
-            Error("Detected invalid cell\n");
+            Error("SYNC:  Detected invalid cell\n");
             phy_state = IDLE;
             break;
           }
@@ -572,7 +574,7 @@ void phch_recv::run_thread() {
 
         switch (cell_sync_sfn()) {
           default:
-            log_h->console("Going IDLE\n");
+            log_h->console("SYNC:  Going IDLE\n");
             phy_state = IDLE;
             break;
           case 1:
@@ -592,14 +594,14 @@ void phch_recv::run_thread() {
         sync_sfn_cnt++;
         if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
           sync_sfn_cnt = 0;
-          phy_state = IDLE;
-          log_h->warning("Timeout while synchronizing SFN\n");
+          phy_state = CELL_SEARCH;
+          log_h->warning("SYNC:  Timeout while synchronizing SFN\n");
         }
         break;
       case CELL_MEASURE:
         switch(cell_meas_rsrp()) {
           case 1:
-            log_h->info("Measured OK. Camping on cell PCI=%d...\n", cell.id);
+            log_h->info("SYNC:  Measured OK. Camping on cell PCI=%d...\n", cell.id);
             phy_state = CELL_CAMP;
             rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
             break;
@@ -623,7 +625,7 @@ void phch_recv::run_thread() {
 
             log_h->step(tti);
 
-            Debug("Worker %d synchronized\n", worker->get_id());
+            Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
 
             metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
             metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
@@ -640,7 +642,7 @@ void phch_recv::run_thread() {
             srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
             worker->set_tx_time(tx_time);
 
-            Debug("Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+            Debug("SYNC:  Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
             worker->set_tti(tti, tx_mutex_cnt);
             tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
 
@@ -657,10 +659,10 @@ void phch_recv::run_thread() {
             // Notify RRC in-sync every 1 frame
             if ((tti % 10) == 0) {
               rrc->in_sync();
-              log_h->debug("Sending in-sync to RRC\n");
+              log_h->debug("SYNC:  Sending in-sync to RRC\n");
             }
           } else {
-            log_h->error("Sync error. Sending out-of-sync to RRC\n");
+            log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
             // Notify RRC of out-of-sync frame
             rrc->out_of_sync();
             worker->release();
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 1437f7fce..5d2823366 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -405,7 +405,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     srslte_dci_msg_t dci_msg; 
     srslte_ra_dl_dci_t dci_unpacked;
     
-    Debug("Looking for RNTI=0x%x\n", dl_rnti);
+    Info("Looking for RNTI=0x%x\n", dl_rnti);
     
     if (srslte_ue_dl_find_dl_dci_type(&ue_dl, phy->config->dedicated.antenna_info_explicit_value.tx_mode, cfi, tti%10,
                                       dl_rnti, type, &dci_msg) != 1) {
@@ -681,22 +681,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
-    if (nof_tb > 0) {
-      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    }
+void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
+  if (nof_tb > 0) {
+    uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+  }
 
-    if (nof_tb > 1) {
-      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    }
+  if (nof_tb > 1) {
+    uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+  }
 
-    if (nof_tb > 2) {
-      Error("Number of transport blocks is not supported");
-    }
+  if (nof_tb > 2) {
+    Error("Number of transport blocks is not supported");
+  }
 
-    uci_data.uci_ack_len = nof_tb;
+  uci_data.uci_ack_len = nof_tb;
 
-  }
+}
 
 void phch_worker::set_uci_sr()
 {

From ea0f6f2e2aa5d8ba9cfed1c8e259a0aa7a1deaee Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 6 Sep 2017 18:13:37 +0200
Subject: [PATCH 156/269] removed pdcch log

---
 srsue/src/phy/phch_worker.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 5d2823366..fff8d2ca4 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -405,7 +405,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     srslte_dci_msg_t dci_msg; 
     srslte_ra_dl_dci_t dci_unpacked;
     
-    Info("Looking for RNTI=0x%x\n", dl_rnti);
+    Debug("Looking for RNTI=0x%x\n", dl_rnti);
     
     if (srslte_ue_dl_find_dl_dci_type(&ue_dl, phy->config->dedicated.antenna_info_explicit_value.tx_mode, cfi, tti%10,
                                       dl_rnti, type, &dci_msg) != 1) {

From ea368516c951aea6b8f4e37b09072e51a718b37f Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 7 Sep 2017 12:52:45 +0200
Subject: [PATCH 157/269] fixed plot for MIMO

---
 srsue/src/phy/phch_worker.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 53dc232dc..4618f2d16 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -1054,7 +1054,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
 {
 
-  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  memcpy(pdsch_d, ue_dl.pdsch.d[0], ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
   return ue_dl.pdsch_cfg.nbits[0].nof_re;
 }
 

From 25e9acd0693d9f47ccca0f38d2a596b8fdb78f2e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 7 Sep 2017 13:03:53 +0200
Subject: [PATCH 158/269] restore RX gain to 50 dB

---
 srsenb/enb.conf.example | 2 +-
 srsue/ue.conf.example   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 1c54cfc45..6847f7392 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -59,7 +59,7 @@ drb_config = drb.conf
 [rf]
 dl_earfcn = 3400
 tx_gain = 80
-rx_gain = 60
+rx_gain = 50
 
 #device_name = auto
 #device_args = auto
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 8c9ba7d3d..ea162e337 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -24,7 +24,7 @@
 dl_freq = 2685000000
 ul_freq = 2565000000
 tx_gain = 80
-rx_gain = 60
+rx_gain = 50
 
 #nof_rx_ant = 1
 #device_name = auto

From f0d9b333b20362fe6b9b4c3b0b9963fdf9ef5b8e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 7 Sep 2017 13:19:53 +0200
Subject: [PATCH 159/269] Improved stability when UE reattaches

---
 lib/include/srslte/phy/phch/pdsch.h |  1 +
 lib/include/srslte/phy/phch/pusch.h |  1 +
 lib/include/srslte/upper/gw.h       |  2 ++
 lib/src/phy/phch/pdsch.c            |  5 +++-
 lib/src/phy/phch/pusch.c            |  6 ++--
 lib/src/upper/gw.cc                 | 44 +++++++++++++++++------------
 srsue/hdr/mac/ul_harq.h             |  3 +-
 srsue/src/phy/phch_worker.cc        | 28 +++++++++---------
 8 files changed, 54 insertions(+), 36 deletions(-)

diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7c959f65c..7ef2d9b3d 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -61,6 +61,7 @@ typedef struct SRSLTE_API {
   
   uint32_t max_re;
 
+  uint16_t ue_rnti;
   bool is_ue;
 
   /* buffers */
diff --git a/lib/include/srslte/phy/phch/pusch.h b/lib/include/srslte/phy/phch/pusch.h
index 834750e38..2328c8ff3 100644
--- a/lib/include/srslte/phy/phch/pusch.h
+++ b/lib/include/srslte/phy/phch/pusch.h
@@ -71,6 +71,7 @@ typedef struct SRSLTE_API {
   srslte_cell_t cell;
 
   bool is_ue;
+  uint16_t ue_rnti;
   uint32_t max_re;
 
   srslte_dft_precoding_t dft_precoding;  
diff --git a/lib/include/srslte/upper/gw.h b/lib/include/srslte/upper/gw.h
index a9d58d2af..aa92ddc32 100644
--- a/lib/include/srslte/upper/gw.h
+++ b/lib/include/srslte/upper/gw.h
@@ -74,6 +74,8 @@ private:
   bool                if_up;
   uint32_t            lcid;
 
+  uint32_t            current_ip_addr;
+
   long                ul_tput_bytes;
   long                dl_tput_bytes;
   struct timeval      metrics_time[3];
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 6bacf99cf..c33a0d1d3 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -381,6 +381,7 @@ int srslte_pdsch_set_rnti(srslte_pdsch_t *q, uint16_t rnti) {
         }
       }
     }
+    q->ue_rnti = rnti; 
     q->users[rnti_idx]->cell_id = q->cell.id;
     q->users[rnti_idx]->sequence_generated = true;
   } else {
@@ -400,6 +401,7 @@ void srslte_pdsch_free_rnti(srslte_pdsch_t* q, uint16_t rnti)
     }
     free(q->users[rnti_idx]);
     q->users[rnti_idx] = NULL;
+    q->ue_rnti = 0;
   }
 }
 
@@ -527,7 +529,8 @@ static srslte_sequence_t *get_user_sequence(srslte_pdsch_t *q, uint16_t rnti,
 
   // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
   if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
-      q->users[rnti_idx]->cell_id == q->cell.id &&
+      q->users[rnti_idx]->cell_id == q->cell.id                    &&
+      q->ue_rnti == rnti                                           &&
       ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
   {
     return &q->users[rnti_idx]->seq[codeword_idx][sf_idx];
diff --git a/lib/src/phy/phch/pusch.c b/lib/src/phy/phch/pusch.c
index ee01484d8..f52bbbb42 100644
--- a/lib/src/phy/phch/pusch.c
+++ b/lib/src/phy/phch/pusch.c
@@ -453,6 +453,7 @@ int srslte_pusch_set_rnti(srslte_pusch_t *q, uint16_t rnti) {
         return SRSLTE_ERROR;
       }
     }
+    q->ue_rnti = rnti;
     q->users[rnti_idx]->cell_id = q->cell.id;
     q->users[rnti_idx]->sequence_generated = true;
   } else {
@@ -471,17 +472,18 @@ void srslte_pusch_free_rnti(srslte_pusch_t *q, uint16_t rnti) {
     }    
     free(q->users[rnti_idx]);
     q->users[rnti_idx] = NULL;
+    q->ue_rnti = 0;
   }
 }
 
-
 static srslte_sequence_t *get_user_sequence(srslte_pusch_t *q, uint16_t rnti, uint32_t sf_idx, uint32_t len)
 {
   uint32_t rnti_idx = q->is_ue?0:rnti;
 
   // The scrambling sequence is pregenerated for all RNTIs in the eNodeB but only for C-RNTI in the UE
   if (q->users[rnti_idx] && q->users[rnti_idx]->sequence_generated &&
-      q->users[rnti_idx]->cell_id == q->cell.id &&
+      q->users[rnti_idx]->cell_id == q->cell.id                    &&
+      q->ue_rnti == rnti                                           &&
       ((rnti >= SRSLTE_CRNTI_START && rnti < SRSLTE_CRNTI_END) || !q->is_ue))
   {
     return &q->users[rnti_idx]->seq[sf_idx];
diff --git a/lib/src/upper/gw.cc b/lib/src/upper/gw.cc
index af5e45762..321d8d9e9 100644
--- a/lib/src/upper/gw.cc
+++ b/lib/src/upper/gw.cc
@@ -42,7 +42,9 @@ namespace srslte {
 
 gw::gw()
   :if_up(false)
-{}
+{
+  current_ip_addr = 0;
+}
 
 void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_)
 {
@@ -77,6 +79,8 @@ void gw::stop()
         thread_cancel();
       }
       wait_thread_finish();
+
+      current_ip_addr = 0;
     }
 
     // TODO: tear down TUN device?
@@ -126,38 +130,42 @@ void gw::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 *******************************************************************************/
 error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
 {
-  if(!if_up)
-  {
+  if (ip_addr != current_ip_addr) {
+    if(!if_up)
+    {
       if(init_if(err_str))
       {
         gw_log->error("init_if failed\n");
         return(ERROR_CANT_START);
       }
-  }
+    }
 
-  // Setup the IP address
-  sock                                                   = socket(AF_INET, SOCK_DGRAM, 0);
-  ifr.ifr_addr.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr.s_addr = htonl(ip_addr);
-  if(0 > ioctl(sock, SIOCSIFADDR, &ifr))
-  {
+    // Setup the IP address
+    sock                                                   = socket(AF_INET, SOCK_DGRAM, 0);
+    ifr.ifr_addr.sa_family                                 = AF_INET;
+    ((struct sockaddr_in *)&ifr.ifr_addr)->sin_addr.s_addr = htonl(ip_addr);
+    if(0 > ioctl(sock, SIOCSIFADDR, &ifr))
+    {
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket address: %s\n", err_str);
       close(tun_fd);
       return(ERROR_CANT_START);
-  }
-  ifr.ifr_netmask.sa_family                                 = AF_INET;
-  ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
-  if(0 > ioctl(sock, SIOCSIFNETMASK, &ifr))
-  {
+    }
+    ifr.ifr_netmask.sa_family                                 = AF_INET;
+    ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
+    if(0 > ioctl(sock, SIOCSIFNETMASK, &ifr))
+    {
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket netmask: %s\n", err_str);
       close(tun_fd);
       return(ERROR_CANT_START);
-  }
+    }
 
-  // Setup a thread to receive packets from the TUN device
-  start(GW_THREAD_PRIO);
+    current_ip_addr = ip_addr;
+
+    // Setup a thread to receive packets from the TUN device
+    start(GW_THREAD_PRIO);
+  }
 
   return(ERROR_NONE);
 }
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 9ce3b37ca..e57af77ba 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -369,7 +369,8 @@ private:
         current_irv = 0;
         is_msg3 = is_msg3_;
         Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
-             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0], cur_grant.rnti);
+             pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0],
+             is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti);
         generate_tx(tti_tx, action);
       }
     }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index fff8d2ca4..0e9932224 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -606,7 +606,7 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
     {
       Error("Converting RAR message to UL grant\n");
       return false; 
-    } 
+    }
     grant->rnti_type = SRSLTE_RNTI_TEMP;
     grant->is_from_rar = true; 
     grant->has_cqi_request = false; // In contention-based Random Access CQI request bit is reserved
@@ -681,22 +681,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
-  if (nof_tb > 0) {
-    uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-  }
+  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
+    if (nof_tb > 0) {
+      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    }
 
-  if (nof_tb > 1) {
-    uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-  }
+    if (nof_tb > 1) {
+      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    }
 
-  if (nof_tb > 2) {
-    Error("Number of transport blocks is not supported");
-  }
+    if (nof_tb > 2) {
+      Error("Number of transport blocks is not supported");
+    }
 
-  uci_data.uci_ack_len = nof_tb;
+    uci_data.uci_ack_len = nof_tb;
 
-}
+  }
 
 void phch_worker::set_uci_sr()
 {
@@ -1075,7 +1075,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
 int phch_worker::read_pdsch_d(cf_t* pdsch_d)
 {
 
-  memcpy(pdsch_d, ue_dl.pdsch.d, ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  memcpy(pdsch_d, ue_dl.pdsch.d[0], ue_dl.pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
   return ue_dl.pdsch_cfg.nbits[0].nof_re;
 }
 

From 8661503d59c8508dbe9813bb52223812f61eb2b6 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 7 Sep 2017 13:03:53 +0200
Subject: [PATCH 160/269] restore RX gain to 50 dB

---
 srsenb/enb.conf.example | 2 +-
 srsue/ue.conf.example   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 1c54cfc45..6847f7392 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -59,7 +59,7 @@ drb_config = drb.conf
 [rf]
 dl_earfcn = 3400
 tx_gain = 80
-rx_gain = 60
+rx_gain = 50
 
 #device_name = auto
 #device_args = auto
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index c281a34fd..76868008f 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -24,7 +24,7 @@
 dl_earfcn = 3400
 freq_offset = 0
 tx_gain = 80
-rx_gain = 60
+rx_gain = 50
 
 #nof_rx_ant = 1
 #device_name = auto

From 6a668ef27fcbc05743cf8a2225ad20b4d06685e7 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 7 Sep 2017 17:46:30 +0200
Subject: [PATCH 161/269] Added PUSCH ACK decoder and test

---
 lib/include/srslte/phy/phch/uci.h  |  3 +-
 lib/src/phy/phch/sch.c             |  5 ++-
 lib/src/phy/phch/test/pusch_test.c | 15 ++++++--
 lib/src/phy/phch/uci.c             | 56 +++++++++++++++++++++---------
 4 files changed, 57 insertions(+), 22 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index bad87866e..c49df67f2 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -130,7 +130,8 @@ SRSLTE_API int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg,
                                      uint32_t H_prime_total, 
                                      uint32_t O_cqi,
                                      srslte_uci_bit_t *ack_bits,
-                                     uint8_t *data); 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks);
 
 SRSLTE_API int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
                                     uint8_t data, 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 72ee50ee9..02ec906ff 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -657,14 +657,17 @@ int srslte_ulsch_uci_decode_ri_ack(srslte_sch_t *q, srslte_pusch_cfg_t *cfg, srs
   
   // Deinterleave and decode HARQ bits
   if (uci_data->uci_ack_len > 0) {
+    uint8_t acks[2] = {0, 0};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, &uci_data->uci_ack);
+    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, acks, uci_data->uci_ack_len);
     if (ret < 0) {
       return ret; 
     }
+    uci_data->uci_ack = acks[0];
+    uci_data->uci_ack_2 = acks[1];
     Q_prime_ack = (uint32_t) ret; 
 
     // Set zeros to HARQ bits
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index 0cb2847b7..3434fac72 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -172,14 +172,15 @@ int main(int argc, char **argv) {
   bzero(&uci_data_tx, sizeof(srslte_uci_data_t));
   uci_data_tx.uci_cqi_len = 4; 
   uci_data_tx.uci_ri_len = 0; 
-  uci_data_tx.uci_ack_len = 0; 
+  uci_data_tx.uci_ack_len = 2;
   memcpy(&uci_data_rx, &uci_data_tx, sizeof(srslte_uci_data_t));
     
   for (uint32_t i=0;i<20;i++) {
     uci_data_tx.uci_cqi [i] = 1;
   }
-  uci_data_tx.uci_ri = 1; 
-  uci_data_tx.uci_ack = 1; 
+  uci_data_tx.uci_ri = 1;
+  uci_data_tx.uci_ack = 1;
+  uci_data_tx.uci_ack_2 = 1;
 
   uint32_t nof_re = SRSLTE_NRE*cell.nof_prb*2*SRSLTE_CP_NSYMB(cell.cp);
   sf_symbols = srslte_vec_malloc(sizeof(cf_t) * nof_re);
@@ -250,11 +251,19 @@ int main(int argc, char **argv) {
   if (uci_data_tx.uci_ack_len) {
     if (uci_data_tx.uci_ack != uci_data_rx.uci_ack) {
       printf("UCI ACK bit error: %d != %d\n", uci_data_tx.uci_ack, uci_data_rx.uci_ack);
+      ret = SRSLTE_ERROR;
+    }
+  }
+  if (uci_data_tx.uci_ack_len > 1) {
+    if (uci_data_tx.uci_ack_2 != uci_data_rx.uci_ack_2) {
+      printf("UCI ACK 2 bit error: %d != %d\n", uci_data_tx.uci_ack_2, uci_data_rx.uci_ack_2);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_ri_len) {
     if (uci_data_tx.uci_ri != uci_data_rx.uci_ri) {
       printf("UCI RI bit error: %d != %d\n", uci_data_tx.uci_ri, uci_data_rx.uci_ri);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_cqi_len) {
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 5bf40cc17..cb274bb83 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -545,15 +545,35 @@ static void encode_ri_ack(uint8_t data, srslte_uci_bit_type_t q_encoded_bits[6],
   }
 }
                        
-static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos) 
+static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
 {
-  uint32_t p0 = pos[0].position;
-  uint32_t p1 = pos[1].position;
-  
-  uint32_t q0 = c_seq[p0]?q_bits[p0]:-q_bits[p0];  
-  uint32_t q1 = c_seq[p0]?q_bits[p1]:-q_bits[p1];
+  if (nof_data == 1) {
+    uint32_t p0 = pos[0].position;
+    uint32_t p1 = pos[1].position;
+
+    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+    int32_t q1 = c_seq[p0] ? q_bits[p1] : -q_bits[p1];
 
-  return -(q0+q1);
+    data[0] -= q0 + q1;
+  } else {
+    uint32_t p0 = pos[Qm * 0 + 0].position;
+    uint32_t p1 = pos[Qm * 0 + 1].position;
+    uint32_t p2 = pos[Qm * 1 + 6].position;
+    uint32_t p3 = pos[Qm * 1 + 7].position;
+    uint32_t p4 = pos[Qm * 2 + 12].position;
+    uint32_t p5 = pos[Qm * 2 + 13].position;
+
+    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+    int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
+    int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
+    int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
+    int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
+    int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
+
+    data[0] -= q0 + q3;
+    data[1] -= q1 + q4;
+    data[2] -= q2 + q5;
+  }
 }
 
 
@@ -562,25 +582,27 @@ static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *
  */
 int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_seq, 
                           float beta, uint32_t H_prime_total, 
-                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t *data)
+                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t acks[2], uint32_t nof_acks)
 {
-  int32_t rx_ack = 0; 
-  
+  int32_t acks_sum[3] = {0, 0, 0};
+
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
 
   // Use the same interleaver function to get the HARQ bit position
-  for (uint32_t i=0;i<Qprime;i++) {
+  for (uint32_t i = 0; i < Qprime; i += (nof_acks > 1) ? 3 : 1) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    rx_ack += (int32_t) decode_ri_ack(q_bits, c_seq, ack_bits);
+    decode_ri_ack(q_bits, c_seq, ack_bits, cfg->grant.Qm, acks_sum, nof_acks);
   }
-  
-  if (data) {
-    *data = rx_ack>0;
+
+  if (acks) {
+    acks[0] = (uint8_t)(acks_sum[0] > 0);
+    acks[1] = (uint8_t)(acks_sum[1] > 0);
+    // TODO: Do something with acks_sum[2]
   }
   return (int) Qprime;  
 }
@@ -629,7 +651,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   // Use the same interleaver function to get the HARQ bit position
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    rx_ri += (int32_t) decode_ri_ack(q_bits, c_seq, ri_bits);
+    decode_ri_ack(q_bits, c_seq, ri_bits, cfg->grant.Qm, &rx_ri, 1);
   }
 
   if (data) {

From c04eadaa6b5c4f08df6d1cd44f1a8db49d702d07 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 7 Sep 2017 16:30:15 +0200
Subject: [PATCH 162/269] PUSCH supports 2 ACK

---
 lib/include/srslte/phy/phch/uci.h |  5 +--
 lib/src/phy/phch/sch.c            |  4 ++-
 lib/src/phy/phch/uci.c            | 54 ++++++++++++++++++++++---------
 3 files changed, 45 insertions(+), 18 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index c49df67f2..a674e43ea 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -117,8 +117,9 @@ SRSLTE_API int srslte_uci_decode_cqi_pusch(srslte_uci_cqi_pusch_t *q,
                                            bool *cqi_ack); 
 
 SRSLTE_API int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg,
-                                     uint8_t data, 
-                                     uint32_t O_cqi, 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks,
+                                     uint32_t O_cqi,
                                      float beta, 
                                      uint32_t H_prime_total, 
                                      srslte_uci_bit_t *ri_bits); 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 02ec906ff..a44ced2d8 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -808,11 +808,13 @@ int srslte_ulsch_uci_encode(srslte_sch_t *q,
   
   // Encode (and interleave) ACK
   if (uci_data.uci_ack_len > 0) {
+    uint8_t acks [2] = {uci_data.uci_ack, uci_data.uci_ack_2};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_encode_ack(cfg, uci_data.uci_ack, uci_data.uci_cqi_len, beta, nb_q/Qm, &q->ack_ri_bits[Q_prime_ri*Qm]);
+    ret = srslte_uci_encode_ack(cfg, acks, uci_data.uci_ack_len, uci_data.uci_cqi_len,
+                                beta, nb_q / Qm, &q->ack_ri_bits[Q_prime_ri * Qm]);
     if (ret < 0) {
       return ret; 
     }
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index cb274bb83..f649abd1a 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -536,13 +536,35 @@ static uint32_t Q_prime_ri_ack(srslte_pusch_cfg_t *cfg,
   return Q_prime; 
 }
 
-static void encode_ri_ack(uint8_t data, srslte_uci_bit_type_t q_encoded_bits[6], uint8_t Qm) 
+static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit_type_t q_encoded_bits[18], uint8_t Qm)
 {
-  q_encoded_bits[0] = data?UCI_BIT_1:UCI_BIT_0;
-  q_encoded_bits[1] = UCI_BIT_REPETITION; 
-  for (uint32_t i=2;i<Qm;i++) {
-    q_encoded_bits[i] = UCI_BIT_PLACEHOLDER;
+  uint32_t i = 0;
+
+  if (data_len == 1) {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = UCI_BIT_REPETITION;
+    while(i < Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+  } else {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*2) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*3) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
   }
+
+  return i;
 }
                        
 static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
@@ -610,23 +632,23 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
 /* Encode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
  *  Currently only supporting 1-bit HARQ
  */
-int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t data, 
+int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t acks[2], uint32_t nof_acks,
                           uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                           srslte_uci_bit_t *ack_bits)
-{  
+{
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(acks, nof_acks, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;
@@ -666,22 +688,24 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
  *  Currently only supporting 1-bit RI
  */
 int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
-                         uint8_t data, 
+                         uint8_t ri,
                          uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                          srslte_uci_bit_t *ri_bits)
 {
+  // FIXME: It supports RI of 1 bit only
+  uint8_t data[2] = {ri, 0};
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
   uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(data, 1, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;

From 9e4528007cbad2be6eae68dfeb0214fd9406434f Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 10:41:52 +0200
Subject: [PATCH 163/269] Improved ACK and RI in PUSCH decoding

---
 lib/src/phy/phch/uci.c | 60 +++++++++++++++++++-----------------------
 1 file changed, 27 insertions(+), 33 deletions(-)

diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index f649abd1a..42d3e6ef2 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -567,35 +567,25 @@ static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit
   return i;
 }
                        
-static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
+static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[3])
 {
-  if (nof_data == 1) {
-    uint32_t p0 = pos[0].position;
-    uint32_t p1 = pos[1].position;
-
-    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
-    int32_t q1 = c_seq[p0] ? q_bits[p1] : -q_bits[p1];
-
-    data[0] -= q0 + q1;
-  } else {
-    uint32_t p0 = pos[Qm * 0 + 0].position;
-    uint32_t p1 = pos[Qm * 0 + 1].position;
-    uint32_t p2 = pos[Qm * 1 + 6].position;
-    uint32_t p3 = pos[Qm * 1 + 7].position;
-    uint32_t p4 = pos[Qm * 2 + 12].position;
-    uint32_t p5 = pos[Qm * 2 + 13].position;
-
-    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
-    int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
-    int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
-    int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
-    int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
-    int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
-
-    data[0] -= q0 + q3;
-    data[1] -= q1 + q4;
-    data[2] -= q2 + q5;
-  }
+  uint32_t p0 = pos[Qm * 0 + 0].position;
+  uint32_t p1 = pos[Qm * 0 + 1].position;
+  uint32_t p2 = pos[Qm * 1 + 0].position;
+  uint32_t p3 = pos[Qm * 1 + 1].position;
+  uint32_t p4 = pos[Qm * 2 + 0].position;
+  uint32_t p5 = pos[Qm * 2 + 1].position;
+
+  int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+  int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
+  int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
+  int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
+  int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
+  int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
+
+  data[0] -= q0 + q3;
+  data[1] -= q1 + q4;
+  data[2] -= q2 + q5;
 }
 
 
@@ -616,9 +606,11 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
   uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
 
   // Use the same interleaver function to get the HARQ bit position
-  for (uint32_t i = 0; i < Qprime; i += (nof_acks > 1) ? 3 : 1) {
+  for (uint32_t i = 0; i < Qprime; i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    decode_ri_ack(q_bits, c_seq, ack_bits, cfg->grant.Qm, acks_sum, nof_acks);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ack_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, acks_sum);
+    }
   }
 
   if (acks) {
@@ -661,7 +653,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
                           float beta, uint32_t H_prime_total, 
                           uint32_t O_cqi, srslte_uci_bit_t *ri_bits, uint8_t *data)
 {
-  int32_t rx_ri = 0; 
+  int32_t ri_sum[3] = {0, 0, 0};
   
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
@@ -673,11 +665,13 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   // Use the same interleaver function to get the HARQ bit position
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    decode_ri_ack(q_bits, c_seq, ri_bits, cfg->grant.Qm, &rx_ri, 1);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ri_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, ri_sum);
+    }
   }
 
   if (data) {
-    *data = rx_ri>0;
+    *data = (uint8_t) ((ri_sum[0] + ri_sum[1] + ri_sum[2]) > 0);
   }
 
   return (int) Qprime;  

From 52bdd9290d39074b9db9eb40cd78924d4241185a Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 11:36:43 +0200
Subject: [PATCH 164/269] Added RI reporting for TM3 and TM4 (no PMI)

---
 lib/include/srslte/phy/ue/ue_dl.h |  2 +-
 lib/src/phy/ue/ue_dl.c            |  4 ++--
 srsue/src/phy/phch_worker.cc      | 17 +++++++++++++----
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index d0723fae7..0f8b25ece 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -187,7 +187,7 @@ SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           float *current_sinr);
 
 SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
-                                      uint32_t *ri,
+                                      uint8_t *ri,
                                       float *cn);
 
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index a67538ff7..dc88685b6 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -516,7 +516,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
 
 /* Compute the Rank Indicator (RI) by computing the condition number, valid for TM3 */
-int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
+int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint8_t *ri, float *cn) {
   float _cn;
   int ret = srslte_pdsch_cn_compute(&q->pdsch, q->ce_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), &_cn);
 
@@ -527,7 +527,7 @@ int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
 
   /* Set rank indicator */
   if (!ret && ri) {
-    *ri = (_cn > 3.0f)? 1:0;
+    *ri = (uint8_t)((_cn < 17.0f)? 1:0);
   }
 
   return ret;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 6e77b3a3e..e2c181c98 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -228,6 +228,14 @@ void phch_worker::work_imp()
       if (dl_action.generate_ack) {
         set_uci_ack(dl_ack, dl_mac_grant.tb_en);
       }
+
+      /* Select Rank Indicator by computing Condition Number */
+      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3 ||
+          phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4) {
+        float cn = 0.0f;
+        srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
+        uci_data.uci_ri_len = 1;
+      }
     }
   }
   
@@ -527,9 +535,9 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
   #endif
 
         Info(
-            "PDSCH: l_crb=%2d, harq=%d, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
-            grant->nof_prb, harq_pid, grant->tb_en[0] ? "on" : "off", grant->tb_en[1] ? "on" : "off",
-            grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
+            "PDSCH: l_crb=%2d, harq=%d, scheme=%s, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d, %s\n",
+            grant->nof_prb, harq_pid, srslte_mimotype2str(mimo_type), grant->tb_en[0] ? "on" : "off",
+            grant->tb_en[1] ? "on" : "off", grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
             grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
             srslte_pdsch_last_noi(&ue_dl.pdsch),
@@ -821,11 +829,12 @@ void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, ui
   snprintf(timestr, 64, ", total_time=%4d us", (int) logtime_start[0].tv_usec);
 #endif
 
-  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, cfo=%.1f Hz%s\n", 
+  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, ri=%s, cfo=%.1f Hz%s\n",
          (tti+4)%10240,
          grant->L_prb, grant->n_prb[0], 
          grant->mcs.tbs/8, grant->mcs.mod, grant->mcs.idx, rv,
          uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+         uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
          cfo*15000, timestr);
 
   // Store metrics

From 0c31a5cfc1751ae1d133aa6b5510ec393ca28e2d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 16:24:34 +0200
Subject: [PATCH 165/269] Added periodic RI on PUCCH for TM3 and TM4

---
 lib/examples/pdsch_ue.c           |  2 +-
 lib/include/srslte/phy/phch/cqi.h |  3 +++
 lib/include/srslte/phy/phch/uci.h |  4 +++
 lib/include/srslte/phy/ue/ue_dl.h |  4 +--
 lib/src/phy/ue/ue_dl.c            | 10 ++++----
 lib/src/phy/ue/ue_ul.c            |  8 ++++++
 srsue/src/phy/phch_worker.cc      | 41 ++++++++++++++++++++++++++-----
 7 files changed, 58 insertions(+), 14 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index aa3e13257..71555234a 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -518,7 +518,7 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  uint32_t ri = 0, pmi = 0;
+  uint8_t ri = 0, pmi = 0;
   float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
       sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index 593592f1c..cfc9e92e7 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -41,11 +41,14 @@
 #include "srslte/phy/common/phy_common.h"
 
 #define SRSLTE_CQI_MAX_BITS 64
+#define SRSLTE_DIF_CQI_MAX_BITS 3
+#define SRSLTE_PMI_MAX_BITS 4
 
 typedef struct {
   bool     configured; 
   uint32_t pmi_idx; 
   uint32_t ri_idx;
+  bool ri_idx_present;
   bool     simul_cqi_ack;
   bool     format_is_subband; 
   uint32_t subband_size; 
diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index a674e43ea..0c53589e6 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -64,6 +64,10 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   uint8_t  uci_cqi[SRSLTE_CQI_MAX_BITS];
   uint32_t uci_cqi_len;
+  uint8_t  uci_dif_cqi[SRSLTE_DIF_CQI_MAX_BITS];
+  uint32_t uci_dif_cqi_len;
+  uint8_t  uci_pmi[SRSLTE_PMI_MAX_BITS];
+  uint8_t  uci_pmi_len;
   uint8_t  uci_ri;  // Only 1-bit supported for RI
   uint32_t uci_ri_len;
   uint8_t  uci_ack;   // 1st codeword bit for HARQ-ACK
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 0f8b25ece..95523be84 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -182,8 +182,8 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
                                         bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
-                                          uint32_t *ri,
-                                          uint32_t *pmi,
+                                          uint8_t *ri,
+                                          uint8_t *pmi,
                                           float *current_sinr);
 
 SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index dc88685b6..6d620be9d 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -454,10 +454,10 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
 
 /* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
  * Noise Ratio (SINR), valid for TM4 */
-int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
+int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, float *current_sinr) {
   float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
   float best_sinr = -INFINITY;
-  uint32_t best_pmi = 0, best_ri = 0;
+  uint8_t best_pmi = 0, best_ri = 0;
 
   if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
     /* Do nothing */
@@ -471,11 +471,11 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
     for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
-      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers;
+      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers * nof_layers;
       if (_sinr > best_sinr + 0.1) {
         best_sinr = _sinr;
-        best_pmi = q->pmi[nof_layers - 1];
-        best_ri = nof_layers;
+        best_pmi = (uint8_t) q->pmi[nof_layers - 1];
+        best_ri = (uint8_t) (nof_layers - 1);
       }
     }
 
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 08edcafe0..6c074ea45 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -230,6 +230,14 @@ void pucch_encode_bits(srslte_uci_data_t *uci_data, srslte_pucch_format_t format
     pucch_bits[1] = uci_data->uci_ack_2; // this will be ignored in format 1a 
   }
   if (format >= SRSLTE_PUCCH_FORMAT_2) {
+    /* Append Differential CQI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_dif_cqi, uci_data->uci_dif_cqi_len);
+    uci_data->uci_cqi_len += uci_data->uci_dif_cqi_len;
+
+    /* Append PMI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_pmi, uci_data->uci_pmi_len);
+    uci_data->uci_cqi_len += uci_data->uci_pmi_len;
+
     srslte_uci_encode_cqi_pucch(uci_data->uci_cqi, uci_data->uci_cqi_len, pucch_bits);
     if (format > SRSLTE_PUCCH_FORMAT_2) {
       pucch2_bits[0] = uci_data->uci_ack; 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index e2c181c98..9ef204cd4 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -230,11 +230,23 @@ void phch_worker::work_imp()
       }
 
       /* Select Rank Indicator by computing Condition Number */
-      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3 ||
-          phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4) {
+      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3) {
         float cn = 0.0f;
         srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
         uci_data.uci_ri_len = 1;
+      } else if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4){
+        float sinr = 0.0f;
+        uint8 packed_pmi = 0;
+        srslte_ue_dl_ri_pmi_select(&ue_dl, &uci_data.uci_ri, &packed_pmi, &sinr);
+        srslte_bit_unpack_vector(&packed_pmi, uci_data.uci_pmi, 2);
+        uci_data.uci_ri_len = 1;
+        if (uci_data.uci_ri == 0) {
+          uci_data.uci_pmi_len = 2;
+          uci_data.uci_dif_cqi_len = 0;
+        } else {
+          uci_data.uci_pmi_len = 1;
+          uci_data.uci_dif_cqi_len = 3;
+        }
       }
     }
   }
@@ -714,7 +726,16 @@ void phch_worker::set_uci_periodic_cqi()
   int cqi_max       = phy->args->cqi_max;
   
   if (period_cqi.configured && rnti_is_set) {
-    if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
+    if (period_cqi.ri_idx_present && srslte_ri_send(period_cqi.pmi_idx, period_cqi.ri_idx, (tti+4)%10240)) {
+      if (uci_data.uci_ri_len) {
+        uci_data.uci_cqi[0] = uci_data.uci_ri;
+        uci_data.uci_cqi_len = uci_data.uci_ri_len;
+        uci_data.uci_ri_len = 0;
+        uci_data.uci_dif_cqi_len = 0;
+        uci_data.uci_pmi_len = 0;
+        Info("PUCCH: Periodic RI=%d\n", uci_data.uci_cqi[0]);
+      }
+    } else if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
       srslte_cqi_value_t cqi_report;
       if (period_cqi.format_is_subband) {
         // TODO: Implement subband periodic reports
@@ -876,11 +897,12 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, sr=%s, cfo=%.1f Hz%s\n",
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
        uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
        uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+       uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
        uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   
@@ -1011,12 +1033,19 @@ void phch_worker::set_ul_params(bool pregen_disabled)
   /* CQI configuration */
   bzero(&period_cqi, sizeof(srslte_cqi_periodic_cfg_t));
   period_cqi.configured        = dedicated->cqi_report_cnfg.report_periodic_setup_present;
-  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx; 
+  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx;
   period_cqi.simul_cqi_ack     = dedicated->cqi_report_cnfg.report_periodic.simult_ack_nack_and_cqi;
   period_cqi.format_is_subband = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic ==
                                  LIBLTE_RRC_CQI_FORMAT_INDICATOR_PERIODIC_SUBBAND_CQI;
   period_cqi.subband_size      = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic_subband_k;
-  
+
+  if (dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx_present) {
+    period_cqi.ri_idx = dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx;
+    period_cqi.ri_idx_present = true;
+  } else {
+    period_cqi.ri_idx_present = false;
+  }
+
   /* SR configuration */
   I_sr                         = dedicated->sched_request_cnfg.sr_cnfg_idx;
   

From 7b854476a233a7f047c736e3ebae69bbaed65068 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 11:10:13 +0200
Subject: [PATCH 166/269] fixed radio set/get freq precision

---
 lib/include/srslte/radio/radio.h | 22 +++++++++++-----------
 lib/src/radio/radio.cc           | 16 ++++++++--------
 srsue/src/phy/phch_recv.cc       | 10 ++++------
 3 files changed, 23 insertions(+), 25 deletions(-)

diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index f04fee98d..30d274f44 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -98,16 +98,16 @@ namespace srslte {
       void set_tx_rx_gain_offset(float offset); 
       double set_rx_gain_th(float gain);
 
-      void set_freq_offset(float freq);
-      void set_tx_freq(float freq);
-      void set_rx_freq(float freq);
+      void set_freq_offset(double freq);
+      void set_tx_freq(double freq);
+      void set_rx_freq(double freq);
 
-      float get_tx_freq();
-      float get_rx_freq();
+      double get_tx_freq();
+      double get_rx_freq();
 
-      void set_master_clock_rate(float rate);
-      void set_tx_srate(float srate);
-      void set_rx_srate(float srate);
+      void set_master_clock_rate(double rate);
+      void set_tx_srate(double srate);
+      void set_rx_srate(double srate);
 
       float get_tx_gain();
       float get_rx_gain();
@@ -157,9 +157,9 @@ namespace srslte {
       
       const static double blade_default_burst_preamble_sec = 0.0;
       const static double blade_default_tx_adv_samples     = 27;
-      const static double blade_default_tx_adv_offset_sec  = 1e-6; 
-      
-      float tx_freq, rx_freq, freq_offset;
+      const static double blade_default_tx_adv_offset_sec  = 1e-6;
+
+      double tx_freq, rx_freq, freq_offset;
       
       trace<uint32_t> tr_local_time;
       trace<uint32_t> tr_usrp_time;
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index b43e837cd..c3592f168 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -250,11 +250,11 @@ void radio::save_trace(uint32_t is_eob, srslte_timestamp_t *tx_time) {
   }
 }
 
-void radio::set_freq_offset(float freq) {
+void radio::set_freq_offset(double freq) {
   freq_offset = freq;
 }
 
-void radio::set_rx_freq(float freq)
+void radio::set_rx_freq(double freq)
 {
   rx_freq = srslte_rf_set_rx_freq(&rf_device, freq+freq_offset);
 }
@@ -269,17 +269,17 @@ double radio::set_rx_gain_th(float gain)
   return srslte_rf_set_rx_gain_th(&rf_device, gain);
 }
 
-void radio::set_master_clock_rate(float rate)
+void radio::set_master_clock_rate(double rate)
 {
   srslte_rf_set_master_clock_rate(&rf_device, rate);
 }
 
-void radio::set_rx_srate(float srate)
+void radio::set_rx_srate(double srate)
 {
   srslte_rf_set_rx_srate(&rf_device, srate);
 }
 
-void radio::set_tx_freq(float freq)
+void radio::set_tx_freq(double freq)
 {
   tx_freq = srslte_rf_set_tx_freq(&rf_device, freq+freq_offset);
 }
@@ -289,12 +289,12 @@ void radio::set_tx_gain(float gain)
   srslte_rf_set_tx_gain(&rf_device, gain);
 }
 
-float radio::get_rx_freq()
+double radio::get_rx_freq()
 {
   return rx_freq;
 }
 
-float radio::get_tx_freq()
+double radio::get_tx_freq()
 {
   return tx_freq; 
 }
@@ -309,7 +309,7 @@ float radio::get_rx_gain()
   return srslte_rf_get_rx_gain(&rf_device);
 }
 
-void radio::set_tx_srate(float srate)
+void radio::set_tx_srate(double srate)
 {
   cur_tx_srate = srslte_rf_set_tx_srate(&rf_device, srate);
   burst_preamble_samples = (uint32_t) (cur_tx_srate * burst_preamble_sec);
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 61eda8562..5b725d49f 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -169,9 +169,7 @@ void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
     srslte_ue_sync_cfo_i_detec_en(q, true);
   }
 
-  float cfo_tol = worker_com->args->cfo_correct_tol_hz;
-  srslte_cfo_set_tol(&q->strack.cfocorr, cfo_tol / (15000 * q->fft_size));
-  srslte_cfo_set_tol(&q->sfind.cfocorr, cfo_tol / (15000 * q->fft_size));
+  srslte_ue_sync_set_cfo_tol(q, worker_com->args->cfo_correct_tol_hz);
 
   int time_correct_period = worker_com->args->time_correct_period;
   if (time_correct_period > 0) {
@@ -499,8 +497,8 @@ bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
 bool phch_recv::set_frequency()
 {
-  float dl_freq = 1e6*srslte_band_fd(current_earfcn);
-  float ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(current_earfcn));
+  double dl_freq = 1e6*srslte_band_fd(current_earfcn);
+  double ul_freq = 1e6*srslte_band_fu(srslte_band_ul_earfcn(current_earfcn));
   if (dl_freq > 0 && ul_freq > 0) {
     log_h->info("SYNC:  Set DL EARFCN=%d, f_dl=%.1f MHz, f_ul=%.1f MHz\n",
                 current_earfcn, dl_freq / 1e6, ul_freq / 1e6);
@@ -510,7 +508,7 @@ bool phch_recv::set_frequency()
 
     radio_h->set_rx_freq(dl_freq);
     radio_h->set_tx_freq(ul_freq);
-    ul_dl_factor = ul_freq / dl_freq;
+    ul_dl_factor = radio_h->get_tx_freq()/radio_h->get_rx_freq();
 
     srslte_ue_sync_reset(&ue_sync);
 

From 41188b409e8b85cf6fcba5298670c16ce93f338b Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 11:10:32 +0200
Subject: [PATCH 167/269] cleaned up cfo set tolerance functions

---
 lib/examples/pdsch_ue.c             |  8 +++++++-
 lib/include/srslte/phy/sync/sync.h  |  4 ++++
 lib/include/srslte/phy/ue/ue_sync.h |  3 +++
 lib/include/srslte/phy/ue/ue_ul.h   | 10 +++++++---
 lib/src/phy/sync/pss.c              | 14 +++++++-------
 lib/src/phy/sync/sync.c             | 25 +++++++++++++++----------
 lib/src/phy/ue/ue_sync.c            | 10 ++++++----
 lib/src/phy/ue/ue_ul.c              | 14 +++++++++++---
 srsue/src/phy/phch_worker.cc        |  3 ++-
 9 files changed, 62 insertions(+), 29 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index cf7bed2e4..b4f96a34b 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -585,6 +585,10 @@ int main(int argc, char **argv) {
       /* If a new line is detected set verbose level to Debug */
       if (fgets(input, sizeof(input), stdin)) {
         srslte_verbose = SRSLTE_VERBOSE_DEBUG;
+        ue_dl.pkt_errors = 0;
+        ue_dl.pkts_total = 0;
+        ue_dl.nof_detected = 0;
+        nof_trials = 0;
       }
     }
 
@@ -778,10 +782,12 @@ int main(int argc, char **argv) {
         if (sfn == 1024) {
           sfn = 0; 
           PRINT_LINE_ADVANCE_CURSOR();
+          /*
           ue_dl.pkt_errors = 0; 
           ue_dl.pkts_total = 0; 
           ue_dl.nof_detected = 0;           
-          nof_trials = 0; 
+          nof_trials = 0;
+          */
         } 
       }
       
diff --git a/lib/include/srslte/phy/sync/sync.h b/lib/include/srslte/phy/sync/sync.h
index 610de2d43..1c70b2449 100644
--- a/lib/include/srslte/phy/sync/sync.h
+++ b/lib/include/srslte/phy/sync/sync.h
@@ -87,6 +87,7 @@ typedef struct SRSLTE_API {
   uint32_t cp_len;
   srslte_cfo_t cfocorr;
   srslte_cfo_t cfocorr2;
+  float current_cfo_tol;
   sss_alg_t sss_alg; 
   bool detect_cp;
   bool sss_en;
@@ -147,6 +148,9 @@ SRSLTE_API srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q,
 SRSLTE_API void srslte_sync_set_threshold(srslte_sync_t *q, 
                                           float threshold);
 
+SRSLTE_API void srslte_sync_set_cfo_tol(srslte_sync_t *q,
+                                        float tol);
+
 /* Gets the subframe idx (0 or 5) */
 SRSLTE_API uint32_t srslte_sync_get_sf_idx(srslte_sync_t *q);
 
diff --git a/lib/include/srslte/phy/ue/ue_sync.h b/lib/include/srslte/phy/ue/ue_sync.h
index dde9fb566..bd280acd0 100644
--- a/lib/include/srslte/phy/ue/ue_sync.h
+++ b/lib/include/srslte/phy/ue/ue_sync.h
@@ -181,6 +181,9 @@ SRSLTE_API int srslte_ue_sync_zerocopy(srslte_ue_sync_t *q,
 SRSLTE_API int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, 
                                              cf_t *input_buffer[SRSLTE_MAX_PORTS]);
 
+SRSLTE_API void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q,
+                                           float tol);
+
 SRSLTE_API void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, 
                                        float cfo); 
 
diff --git a/lib/include/srslte/phy/ue/ue_ul.h b/lib/include/srslte/phy/ue/ue_ul.h
index 843948087..3492180e3 100644
--- a/lib/include/srslte/phy/ue/ue_ul.h
+++ b/lib/include/srslte/phy/ue/ue_ul.h
@@ -75,7 +75,8 @@ typedef struct SRSLTE_API {
   
   bool normalize_en; 
   bool cfo_en; 
-  
+
+  float current_cfo_tol;
   float current_cfo; 
   srslte_pucch_format_t last_pucch_format;
   
@@ -114,10 +115,13 @@ SRSLTE_API void srslte_ue_ul_free(srslte_ue_ul_t *q);
 SRSLTE_API int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
                                      srslte_cell_t cell);
 
-SRSLTE_API void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, 
+SRSLTE_API void srslte_ue_ul_set_cfo_tol(srslte_ue_ul_t *q,
+                                         float tol);
+
+SRSLTE_API void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q,
                                      float cur_cfo); 
 
-SRSLTE_API void srslte_ue_ul_set_cfo_enable(srslte_ue_ul_t *q, 
+SRSLTE_API void srslte_ue_ul_set_cfo_enable(srslte_ue_ul_t *q,
                                             bool enabled); 
 
 SRSLTE_API void srslte_ue_ul_set_normalization(srslte_ue_ul_t *q, 
diff --git a/lib/src/phy/sync/pss.c b/lib/src/phy/sync/pss.c
index 3d221528c..f7b35071d 100644
--- a/lib/src/phy/sync/pss.c
+++ b/lib/src/phy/sync/pss.c
@@ -30,6 +30,7 @@
 #include <stdlib.h>
 #include <complex.h>
 #include <math.h>
+#include <srslte/phy/sync/pss.h>
 
 #include "srslte/phy/sync/pss.h"
 #include "srslte/phy/dft/dft.h"
@@ -95,9 +96,9 @@ int srslte_pss_synch_init_fft_offset(srslte_pss_synch_t *q, uint32_t frame_size,
  */
 int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
                                            uint32_t max_frame_size, uint32_t max_fft_size,
-                                           int offset, int decimate) {
+                                           int offset, int decimate)
+{
 
-    
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
   if (q != NULL) {
   
@@ -121,7 +122,7 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     q->frame_size = frame_size;
 
     buffer_size = fft_size + frame_size + 1;
-    
+
     if(q->decimate > 1) {
         int filter_order = 3;
         srslte_filt_decim_cc_init(&q->filter,q->decimate,filter_order);
@@ -183,16 +184,15 @@ int srslte_pss_synch_init_fft_offset_decim(srslte_pss_synch_t *q,
     #ifdef CONVOLUTION_FFT
 
 
-    for(N_id_2 = 0; N_id_2<3; N_id_2++)
+    for(N_id_2=0; N_id_2<3; N_id_2++)
     q->pss_signal_freq_full[N_id_2]   = srslte_vec_malloc(buffer_size * sizeof(cf_t));
 
     if (srslte_conv_fft_cc_init(&q->conv_fft, frame_size, fft_size)) {
       fprintf(stderr, "Error initiating convolution FFT\n");
       goto clean_and_exit;
     }
-    for(int i =0; i< 3; i++)
-    {
-        srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
+    for(int i=0; i<3; i++) {
+      srslte_dft_run_c(&q->conv_fft.filter_plan, q->pss_signal_time[i], q->pss_signal_freq_full[i]);
     }
     
     #endif
diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index 2ec4bfc78..2d7128e39 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -38,9 +38,11 @@
 #include "srslte/phy/sync/cfo.h"
 
 #define MEANPEAK_EMA_ALPHA      0.1
-#define CFO_EMA_ALPHA           0.2
+#define CFO_EMA_ALPHA           0.1
 #define CP_EMA_ALPHA            0.1
 
+#define DEFAULT_CFO_TOL         50.0 // Hz
+
 static bool fft_size_isvalid(uint32_t fft_size) {
   if (fft_size >= SRSLTE_SYNC_FFT_SZ_MIN && fft_size <= SRSLTE_SYNC_FFT_SZ_MAX && (fft_size%64) == 0) {
     return true;
@@ -94,9 +96,8 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
       goto clean_exit;
     }
     
-    // Set a CFO tolerance of approx 50 Hz
-    srslte_cfo_set_tol(&q->cfocorr, 50.0/(15000.0*q->fft_size));
-    srslte_cfo_set_tol(&q->cfocorr2, 50.0/(15000.0*q->fft_size));
+    // Set default CFO tolerance
+    srslte_sync_set_cfo_tol(q, DEFAULT_CFO_TOL);
 
     for (int i=0;i<2;i++) {
       q->cfo_i_corr[i] = srslte_vec_malloc(sizeof(cf_t)*q->frame_size);
@@ -114,10 +115,11 @@ int srslte_sync_init_decim(srslte_sync_t *q, uint32_t frame_size, uint32_t max_o
     
     srslte_sync_set_cp(q, SRSLTE_CP_NORM);
     q->decimate = decimate;
-    if(!decimate)
+    if(!decimate) {
       decimate = 1;
+    }
 
-    if (srslte_pss_synch_init_fft_offset_decim(&q->pss, max_offset, fft_size,0,decimate)) {
+    if (srslte_pss_synch_init_fft_offset_decim(&q->pss, max_offset, fft_size, 0, decimate)) {
       fprintf(stderr, "Error initializing PSS object\n");
       goto clean_exit;
     }
@@ -220,9 +222,7 @@ int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offse
     }
 
     // Update CFO tolerance
-    srslte_cfo_set_tol(&q->cfocorr, 50.0/(15000.0*q->fft_size));
-    srslte_cfo_set_tol(&q->cfocorr2, 50.0/(15000.0*q->fft_size));
-
+    srslte_sync_set_cfo_tol(q, q->current_cfo_tol);
 
     DEBUG("SYNC init with frame_size=%d, max_offset=%d and fft_size=%d\n", frame_size, max_offset, fft_size);
 
@@ -234,6 +234,11 @@ int srslte_sync_resize(srslte_sync_t *q, uint32_t frame_size, uint32_t max_offse
   return ret;
 }
 
+void srslte_sync_set_cfo_tol(srslte_sync_t *q, float tol) {
+  q->current_cfo_tol = tol;
+  srslte_cfo_set_tol(&q->cfocorr, tol/(15000.0*q->fft_size));
+  srslte_cfo_set_tol(&q->cfocorr2, tol/(15000.0*q->fft_size));
+}
 
 void srslte_sync_set_threshold(srslte_sync_t *q, float threshold) {
   q->threshold = threshold;
@@ -591,7 +596,7 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
         } else {
           q->mean_cfo2 = SRSLTE_VEC_EMA(cfo2, q->mean_cfo2, q->cfo_ema_alpha);
         }
-        
+
         ret = SRSLTE_SYNC_FOUND;
       } else {
         ret = SRSLTE_SYNC_FOUND_NOSPACE; 
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index 02d28c258..2126950a6 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -210,7 +210,7 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
       if(srslte_sync_init(&q->strack, q->frame_len, TRACK_FRAME_SIZE, q->fft_size)) {
         fprintf(stderr, "Error initiating sync track\n");
         goto clean_exit;
-      }      
+      }
     } else {
       if(srslte_sync_init(&q->strack, q->frame_len, SRSLTE_CP_LEN_NORM(1,q->fft_size), q->fft_size)) {
         fprintf(stderr, "Error initiating sync track\n");
@@ -220,12 +220,12 @@ int srslte_ue_sync_init_multi_decim(srslte_ue_sync_t *q,
 
     ret = SRSLTE_SUCCESS;
   }
-  
+
 clean_exit:
   if (ret == SRSLTE_ERROR) {
     srslte_ue_sync_free(q);
   }
-  return ret; 
+  return ret;
 }
 
 uint32_t srslte_ue_sync_sf_len(srslte_ue_sync_t *q) {
@@ -238,7 +238,7 @@ void srslte_ue_sync_free(srslte_ue_sync_t *q) {
   }
   if (!q->file_mode) {
     srslte_sync_free(&q->sfind);
-    srslte_sync_free(&q->strack);    
+    srslte_sync_free(&q->strack);
   } else {
     srslte_filesource_free(&q->file_source);
   }
@@ -380,6 +380,8 @@ void srslte_ue_sync_set_cfo(srslte_ue_sync_t *q, float cfo) {
   srslte_sync_set_cfo(&q->strack, cfo/15000);
 }
 
+void srslte_ue_sync_set_cfo_tol(srslte_ue_sync_t *q, float cfo_tol) {}
+
 float srslte_ue_sync_get_sfo(srslte_ue_sync_t *q) {
   return q->mean_sfo/5e-3;
 }
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 82f0eec91..7032df215 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -38,6 +38,8 @@
 
 #define MAX_SFLEN     SRSLTE_SF_LEN(srslte_symbol_sz(max_prb))
 
+#define DEFAULT_CFO_TOL   50.0 // Hz
+
 int srslte_ue_ul_init(srslte_ue_ul_t *q,
                       uint32_t max_prb)
 {
@@ -62,8 +64,8 @@ int srslte_ue_ul_init(srslte_ue_ul_t *q,
       fprintf(stderr, "Error creating CFO object\n");
       goto clean_exit;
     }
-    
-    srslte_cfo_set_tol(&q->cfo, 0);
+
+    srslte_ue_ul_set_cfo_tol(q, DEFAULT_CFO_TOL);
 
     if (srslte_pusch_init_ue(&q->pusch, max_prb)) {
       fprintf(stderr, "Error creating PUSCH object\n");
@@ -155,7 +157,9 @@ int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
         fprintf(stderr, "Error resizing CFO object\n");
         return SRSLTE_ERROR;
       }
-      srslte_cfo_set_tol(&q->cfo, 50.0/(15000.0*srslte_symbol_sz(q->cell.nof_prb)));
+
+      srslte_ue_ul_set_cfo_tol(q, q->current_cfo_tol);
+
       if (srslte_pusch_set_cell(&q->pusch, q->cell)) {
         fprintf(stderr, "Error resizing PUSCH object\n");
         return SRSLTE_ERROR;
@@ -178,6 +182,10 @@ int srslte_ue_ul_set_cell(srslte_ue_ul_t *q,
   return ret;
 }
 
+void srslte_ue_ul_set_cfo_tol(srslte_ue_ul_t *q, float tol) {
+  q->current_cfo_tol = tol;
+  srslte_cfo_set_tol(&q->cfo, tol/(15000.0*srslte_symbol_sz(q->cell.nof_prb)));
+}
 
 void srslte_ue_ul_set_cfo(srslte_ue_ul_t *q, float cur_cfo) {
   q->current_cfo = cur_cfo; 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 0e9932224..17dc3e1ea 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -545,11 +545,12 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
+        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, cfo=%.1f Hzn_iter=%d%s\n",
              grant->nof_prb, harq_pid,
              grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
              rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
              10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+             cfo*15000,
              srslte_pdsch_last_noi(&ue_dl.pdsch),
              timestr);
 

From 20cdc85ebd3aaf34c0f37e3d584461e185dc371f Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 12:13:14 +0200
Subject: [PATCH 168/269] fixed scheduler for constant mcs

---
 srsenb/src/mac/scheduler_ue.cc | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 9a258a1a3..b7e40e1bf 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -397,7 +397,7 @@ int sched_ue::generate_format1(dl_harq_proc *h,
     if (fixed_mcs_dl < 0) {
       tbs = alloc_tbs_dl(nof_prb, nof_re, req_bytes, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), nof_prb);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), nof_prb)/8;
       mcs = fixed_mcs_dl; 
     }
 
@@ -468,7 +468,7 @@ int sched_ue::generate_format0(ul_harq_proc *h,
     if (fixed_mcs_ul < 0) {
       tbs = alloc_tbs_ul(allocation.L, nof_re, req_bytes, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), allocation.L);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), allocation.L)/8;
       mcs = fixed_mcs_ul;
     }
     
@@ -607,12 +607,12 @@ uint32_t sched_ue::get_required_prb_dl(uint32_t req_bytes, uint32_t nof_ctrl_sym
   
   uint32_t nof_re = 0; 
   int tbs = 0; 
-  for (n=1;n<cell.nof_prb && nbytes < req_bytes;n++) {
+  for (n=1;n<=cell.nof_prb && nbytes < req_bytes;n++) {
     nof_re = srslte_ra_dl_approx_nof_re(cell, n, nof_ctrl_symbols);
     if (fixed_mcs_dl < 0) {
       tbs = alloc_tbs_dl(n, nof_re, 0, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), n);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_dl), n)/8;
     }
     if (tbs > 0) {
       nbytes = tbs; 
@@ -635,13 +635,13 @@ uint32_t sched_ue::get_required_prb_ul(uint32_t req_bytes)
     return 0; 
   }
   
-  for (n=1;n<cell.nof_prb && nbytes < req_bytes + 4;n++) {
+  for (n=1;n<=cell.nof_prb && nbytes < req_bytes + 4;n++) {
     uint32_t nof_re = (2*(SRSLTE_CP_NSYMB(cell.cp)-1) - N_srs)*n*SRSLTE_NRE;
     int tbs = 0; 
     if (fixed_mcs_ul < 0) {
       tbs = alloc_tbs_ul(n, nof_re, 0, &mcs);
     } else {
-      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), n);
+      tbs = srslte_ra_tbs_from_idx(srslte_ra_tbs_idx_from_mcs(fixed_mcs_ul), n)/8;
     }
     if (tbs > 0) {
       nbytes = tbs; 

From 9a2c8d9b89054e3c74c3d5f78b460f841307cbd4 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 13:19:04 +0200
Subject: [PATCH 169/269] fixed error in decreased performance due to
 oscilating CFO

---
 lib/src/phy/sync/sync.c | 2 --
 1 file changed, 2 deletions(-)

diff --git a/lib/src/phy/sync/sync.c b/lib/src/phy/sync/sync.c
index 2d7128e39..ee2ba76d1 100644
--- a/lib/src/phy/sync/sync.c
+++ b/lib/src/phy/sync/sync.c
@@ -617,8 +617,6 @@ srslte_sync_find_ret_t srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t
 }
 
 void srslte_sync_reset(srslte_sync_t *q) {
-  q->mean_cfo2_isunset = true;
-  q->mean_cfo_isunset  = true;
   q->M_ext_avg  = 0;
   q->M_norm_avg = 0; 
   srslte_pss_synch_reset(&q->pss);

From a4ab30a0c553e3e865f6c26614275b1e92c3ea9f Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 13:39:06 +0200
Subject: [PATCH 170/269] Use empirical SNR-to-CQI mapping table

---
 lib/src/phy/phch/cqi.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/phch/cqi.c b/lib/src/phy/phch/cqi.c
index 0041c3fcb..675f32897 100644
--- a/lib/src/phy/phch/cqi.c
+++ b/lib/src/phy/phch/cqi.c
@@ -272,10 +272,10 @@ float srslte_cqi_to_coderate(uint32_t cqi) {
  * Table III. 
 */
 // From paper
-static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 19.9, 21.5, 23.45, 25.0, 27.30, 29};
+//static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 19.9, 21.5, 23.45, 25.0, 27.30, 29};
 
 // From experimental measurements @ 5 MHz 
-//static float cqi_to_snr_table[15] = { 1, 1.75, 3, 4, 5, 6, 7.5, 9, 11.5, 13.0, 15.0, 18, 20, 22.5, 26.5};
+static float cqi_to_snr_table[15] = { 1, 1.75, 3, 4, 5, 6, 7.5, 9, 11.5, 13.0, 15.0, 18, 20, 22.5, 26.5};
 
 uint8_t srslte_cqi_from_snr(float snr)
 {

From 17b97c50cfcf3e1d9d4f068fb2d6ee9cfa3e280a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 12 Sep 2017 13:42:27 +0200
Subject: [PATCH 171/269] fixed CQI reduction due to UCI in UL only

---
 srsenb/src/mac/scheduler_ue.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index b7e40e1bf..b9a3093c2 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -814,7 +814,7 @@ int sched_ue::alloc_tbs(uint32_t nof_prb,
   uint32_t max_Qm  = is_ul?4:6; // Allow 16-QAM in PUSCH Only
 
   // TODO: Compute real spectral efficiency based on PUSCH-UCI configuration
-  if (has_pucch) {
+  if (has_pucch && is_ul) {
     cqi-=2;
   }
 

From 8eab3b46880a981e9120b346f12a71c7dd2ee6ea Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 17:50:20 +0200
Subject: [PATCH 172/269] Removed redundant condition

---
 srsue/hdr/mac/dl_harq.h | 26 ++++++++++++--------------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index ba5888a30..d79c230f5 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -301,20 +301,18 @@ private:
               harq_entity->pcap->write_dl_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid], cur_grant.rnti, ack,
                                                 cur_grant.tti);
             }
-            if (ack) {
-              if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
-                      cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
-              } else {
-                Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
-                harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
-                                                  cur_grant.tti);
-
-                // Compute average number of retransmissions per packet
-                harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
-                                                           harq_entity->nof_pkts++);
-              }
+            if (cur_grant.rnti_type == SRSLTE_RNTI_TEMP) {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit (Temporal C-RNTI)\n",
+                    cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu_temp_crnti(payload_buffer_ptr, cur_grant.n_bytes[tid]);
+            } else {
+              Debug("Delivering PDU=%d bytes to Dissassemble and Demux unit\n", cur_grant.n_bytes[tid]);
+              harq_entity->demux_unit->push_pdu(pid * SRSLTE_MAX_TB + tid, payload_buffer_ptr, cur_grant.n_bytes[tid],
+                                                cur_grant.tti);
+
+              // Compute average number of retransmissions per packet
+              harq_entity->average_retx = SRSLTE_VEC_CMA((float) n_retx, harq_entity->average_retx,
+                                                         harq_entity->nof_pkts++);
             }
           }
         } else {

From 1ac7f325dfe0d7c1707634a97fc65e46ea5a8cd1 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 17:51:56 +0200
Subject: [PATCH 173/269] Solved bug for plotting srsue contellation and
 removed trace of tb_cw_swap

---
 srsue/src/phy/phch_worker.cc | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 0e9932224..95c72a1fa 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -429,7 +429,10 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     grant->rnti = dl_rnti;
     grant->rnti_type = type;
     grant->last_tti = 0;
-    
+    grant->tb_en[0] = dci_unpacked.tb_en[0];
+    grant->tb_en[1] = dci_unpacked.tb_en[1];
+    grant->tb_cw_swap = dci_unpacked.tb_cw_swap; // FIXME: tb_cw_swap not supported
+
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
     char hexstr[16];
@@ -1064,7 +1067,7 @@ int phch_worker::read_ce_abs(float *ce_abs) {
   bzero(ce_abs, sizeof(float)*sz);
   int g = (sz - 12*cell.nof_prb)/2;
   for (i = 0; i < 12*cell.nof_prb; i++) {
-    ce_abs[g+i] = 20 * log10(cabs(ue_dl.ce[0][i]));
+    ce_abs[g+i] = 20 * log10f(cabsf(ue_dl.ce_m[0][0][i]));
     if (isinf(ce_abs[g+i])) {
       ce_abs[g+i] = -80;
     }

From 4b00908abb9bd65484cdc44aa9de74cf8a734852 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 6 Sep 2017 14:45:34 +0200
Subject: [PATCH 174/269] Removed bug in dl_harq for multiple TB

---
 srsue/hdr/mac/dl_harq.h | 20 ++++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index d79c230f5..e897ac76f 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -166,8 +166,17 @@ private:
     }
 
     void new_grant_dl(Tgrant grant, Taction *action) {
-      for (uint32_t tb = 0; tb < grant.phy_grant.dl.nof_tb; tb++) {
-        subproc[tb].new_grant_dl(grant, action);
+      /* Fill action structure */
+      bzero(action, sizeof(Taction));
+      action->default_ack = false;
+      action->generate_ack = true;
+      action->decode_enabled = false;
+
+      /* For each subprocess... */
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          subproc[tb].new_grant_dl(grant, action);
+        }
       }
     }
 
@@ -241,12 +250,6 @@ private:
         grant.last_tti = cur_grant.tti;
         memcpy(&cur_grant, &grant, sizeof(Tgrant));
 
-        // Fill action structure
-        bzero(action, sizeof(Taction));
-        action->default_ack = ack;
-        action->generate_ack = true;
-        action->decode_enabled = false;
-
         // If data has not yet been successfully decoded
         if (!ack) {
 
@@ -268,6 +271,7 @@ private:
 
         } else {
           Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
+          action->phy_grant.dl.tb_en[tid] = false;
         }
 
         if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {

From 230eb63a97d5f0238c9a399e17ce18c520baf5b9 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 5 Sep 2017 13:17:33 +0200
Subject: [PATCH 175/269] Refactored grant: removed nof_tb from grant. Use
 tb_en instead.

---
 lib/examples/pdsch_enodeb.c                   | 18 +++--
 lib/examples/pdsch_ue.c                       |  9 ++-
 lib/include/srslte/interfaces/ue_interfaces.h |  2 +
 lib/include/srslte/phy/phch/ra.h              |  4 +-
 lib/src/phy/phch/pdsch.c                      | 74 ++++++++++-------
 lib/src/phy/phch/ra.c                         | 29 ++++---
 lib/src/phy/phch/sch.c                        |  4 +-
 lib/src/phy/phch/test/pdsch_test.c            | 39 +++++----
 lib/src/phy/ue/ue_dl.c                        | 61 +++++++-------
 srsue/hdr/phy/phch_worker.h                   |  2 +-
 srsue/src/phy/phch_worker.cc                  | 80 +++++++++++--------
 11 files changed, 184 insertions(+), 138 deletions(-)

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 3be278180..d481ff20f 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -793,9 +793,11 @@ int main(int argc, char **argv) {
         }
       } else {
         INFO("SF: %d, Generating %d random bits\n", sf_idx, pdsch_cfg.grant.mcs[0].tbs + pdsch_cfg.grant.mcs[1].tbs);
-        for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-          for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
-            data[tb][i] = (uint8_t) rand();
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+          if (pdsch_cfg.grant.tb_en[tb]) {
+            for (i = 0; i < pdsch_cfg.grant.mcs[tb].tbs / 8; i++) {
+              data[tb][i] = (uint8_t) rand();
+            }
           }
         }
         /* Uncomment this to transmit on sf 0 and 5 only  */
@@ -851,10 +853,12 @@ int main(int argc, char **argv) {
         }        
         if (net_port > 0 && net_packet_ready) {
           if (null_file_sink) {
-            for (uint32_t tb = 0; tb < pdsch_cfg.grant.nof_tb; tb++) {
-              srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
-              if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
-                fprintf(stderr, "Error sending data through UDP socket\n");
+            for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+              if (pdsch_cfg.grant.tb_en[tb]) {
+                srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
+                if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
+                  fprintf(stderr, "Error sending data through UDP socket\n");
+                }
               }
             }
           }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index cf7bed2e4..88d9a2bf9 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -660,9 +660,10 @@ int main(int argc, char **argv) {
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
                 // FIXME: UDP Data transmission does not work
-                for (uint32_t tb = 0; tb < ue_dl.pdsch_cfg.grant.nof_tb; tb++) {
-                  srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
-
+                for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+                  if (ue_dl.pdsch_cfg.grant.tb_en[tb]) {
+                    srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+                  }
                 }
               }
               
@@ -723,7 +724,7 @@ int main(int argc, char **argv) {
 
             /* Print basic Parameters */
             PRINT_LINE("   nof layers: %d", ue_dl.pdsch_cfg.nof_layers);
-            PRINT_LINE("nof codewords: %d", ue_dl.pdsch_cfg.grant.nof_tb);
+            PRINT_LINE("nof codewords: %d", SRSLTE_RA_DL_GRANT_NOF_TB(&ue_dl.pdsch_cfg.grant));
             PRINT_LINE("          CFO: %+5.2f kHz", srslte_ue_sync_get_cfo(&ue_sync) / 1000);
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
             PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index dcf477283..cf8a7c861 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -284,6 +284,8 @@ public:
     bool        last_ndi[SRSLTE_MAX_CODEWORDS];
     uint32_t    n_bytes[SRSLTE_MAX_CODEWORDS];
     int         rv[SRSLTE_MAX_CODEWORDS];
+    bool        tb_en[SRSLTE_MAX_CODEWORDS];
+    bool        tb_cw_swap;
     uint16_t    rnti; 
     bool        is_from_rar;
     bool        is_sps_release;
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 9f31a0218..94cc9a084 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -104,10 +104,12 @@ typedef struct SRSLTE_API {
   uint32_t nof_prb;  
   uint32_t Qm[SRSLTE_MAX_CODEWORDS];
   srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
-  uint32_t nof_tb;
+  bool tb_en[SRSLTE_MAX_CODEWORDS];
   uint32_t pinfo;
 } srslte_ra_dl_grant_t;
 
+#define SRSLTE_RA_DL_GRANT_NOF_TB(G) ((((G)->tb_en[0])?1:0)+(((G)->tb_en[1])?1:0))
+
 /** Unpacked DCI message for DL grant */
 typedef struct SRSLTE_API {
   
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index c33a0d1d3..455880ff1 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -463,15 +463,17 @@ int srslte_pdsch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_g
 int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi,
                            uint32_t sf_idx, int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type,
                            uint32_t pmi) {
-  if (cfg) {
-    if (grant) {
-      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
-    }
+  if (cfg && grant) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
+    memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
 
-    for (int i = 0; i < grant->nof_tb; i++) {
-      if (srslte_cbsegm(&cfg->cb_segm[i], (uint32_t) cfg->grant.mcs[i].tbs)) {
-        fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[i].tbs);
-        return SRSLTE_ERROR;
+
+    for (int cw = 0; cw < SRSLTE_MAX_CODEWORDS; cw++) {
+      if (grant->tb_en[cw]) {
+        if (srslte_cbsegm(&cfg->cb_segm[cw], (uint32_t) cfg->grant.mcs[cw].tbs)) {
+          fprintf(stderr, "Error computing Codeblock (1) segmentation for TBS=%d\n", cfg->grant.mcs[cw].tbs);
+          return SRSLTE_ERROR;
+        }
       }
     }
     srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
@@ -483,33 +485,36 @@ int srslte_pdsch_cfg_mimo(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra
     /* Check and configure PDSCH transmission modes */
     switch(mimo_type) {
       case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for single transmission mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for single antenna.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 1;
         break;
       case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-        if (grant->nof_tb != 1) {
-          ERROR("Number of transport blocks is not supported for transmit diversity mode.");
+        if (nof_tb != 1) {
+          ERROR("Wrong number of transport blocks (%d) for transmit diversity.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
         break;
       case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-        if (grant->nof_tb == 1) {
+        if (nof_tb == 1) {
           cfg->codebook_idx = pmi;
           cfg->nof_layers = 1;
-        } else {
+        } else if (nof_tb == 2) {
           cfg->codebook_idx = pmi + 1;
           cfg->nof_layers = 2;
+        } else {
+          ERROR("Wrong number of transport blocks (%d) for spatial multiplexing.", nof_tb);
+          return SRSLTE_ERROR;
         }
         INFO("PDSCH configured for Spatial Multiplex; nof_codewords=%d; nof_layers=%d; codebook_idx=%d;\n",
-             grant->nof_tb, cfg->nof_layers, cfg->codebook_idx);
+             nof_tb, cfg->nof_layers, cfg->codebook_idx);
         break;
       case SRSLTE_MIMO_TYPE_CDD:
-        if (grant->nof_tb != 2) {
-          ERROR("Number of transport blocks (%d) is not supported for CDD transmission mode.", grant->nof_tb);
+        if (nof_tb != 2) {
+          ERROR("Wrong number of transport blocks (%d) for CDD.", nof_tb);
           return SRSLTE_ERROR;
         }
         cfg->nof_layers = 2;
@@ -622,9 +627,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
       data         != NULL &&
       cfg          != NULL)
   {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
 
-    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, nof_layers=%d, nof_tb=%d\n",
-        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, cfg->grant.nof_tb);
+    INFO("Decoding PDSCH SF: %d, RNTI: 0x%x, NofSymbols: %d, C_prb=%d, mimo_type=%d, nof_layers=%d, nof_tb=%d\n",
+        cfg->sf_idx, rnti, cfg->nbits[0].nof_re, cfg->grant.nof_prb, cfg->nof_layers, nof_tb);
 
     // Extract Symbols and Channel Estimates
     for (int j=0;j<q->nof_rx_antennas;j++) {
@@ -645,10 +651,10 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
 
     // Prepare layers
     int nof_symbols [SRSLTE_MAX_CODEWORDS];
-    nof_symbols[0] = cfg->nbits[0].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
-    nof_symbols[1] = cfg->nbits[1].nof_re * cfg->grant.nof_tb / cfg->nof_layers;
+    nof_symbols[0] = cfg->nbits[0].nof_re * nof_tb / cfg->nof_layers;
+    nof_symbols[1] = cfg->nbits[1].nof_re * nof_tb / cfg->nof_layers;
 
-    if (cfg->nof_layers == cfg->grant.nof_tb) {
+    if (cfg->nof_layers == nof_tb) {
       /* Skip layer demap */
       for (i = 0; i < cfg->nof_layers; i++) {
         x[i] = q->d[i];
@@ -666,15 +672,17 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
                                     cfg->codebook_idx, cfg->nbits[0].nof_re, cfg->mimo_type, noise_estimate);
 
     // Layer demapping only if necessary
-    if (cfg->nof_layers != cfg->grant.nof_tb) {
-        srslte_layerdemap_type(x, q->d, cfg->nof_layers, cfg->grant.nof_tb,
+    if (cfg->nof_layers != nof_tb) {
+        srslte_layerdemap_type(x, q->d, cfg->nof_layers, nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
 
     // Codeword decoding
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
-      acks[tb] = (ret == SRSLTE_SUCCESS);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+        acks[tb] = (ret == SRSLTE_SUCCESS);
+      }
     }
 
     pdsch_decode_debug(q, cfg, sf_symbols, ce);
@@ -725,6 +733,8 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
 
   if (q != NULL &&
       cfg != NULL) {
+    uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant);
+
 
     for (i = 0; i < q->cell.nof_ports; i++) {
       if (sf_symbols[i] == NULL) {
@@ -744,15 +754,17 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
       return SRSLTE_ERROR_INVALID_INPUTS;
     }
 
-    for (uint32_t tb = 0; tb < cfg->grant.nof_tb; tb ++) {
-      ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+    for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
+      if (cfg->grant.tb_en[tb]) {
+        ret |= srslte_pdsch_codeword_encode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
+      }
     }
 
     // Layer mapping & precode if necessary
     if (q->cell.nof_ports > 1) {
       int nof_symbols;
       /* If number of layers is equal to transport blocks (codewords) skip layer mapping */
-      if (cfg->nof_layers == cfg->grant.nof_tb) {
+      if (cfg->nof_layers == nof_tb) {
         for (i = 0; i < cfg->nof_layers; i++) {
           x[i] = q->d[i];
         }
@@ -764,7 +776,7 @@ int srslte_pdsch_encode(srslte_pdsch_t *q,
         }
         memset(&x[cfg->nof_layers], 0, sizeof(cf_t *) * (SRSLTE_MAX_LAYERS - cfg->nof_layers));
 
-        nof_symbols = srslte_layermap_type(q->d, x, cfg->grant.nof_tb, cfg->nof_layers,
+        nof_symbols = srslte_layermap_type(q->d, x, nof_tb, cfg->nof_layers,
                                            (int[SRSLTE_MAX_CODEWORDS]) {cfg->nbits[0].nof_re, cfg->nbits[1].nof_re},
                                            cfg->mimo_type);
       }
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 00713e6c1..b85d2ea2f 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -521,11 +521,10 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
       grant->mcs[1].tbs = 0;
     }
   }
-  grant->nof_tb = 0;
   for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    grant->tb_en[tb] = dci->tb_en[tb];
     if (dci->tb_en[tb]) {
       grant->Qm[tb] = srslte_mod_bits_x_symbol(grant->mcs[tb].mod);
-      grant->nof_tb++;
     }
   }
   grant->pinfo = dci->pinfo;
@@ -541,12 +540,14 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
                                  srslte_ra_nbits_t nbits [SRSLTE_MAX_CODEWORDS])
 {
   // Compute number of RE 
-  for (int i = 0; i < grant->nof_tb; i++) {
-    /* Compute number of RE for first transport block */
-    nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
-    nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
-    nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
-    nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      /* Compute number of RE for first transport block */
+      nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
+      nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
+      nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+      nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
+    }
   }
 }
 
@@ -820,11 +821,13 @@ void srslte_ra_pdsch_fprint(FILE *f, srslte_ra_dl_dci_t *dci, uint32_t nof_prb)
 void srslte_ra_dl_grant_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
   srslte_ra_prb_fprint(f, grant);
   fprintf(f, " - Number of PRBs:\t\t\t%d\n", grant->nof_prb);
-  fprintf(f, " - Number of TBs:\t\t\t%d\n", grant->nof_tb);
-  for (int i = 0; i < grant->nof_tb; i++) {
-    fprintf(f, "  - Transport block:\t\t\t%d\n", i);
-    fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
-    fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+  fprintf(f, " - Number of TBs:\t\t\t%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant->tb_en[i]) {
+      fprintf(f, "  - Transport block:\t\t\t%d\n", i);
+      fprintf(f, "   -> Modulation type:\t\t\t%s\n", srslte_mod_string(grant->mcs[i].mod));
+      fprintf(f, "   -> Transport block size:\t\t%d\n", grant->mcs[i].tbs);
+    }
   }
 }
 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index e0002429e..72ee50ee9 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -524,7 +524,7 @@ int srslte_dlsch_decode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                          int16_t *e_bits, uint8_t *data, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
@@ -553,7 +553,7 @@ int srslte_dlsch_encode2(srslte_sch_t *q, srslte_pdsch_cfg_t *cfg, srslte_softbu
                               uint8_t *data, uint8_t *e_bits, int codeword_idx) {
   uint32_t Nl = 1;
 
-  if (cfg->nof_layers != cfg->grant.nof_tb) {
+  if (cfg->nof_layers != SRSLTE_RA_DL_GRANT_NOF_TB(&cfg->grant)) {
     Nl = 2;
   }
 
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index e820f677a..c5b55b48d 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -255,8 +255,8 @@ int main(int argc, char **argv) {
   }
 
 
-  for (int i = 0; i < grant.nof_tb; i++) {
-    if (grant.mcs[i].tbs) {
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant.tb_en[i]) {
       data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
       if (!data_tx[i]) {
         perror("srslte_vec_malloc");
@@ -305,12 +305,11 @@ int main(int argc, char **argv) {
   INFO("              cp=%s\n", srslte_cp_string(cell.cp));
   INFO("    phich_length=%d\n", (int) cell.phich_length);
   INFO(" phich_resources=%d\n", (int) cell.phich_resources);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
   INFO("         nof_prb=%d\n", pdsch_cfg.grant.nof_prb);
   INFO("          sf_idx=%d\n", pdsch_cfg.sf_idx);
   INFO("       mimo_type=%s\n", srslte_mimotype2str(pdsch_cfg.mimo_type));
   INFO("      nof_layers=%d\n", pdsch_cfg.nof_layers);
-  INFO("          nof_tb=%d\n", pdsch_cfg.grant.nof_tb);
+  INFO("          nof_tb=%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(&pdsch_cfg.grant));
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     INFO(" Tranport block index %d:\n", i);
     INFO("              Qm=%d\n", pdsch_cfg.grant.Qm[i]);
@@ -382,9 +381,11 @@ int main(int argc, char **argv) {
       }
     }
 
-    for (int tb = 0; tb < grant.nof_tb; tb++) {
-      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-        data_tx[tb][byte] = (uint8_t)(rand() % 256);
+    for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+      if (grant.tb_en[tb]) {
+        for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+          data_tx[tb][byte] = (uint8_t) (rand() % 256);
+        }
       }
     }
 
@@ -454,8 +455,8 @@ int main(int argc, char **argv) {
       srslte_ofdm_rx_sf(&ofdm_rx, tx_sf_symbols[i], rx_slot_symbols[i]);
     }
 #endif
-    for (i = 0; i < grant.nof_tb; i++) {
-      if (grant.mcs[i].tbs) {
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+      if (grant.tb_en[i]) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
@@ -474,19 +475,23 @@ int main(int argc, char **argv) {
   }
 
   /* Check Tx and Rx bytes */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
-      if (data_tx[tb][byte] != data_rx[tb][byte]) {
-        ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
-        ret = SRSLTE_ERROR;
-        goto quit;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        if (data_tx[tb][byte] != data_rx[tb][byte]) {
+          ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+          ret = SRSLTE_ERROR;
+          goto quit;
+        }
       }
     }
   }
 
   /* Check all transport blocks have been decoded OK */
-  for (int tb = 0; tb < grant.nof_tb; tb++) {
-    ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      ret |= (acks[tb]) ? SRSLTE_SUCCESS : SRSLTE_ERROR;
+    }
   }
 
   ret = SRSLTE_SUCCESS;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 95e670642..963b2fc55 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -337,21 +337,22 @@ int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *c
 int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx,
                                  int rvidx[SRSLTE_MAX_CODEWORDS], srslte_mimo_type_t mimo_type) {
   uint32_t pmi = 0;
+  uint32_t nof_tb = SRSLTE_RA_DL_GRANT_NOF_TB(grant);
 
   /* Translates Precoding Information (pinfo) to Precoding matrix Index (pmi) as 3GPP 36.212 Table 5.3.3.1.5-4 */
   if (mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-    if (grant->nof_tb == 1) {
+    if (nof_tb == 1) {
       if (grant->pinfo > 0 && grant->pinfo < 5) {
         pmi = grant->pinfo - 1;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     } else {
       if (grant->pinfo < 2) {
         pmi = grant->pinfo;
       } else {
-        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", q->pdsch_cfg.grant.nof_tb, grant->pinfo);
+        ERROR("Not Implemented (nof_tb=%d, pinfo=%d)", nof_tb, grant->pinfo);
         return SRSLTE_ERROR;
       }
     }
@@ -393,29 +394,33 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
     
     /* ===== These lines of code are supposed to be MAC functionality === */
 
-    
+
     int rvidx[SRSLTE_MAX_CODEWORDS] = {1};
     if (dci_unpacked.rv_idx < 0) {
-      uint32_t sfn = tti/10; 
-      uint32_t k   = (sfn/2)%4;
-      for (int i = 0; i < grant.nof_tb; i++) {
-        rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+      uint32_t sfn = tti / 10;
+      uint32_t k = (sfn / 2) % 4;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          rvidx[i] = ((uint32_t) ceilf((float) 1.5 * k)) % 4;
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
+        }
       }
     } else {
-      for (int i = 0; i < grant.nof_tb; i++) {
-        switch(i) {
-          case 0:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
-            break;
-          case 1:
-            rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
-            break;
-          default:
-            ERROR("Wrong number of transport blocks");
-            return SRSLTE_ERROR;
+      for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+        if (grant.tb_en[i]) {
+          switch (i) {
+            case 0:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx;
+              break;
+            case 1:
+              rvidx[i] = (uint32_t) dci_unpacked.rv_idx_1;
+              break;
+            default:
+              ERROR("Wrong number of transport blocks");
+              return SRSLTE_ERROR;
+          }
+          srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
         }
-        srslte_softbuffer_rx_reset_tbs(q->softbuffers[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
 
@@ -429,14 +434,14 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
         }
         break;
       case SRSLTE_DCI_FORMAT2:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
         }
         break;
       case SRSLTE_DCI_FORMAT2A:
-        if (grant.nof_tb == 1 && dci_unpacked.pinfo == 0) {
+        if (SRSLTE_RA_DL_GRANT_NOF_TB(&grant) == 1 && dci_unpacked.pinfo == 0) {
           mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
         } else {
           mimo_type = SRSLTE_MIMO_TYPE_CDD;
@@ -470,11 +475,13 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
                                 noise_estimate,
                                 rnti, data, acks);
 
-      for (int tb = 0; tb < q->pdsch_cfg.grant.nof_tb; tb++) {
-        if (!acks[tb]) {
-          q->pkt_errors++;
+      for (int tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (grant.tb_en[tb]) {
+          if (!acks[tb]) {
+            q->pkt_errors++;
+          }
+          q->pkts_total++;
         }
-        q->pkts_total++;
       }
 
       if (ret == SRSLTE_ERROR) {
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index d81c4c2f6..e6fce4f02 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -97,7 +97,7 @@ private:
   void set_uci_sr();
   void set_uci_periodic_cqi();
   void set_uci_aperiodic_cqi();
-  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb);
+  void set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS]);
   bool srs_is_ready_to_send();
   float set_power(float tx_power);
   void setup_tx_gain();
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 95c72a1fa..8b8f13857 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -242,15 +242,17 @@ void phch_worker::work_imp()
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
 
         // NOTE: Currently hard-coded to 1st TB only
-        for (uint32_t tb = 0; tb < 1; tb++) {
-          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
-          dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
-          Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+        for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+          if (dl_mac_grant.tb_en[tb]) {
+            phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+            dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
+            Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
+          }
         }
       }
       Debug("dl_ack={%d, %d}, generate_ack=%d\n", dl_ack[0], dl_ack[1], dl_action.generate_ack);
       if (dl_action.generate_ack) {
-        set_uci_ack(dl_ack, dl_mac_grant.phy_grant.dl.nof_tb);
+        set_uci_ack(dl_ack, dl_mac_grant.tb_en);
       }
     }
   }
@@ -316,8 +318,10 @@ void phch_worker::work_imp()
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
       phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
-      for (uint32_t tb = 0; tb < dl_mac_grant.phy_grant.dl.nof_tb; tb++) {
-        phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+      for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        if (dl_mac_grant.tb_en[tb]) {
+          phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
+        }
       }
     }
   }
@@ -433,6 +437,11 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
     grant->tb_en[1] = dci_unpacked.tb_en[1];
     grant->tb_cw_swap = dci_unpacked.tb_cw_swap; // FIXME: tb_cw_swap not supported
 
+    if (grant->tb_cw_swap) {
+      Info("tb_cw_swap = true\n");
+      printf("tb_cw_swap = true\n");
+    }
+
     last_dl_pdcch_ncce = srslte_ue_dl_get_ncce(&ue_dl);
 
     char hexstr[16];
@@ -459,8 +468,8 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
   srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
   int ret = SRSLTE_SUCCESS;
 
-  for (uint32_t tb = 0; tb < grant->nof_tb; tb++) {
-    if (rv[tb] < 0 || rv[tb] > 3) {
+  for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant->tb_en[tb] && (rv[tb] < 0 || rv[tb] > 3)) {
       valid_config = false;
       Error("Wrong RV (%d) for TB index %d", rv[tb], tb);
     }
@@ -479,22 +488,22 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM3\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM3\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
-      if (grant->nof_tb == 1) {
+      if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (grant->nof_tb == 2) {
+      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM4\n", grant->nof_tb);
+        Error("Wrong number of transport blocks (%d) for TM4\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
     break;
@@ -548,13 +557,14 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info("PDSCH: l_crb=%2d, harq=%d, nof_tb=%d, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
-             grant->nof_prb, harq_pid,
-             grant->nof_tb, grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx, grant->mcs[1].idx,
-             rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
-             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
-             srslte_pdsch_last_noi(&ue_dl.pdsch),
-             timestr);
+        Info(
+            "PDSCH: l_crb=%2d, harq=%d, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
+            grant->nof_prb, harq_pid, grant->tb_en[0] ? "on" : "off", grant->tb_en[1] ? "on" : "off",
+            grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
+            grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
+            10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
+            srslte_pdsch_last_noi(&ue_dl.pdsch),
+            timestr);
 
         // Store metrics
         dl_metrics.mcs    = grant->mcs[0].idx;
@@ -684,22 +694,22 @@ void phch_worker::reset_uci()
   bzero(&uci_data, sizeof(srslte_uci_data_t));
 }
 
-  void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], uint32_t nof_tb) {
-    if (nof_tb > 0) {
-      uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    }
-
-    if (nof_tb > 1) {
-      uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    }
+void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_MAX_CODEWORDS])
+{
+  uint32_t nof_tb = 0;
+  if (tb_en[0]) {
+    uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
+    nof_tb++;
+  }
 
-    if (nof_tb > 2) {
-      Error("Number of transport blocks is not supported");
-    }
+  if (tb_en[1]) {
+    uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
+    nof_tb++;
+  }
 
-    uci_data.uci_ack_len = nof_tb;
+  uci_data.uci_ack_len = nof_tb;
 
-  }
+}
 
 void phch_worker::set_uci_sr()
 {

From 709787b581544c70b218048f79c56450ed002203 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 6 Sep 2017 15:25:12 +0200
Subject: [PATCH 176/269] Improved robustness of PDSCH decoder

---
 lib/src/phy/phch/pdsch.c     | 26 ++++++++++++++++++++------
 srsue/src/phy/phch_worker.cc | 16 +++++++++-------
 2 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 455880ff1..0908793db 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -581,12 +581,13 @@ static int srslte_pdsch_codeword_encode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
 
 static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *cfg,
                                                srslte_softbuffer_rx_t *softbuffer, uint16_t rnti, uint8_t *data,
-                                               uint32_t codeword_idx) {
+                                               uint32_t codeword_idx, bool *ack) {
   srslte_ra_nbits_t *nbits = &cfg->nbits[codeword_idx];
   srslte_ra_mcs_t *mcs = &cfg->grant.mcs[codeword_idx];
   uint32_t rv = cfg->rv[codeword_idx];
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
 
-  if (nbits->nof_bits) {
+  if (softbuffer && data && ack) {
     INFO("Decoding PDSCH SF: %d (TB %d), Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
          cfg->sf_idx, codeword_idx, srslte_mod_string(mcs->mod), mcs->tbs,
          nbits->nof_re, nbits->nof_bits, rv);
@@ -603,10 +604,19 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
     /* Bit scrambling */
     srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
 
-    return srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
+    /* Return  */
+    ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
+    if (ret == SRSLTE_SUCCESS) {
+      *ack = true;
+    } else if (ret == SRSLTE_ERROR) {
+      *ack = false;
+      ret = SRSLTE_SUCCESS;
+    }
+  } else {
+    ERROR("Detected NULL pointer");
   }
 
-  return SRSLTE_SUCCESS;
+  return ret;
 }
 
 /** Decodes the PDSCH from the received symbols
@@ -680,8 +690,12 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
     // Codeword decoding
     for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
       if (cfg->grant.tb_en[tb]) {
-        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb);
-        acks[tb] = (ret == SRSLTE_SUCCESS);
+        int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb, &acks[tb]);
+
+        /* Check if there has been any execution error */
+        if (ret) {
+          return ret;
+        }
       }
     }
 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 8b8f13857..e43c6e3f2 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -240,8 +240,6 @@ void phch_worker::work_imp()
                       dl_mac_grant.pid, dl_ack);
       }
       if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
-
-        // NOTE: Currently hard-coded to 1st TB only
         for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
           if (dl_mac_grant.tb_en[tb]) {
             phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
@@ -549,7 +547,7 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         ret = srslte_pdsch_decode(&ue_dl.pdsch, &ue_dl.pdsch_cfg, softbuffers, ue_dl.sf_symbols_m,
                                   ue_dl.ce_m, noise_estimate, rnti, payload, acks);
         if (ret) {
-          Error("Decoding PDSCH");
+          Error("ERROR: Decoding PDSCH\n");
         }
   #ifdef LOG_EXECTIME
         gettimeofday(&t[2], NULL);
@@ -699,12 +697,14 @@ void phch_worker::set_uci_ack(bool ack[SRSLTE_MAX_CODEWORDS], bool tb_en[SRSLTE_
   uint32_t nof_tb = 0;
   if (tb_en[0]) {
     uci_data.uci_ack = (uint8_t) ((ack[0]) ? 1 : 0);
-    nof_tb++;
+    nof_tb = 1;
+  } else {
+    uci_data.uci_ack = 1;
   }
 
   if (tb_en[1]) {
     uci_data.uci_ack_2 = (uint8_t) ((ack[1]) ? 1 : 0);
-    nof_tb++;
+    nof_tb = 2;
   }
 
   uci_data.uci_ack_len = nof_tb;
@@ -893,10 +893,12 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s, sr=%s, cfo=%.1f Hz%s\n", 
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
-       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",uci_data.scheduling_request?"yes":"no", 
+       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+       uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+       uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   
   

From 91fe0322762220a8381b8bb111438d928f7a3325 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 7 Sep 2017 17:46:30 +0200
Subject: [PATCH 177/269] Added PUSCH ACK decoder and test

---
 lib/include/srslte/phy/phch/uci.h  |  3 +-
 lib/src/phy/phch/sch.c             |  5 ++-
 lib/src/phy/phch/test/pusch_test.c | 15 ++++++--
 lib/src/phy/phch/uci.c             | 56 +++++++++++++++++++++---------
 4 files changed, 57 insertions(+), 22 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index 01eae8e89..23ed3962c 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -132,7 +132,8 @@ SRSLTE_API int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg,
                                      uint32_t H_prime_total, 
                                      uint32_t O_cqi,
                                      srslte_uci_bit_t *ack_bits,
-                                     uint8_t *data); 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks);
 
 SRSLTE_API int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
                                     uint8_t data, 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 72ee50ee9..02ec906ff 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -657,14 +657,17 @@ int srslte_ulsch_uci_decode_ri_ack(srslte_sch_t *q, srslte_pusch_cfg_t *cfg, srs
   
   // Deinterleave and decode HARQ bits
   if (uci_data->uci_ack_len > 0) {
+    uint8_t acks[2] = {0, 0};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, &uci_data->uci_ack);
+    ret = srslte_uci_decode_ack(cfg, q_bits, c_seq, beta, nb_q/Qm, uci_data->uci_cqi_len, q->ack_ri_bits, acks, uci_data->uci_ack_len);
     if (ret < 0) {
       return ret; 
     }
+    uci_data->uci_ack = acks[0];
+    uci_data->uci_ack_2 = acks[1];
     Q_prime_ack = (uint32_t) ret; 
 
     // Set zeros to HARQ bits
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index 991764bc0..ece098dcf 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -190,14 +190,15 @@ int main(int argc, char **argv) {
   bzero(&uci_data_tx, sizeof(srslte_uci_data_t));
   uci_data_tx.uci_cqi_len = 4; 
   uci_data_tx.uci_ri_len = 0; 
-  uci_data_tx.uci_ack_len = 0; 
+  uci_data_tx.uci_ack_len = 2;
   memcpy(&uci_data_rx, &uci_data_tx, sizeof(srslte_uci_data_t));
     
   for (uint32_t i=0;i<20;i++) {
     uci_data_tx.uci_cqi [i] = 1;
   }
-  uci_data_tx.uci_ri = 1; 
-  uci_data_tx.uci_ack = 1; 
+  uci_data_tx.uci_ri = 1;
+  uci_data_tx.uci_ack = 1;
+  uci_data_tx.uci_ack_2 = 1;
 
   uint32_t nof_re = SRSLTE_NRE*cell.nof_prb*2*SRSLTE_CP_NSYMB(cell.cp);
   sf_symbols = srslte_vec_malloc(sizeof(cf_t) * nof_re);
@@ -268,11 +269,19 @@ int main(int argc, char **argv) {
   if (uci_data_tx.uci_ack_len) {
     if (uci_data_tx.uci_ack != uci_data_rx.uci_ack) {
       printf("UCI ACK bit error: %d != %d\n", uci_data_tx.uci_ack, uci_data_rx.uci_ack);
+      ret = SRSLTE_ERROR;
+    }
+  }
+  if (uci_data_tx.uci_ack_len > 1) {
+    if (uci_data_tx.uci_ack_2 != uci_data_rx.uci_ack_2) {
+      printf("UCI ACK 2 bit error: %d != %d\n", uci_data_tx.uci_ack_2, uci_data_rx.uci_ack_2);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_ri_len) {
     if (uci_data_tx.uci_ri != uci_data_rx.uci_ri) {
       printf("UCI RI bit error: %d != %d\n", uci_data_tx.uci_ri, uci_data_rx.uci_ri);
+      ret = SRSLTE_ERROR;
     }
   }
   if (uci_data_tx.uci_cqi_len) {
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 3b22eb792..0e75b91bf 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -560,15 +560,35 @@ static void encode_ri_ack(uint8_t data, srslte_uci_bit_type_t q_encoded_bits[6],
   }
 }
                        
-static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos) 
+static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
 {
-  uint32_t p0 = pos[0].position;
-  uint32_t p1 = pos[1].position;
-  
-  uint32_t q0 = c_seq[p0]?q_bits[p0]:-q_bits[p0];  
-  uint32_t q1 = c_seq[p0]?q_bits[p1]:-q_bits[p1];
+  if (nof_data == 1) {
+    uint32_t p0 = pos[0].position;
+    uint32_t p1 = pos[1].position;
+
+    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+    int32_t q1 = c_seq[p0] ? q_bits[p1] : -q_bits[p1];
 
-  return -(q0+q1);
+    data[0] -= q0 + q1;
+  } else {
+    uint32_t p0 = pos[Qm * 0 + 0].position;
+    uint32_t p1 = pos[Qm * 0 + 1].position;
+    uint32_t p2 = pos[Qm * 1 + 6].position;
+    uint32_t p3 = pos[Qm * 1 + 7].position;
+    uint32_t p4 = pos[Qm * 2 + 12].position;
+    uint32_t p5 = pos[Qm * 2 + 13].position;
+
+    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+    int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
+    int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
+    int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
+    int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
+    int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
+
+    data[0] -= q0 + q3;
+    data[1] -= q1 + q4;
+    data[2] -= q2 + q5;
+  }
 }
 
 
@@ -577,25 +597,27 @@ static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *
  */
 int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_seq, 
                           float beta, uint32_t H_prime_total, 
-                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t *data)
+                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t acks[2], uint32_t nof_acks)
 {
-  int32_t rx_ack = 0; 
-  
+  int32_t acks_sum[3] = {0, 0, 0};
+
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
 
   // Use the same interleaver function to get the HARQ bit position
-  for (uint32_t i=0;i<Qprime;i++) {
+  for (uint32_t i = 0; i < Qprime; i += (nof_acks > 1) ? 3 : 1) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    rx_ack += (int32_t) decode_ri_ack(q_bits, c_seq, ack_bits);
+    decode_ri_ack(q_bits, c_seq, ack_bits, cfg->grant.Qm, acks_sum, nof_acks);
   }
-  
-  if (data) {
-    *data = rx_ack>0;
+
+  if (acks) {
+    acks[0] = (uint8_t)(acks_sum[0] > 0);
+    acks[1] = (uint8_t)(acks_sum[1] > 0);
+    // TODO: Do something with acks_sum[2]
   }
   return (int) Qprime;  
 }
@@ -644,7 +666,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   // Use the same interleaver function to get the HARQ bit position
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    rx_ri += (int32_t) decode_ri_ack(q_bits, c_seq, ri_bits);
+    decode_ri_ack(q_bits, c_seq, ri_bits, cfg->grant.Qm, &rx_ri, 1);
   }
 
   if (data) {

From d9b6ae7563b842b4658e8a2621b299c66f66bf9d Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 7 Sep 2017 16:30:15 +0200
Subject: [PATCH 178/269] PUSCH supports 2 ACK

---
 lib/include/srslte/phy/phch/uci.h |  5 +--
 lib/src/phy/phch/sch.c            |  4 ++-
 lib/src/phy/phch/uci.c            | 54 ++++++++++++++++++++++---------
 3 files changed, 45 insertions(+), 18 deletions(-)

diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index 23ed3962c..fa1360876 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -119,8 +119,9 @@ SRSLTE_API int srslte_uci_decode_cqi_pusch(srslte_uci_cqi_pusch_t *q,
                                            bool *cqi_ack); 
 
 SRSLTE_API int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg,
-                                     uint8_t data, 
-                                     uint32_t O_cqi, 
+                                     uint8_t acks[2],
+                                     uint32_t nof_acks,
+                                     uint32_t O_cqi,
                                      float beta, 
                                      uint32_t H_prime_total, 
                                      srslte_uci_bit_t *ri_bits); 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 02ec906ff..a44ced2d8 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -808,11 +808,13 @@ int srslte_ulsch_uci_encode(srslte_sch_t *q,
   
   // Encode (and interleave) ACK
   if (uci_data.uci_ack_len > 0) {
+    uint8_t acks [2] = {uci_data.uci_ack, uci_data.uci_ack_2};
     float beta = beta_harq_offset[cfg->uci_cfg.I_offset_ack]; 
     if (cfg->cb_segm.tbs == 0) {
         beta /= beta_cqi_offset[cfg->uci_cfg.I_offset_cqi];
     }
-    ret = srslte_uci_encode_ack(cfg, uci_data.uci_ack, uci_data.uci_cqi_len, beta, nb_q/Qm, &q->ack_ri_bits[Q_prime_ri*Qm]);
+    ret = srslte_uci_encode_ack(cfg, acks, uci_data.uci_ack_len, uci_data.uci_cqi_len,
+                                beta, nb_q / Qm, &q->ack_ri_bits[Q_prime_ri * Qm]);
     if (ret < 0) {
       return ret; 
     }
diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 0e75b91bf..530d72467 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -551,13 +551,35 @@ static uint32_t Q_prime_ri_ack(srslte_pusch_cfg_t *cfg,
   return Q_prime; 
 }
 
-static void encode_ri_ack(uint8_t data, srslte_uci_bit_type_t q_encoded_bits[6], uint8_t Qm) 
+static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit_type_t q_encoded_bits[18], uint8_t Qm)
 {
-  q_encoded_bits[0] = data?UCI_BIT_1:UCI_BIT_0;
-  q_encoded_bits[1] = UCI_BIT_REPETITION; 
-  for (uint32_t i=2;i<Qm;i++) {
-    q_encoded_bits[i] = UCI_BIT_PLACEHOLDER;
+  uint32_t i = 0;
+
+  if (data_len == 1) {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = UCI_BIT_REPETITION;
+    while(i < Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+  } else {
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = data[0] ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*2) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
+    q_encoded_bits[i++] = data[1] ? UCI_BIT_1 : UCI_BIT_0;
+    q_encoded_bits[i++] = (data[0]^data[1]) ? UCI_BIT_1 : UCI_BIT_0;
+    while(i<Qm*3) {
+      q_encoded_bits[i++] = UCI_BIT_PLACEHOLDER;
+    }
   }
+
+  return i;
 }
                        
 static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
@@ -625,23 +647,23 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
 /* Encode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
  *  Currently only supporting 1-bit HARQ
  */
-int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t data, 
+int srslte_uci_encode_ack(srslte_pusch_cfg_t *cfg, uint8_t acks[2], uint32_t nof_acks,
                           uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                           srslte_uci_bit_t *ack_bits)
-{  
+{
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
 
-  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(acks, nof_acks, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ack_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;
@@ -681,22 +703,24 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
  *  Currently only supporting 1-bit RI
  */
 int srslte_uci_encode_ri(srslte_pusch_cfg_t *cfg,
-                         uint8_t data, 
+                         uint8_t ri,
                          uint32_t O_cqi, float beta, uint32_t H_prime_total, 
                          srslte_uci_bit_t *ri_bits)
 {
+  // FIXME: It supports RI of 1 bit only
+  uint8_t data[2] = {ri, 0};
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
     return -1; 
   }
   uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
-  srslte_uci_bit_type_t q_encoded_bits[6];
+  srslte_uci_bit_type_t q_encoded_bits[18];
 
-  encode_ri_ack(data, q_encoded_bits, cfg->grant.Qm);
+  uint32_t nof_encoded_bits = encode_ri_ack(data, 1, q_encoded_bits, cfg->grant.Qm);
   
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    uci_ulsch_interleave_put(q_encoded_bits, cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
+    uci_ulsch_interleave_put(&q_encoded_bits[(i*cfg->grant.Qm)%nof_encoded_bits], cfg->grant.Qm, &ri_bits[cfg->grant.Qm*i]);
   }
   
   return (int) Qprime;

From 8fd8175320cb7712558bc7d6ce4857171f3435f0 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 10:41:52 +0200
Subject: [PATCH 179/269] Improved ACK and RI in PUSCH decoding

---
 lib/src/phy/phch/uci.c | 60 +++++++++++++++++++-----------------------
 1 file changed, 27 insertions(+), 33 deletions(-)

diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index 530d72467..ed253d2cc 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -582,35 +582,25 @@ static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit
   return i;
 }
                        
-static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[2], uint32_t nof_data)
+static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[3])
 {
-  if (nof_data == 1) {
-    uint32_t p0 = pos[0].position;
-    uint32_t p1 = pos[1].position;
-
-    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
-    int32_t q1 = c_seq[p0] ? q_bits[p1] : -q_bits[p1];
-
-    data[0] -= q0 + q1;
-  } else {
-    uint32_t p0 = pos[Qm * 0 + 0].position;
-    uint32_t p1 = pos[Qm * 0 + 1].position;
-    uint32_t p2 = pos[Qm * 1 + 6].position;
-    uint32_t p3 = pos[Qm * 1 + 7].position;
-    uint32_t p4 = pos[Qm * 2 + 12].position;
-    uint32_t p5 = pos[Qm * 2 + 13].position;
-
-    int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
-    int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
-    int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
-    int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
-    int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
-    int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
-
-    data[0] -= q0 + q3;
-    data[1] -= q1 + q4;
-    data[2] -= q2 + q5;
-  }
+  uint32_t p0 = pos[Qm * 0 + 0].position;
+  uint32_t p1 = pos[Qm * 0 + 1].position;
+  uint32_t p2 = pos[Qm * 1 + 0].position;
+  uint32_t p3 = pos[Qm * 1 + 1].position;
+  uint32_t p4 = pos[Qm * 2 + 0].position;
+  uint32_t p5 = pos[Qm * 2 + 1].position;
+
+  int32_t q0 = c_seq[p0] ? q_bits[p0] : -q_bits[p0];
+  int32_t q1 = c_seq[p1] ? q_bits[p1] : -q_bits[p1];
+  int32_t q2 = c_seq[p2] ? q_bits[p2] : -q_bits[p2];
+  int32_t q3 = c_seq[p3] ? q_bits[p3] : -q_bits[p3];
+  int32_t q4 = c_seq[p4] ? q_bits[p4] : -q_bits[p4];
+  int32_t q5 = c_seq[p5] ? q_bits[p5] : -q_bits[p5];
+
+  data[0] -= q0 + q3;
+  data[1] -= q1 + q4;
+  data[2] -= q2 + q5;
 }
 
 
@@ -631,9 +621,11 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
   uint32_t Qprime = Q_prime_ri_ack(cfg, nof_acks, O_cqi, beta);
 
   // Use the same interleaver function to get the HARQ bit position
-  for (uint32_t i = 0; i < Qprime; i += (nof_acks > 1) ? 3 : 1) {
+  for (uint32_t i = 0; i < Qprime; i++) {
     uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
-    decode_ri_ack(q_bits, c_seq, ack_bits, cfg->grant.Qm, acks_sum, nof_acks);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ack_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, acks_sum);
+    }
   }
 
   if (acks) {
@@ -676,7 +668,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
                           float beta, uint32_t H_prime_total, 
                           uint32_t O_cqi, srslte_uci_bit_t *ri_bits, uint8_t *data)
 {
-  int32_t rx_ri = 0; 
+  int32_t ri_sum[3] = {0, 0, 0};
   
   if (beta < 0) {
     fprintf(stderr, "Error beta is reserved\n");
@@ -688,11 +680,13 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   // Use the same interleaver function to get the HARQ bit position
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
-    decode_ri_ack(q_bits, c_seq, ri_bits, cfg->grant.Qm, &rx_ri, 1);
+    if ((i % 3 == 0) && i > 0) {
+      decode_ri_ack(q_bits, &c_seq[0], &ri_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, ri_sum);
+    }
   }
 
   if (data) {
-    *data = rx_ri>0;
+    *data = (uint8_t) ((ri_sum[0] + ri_sum[1] + ri_sum[2]) > 0);
   }
 
   return (int) Qprime;  

From 65f5987ea8682d8b749cc821e002c05bd02283ec Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 11:36:43 +0200
Subject: [PATCH 180/269] Added RI reporting for TM3 and TM4 (no PMI)

---
 lib/include/srslte/phy/ue/ue_dl.h |  2 +-
 lib/src/phy/ue/ue_dl.c            |  4 ++--
 srsue/src/phy/phch_worker.cc      | 17 +++++++++++++----
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index c4e799424..657dc5ab4 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -190,7 +190,7 @@ SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           float *current_sinr);
 
 SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
-                                      uint32_t *ri,
+                                      uint8_t *ri,
                                       float *cn);
 
 SRSLTE_API bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, 
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 963b2fc55..647c5e37b 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -573,7 +573,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
 
 /* Compute the Rank Indicator (RI) by computing the condition number, valid for TM3 */
-int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
+int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint8_t *ri, float *cn) {
   float _cn;
   int ret = srslte_pdsch_cn_compute(&q->pdsch, q->ce_m, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), &_cn);
 
@@ -584,7 +584,7 @@ int srslte_ue_dl_ri_select(srslte_ue_dl_t *q, uint32_t *ri, float *cn) {
 
   /* Set rank indicator */
   if (!ret && ri) {
-    *ri = (_cn > 3.0f)? 1:0;
+    *ri = (uint8_t)((_cn < 17.0f)? 1:0);
   }
 
   return ret;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index e43c6e3f2..8d3e2c2c1 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -252,6 +252,14 @@ void phch_worker::work_imp()
       if (dl_action.generate_ack) {
         set_uci_ack(dl_ack, dl_mac_grant.tb_en);
       }
+
+      /* Select Rank Indicator by computing Condition Number */
+      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3 ||
+          phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4) {
+        float cn = 0.0f;
+        srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
+        uci_data.uci_ri_len = 1;
+      }
     }
   }
   
@@ -556,9 +564,9 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
   #endif
 
         Info(
-            "PDSCH: l_crb=%2d, harq=%d, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d%s\n",
-            grant->nof_prb, harq_pid, grant->tb_en[0] ? "on" : "off", grant->tb_en[1] ? "on" : "off",
-            grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
+            "PDSCH: l_crb=%2d, harq=%d, scheme=%s, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d, %s\n",
+            grant->nof_prb, harq_pid, srslte_mimotype2str(mimo_type), grant->tb_en[0] ? "on" : "off",
+            grant->tb_en[1] ? "on" : "off", grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
             grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
             10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
             srslte_pdsch_last_noi(&ue_dl.pdsch),
@@ -847,11 +855,12 @@ void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, ui
   snprintf(timestr, 64, ", total_time=%4d us", (int) logtime_start[0].tv_usec);
 #endif
 
-  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, cfo=%.1f Hz%s\n", 
+  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, ri=%s, cfo=%.1f Hz%s\n",
          (tti+4)%10240,
          grant->L_prb, grant->n_prb[0], 
          grant->mcs.tbs/8, grant->mcs.mod, grant->mcs.idx, rv,
          uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+         uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
          cfo*15000, timestr);
 
   // Store metrics

From afd2bbbfc22a70ed02c2d7eea8468f8d0bacb102 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 8 Sep 2017 16:24:34 +0200
Subject: [PATCH 181/269] Added periodic RI on PUCCH for TM3 and TM4

---
 lib/examples/pdsch_ue.c           |  2 +-
 lib/include/srslte/phy/phch/cqi.h |  3 +++
 lib/include/srslte/phy/phch/uci.h |  4 +++
 lib/include/srslte/phy/ue/ue_dl.h |  4 +--
 lib/src/phy/ue/ue_dl.c            | 10 ++++----
 lib/src/phy/ue/ue_ul.c            |  8 ++++++
 srsue/src/phy/phch_worker.cc      | 41 ++++++++++++++++++++++++++-----
 7 files changed, 58 insertions(+), 14 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 88d9a2bf9..d48aafd2b 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -537,7 +537,7 @@ int main(int argc, char **argv) {
     
   // Variables for measurements 
   uint32_t nframes=0;
-  uint32_t ri = 0, pmi = 0;
+  uint8_t ri = 0, pmi = 0;
   float rsrp0=0.0, rsrp1=0.0, rsrq=0.0, noise=0.0, enodebrate = 0.0, uerate = 0.0,
       sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS], cn = 0.0;
   bool decode_pdsch = false; 
diff --git a/lib/include/srslte/phy/phch/cqi.h b/lib/include/srslte/phy/phch/cqi.h
index 593592f1c..cfc9e92e7 100644
--- a/lib/include/srslte/phy/phch/cqi.h
+++ b/lib/include/srslte/phy/phch/cqi.h
@@ -41,11 +41,14 @@
 #include "srslte/phy/common/phy_common.h"
 
 #define SRSLTE_CQI_MAX_BITS 64
+#define SRSLTE_DIF_CQI_MAX_BITS 3
+#define SRSLTE_PMI_MAX_BITS 4
 
 typedef struct {
   bool     configured; 
   uint32_t pmi_idx; 
   uint32_t ri_idx;
+  bool ri_idx_present;
   bool     simul_cqi_ack;
   bool     format_is_subband; 
   uint32_t subband_size; 
diff --git a/lib/include/srslte/phy/phch/uci.h b/lib/include/srslte/phy/phch/uci.h
index fa1360876..ff315384e 100644
--- a/lib/include/srslte/phy/phch/uci.h
+++ b/lib/include/srslte/phy/phch/uci.h
@@ -64,6 +64,10 @@ typedef struct SRSLTE_API {
 typedef struct SRSLTE_API {
   uint8_t  uci_cqi[SRSLTE_CQI_MAX_BITS];
   uint32_t uci_cqi_len;
+  uint8_t  uci_dif_cqi[SRSLTE_DIF_CQI_MAX_BITS];
+  uint32_t uci_dif_cqi_len;
+  uint8_t  uci_pmi[SRSLTE_PMI_MAX_BITS];
+  uint8_t  uci_pmi_len;
   uint8_t  uci_ri;  // Only 1-bit supported for RI
   uint32_t uci_ri_len;
   uint8_t  uci_ack;   // 1st codeword bit for HARQ-ACK
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index 657dc5ab4..d88148e9e 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -185,8 +185,8 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
                                         bool acks[SRSLTE_MAX_CODEWORDS]);
 
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
-                                          uint32_t *ri,
-                                          uint32_t *pmi,
+                                          uint8_t *ri,
+                                          uint8_t *pmi,
                                           float *current_sinr);
 
 SRSLTE_API int srslte_ue_dl_ri_select(srslte_ue_dl_t *q,
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 647c5e37b..46dfea566 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -511,10 +511,10 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
 
 /* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
  * Noise Ratio (SINR), valid for TM4 */
-int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, float *current_sinr) {
+int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, float *current_sinr) {
   float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
   float best_sinr = -INFINITY;
-  uint32_t best_pmi = 0, best_ri = 0;
+  uint8_t best_pmi = 0, best_ri = 0;
 
   if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
     /* Do nothing */
@@ -528,11 +528,11 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint32_t *ri, uint32_t *pmi, f
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
     for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
-      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers;
+      float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers * nof_layers;
       if (_sinr > best_sinr + 0.1) {
         best_sinr = _sinr;
-        best_pmi = q->pmi[nof_layers - 1];
-        best_ri = nof_layers;
+        best_pmi = (uint8_t) q->pmi[nof_layers - 1];
+        best_ri = (uint8_t) (nof_layers - 1);
       }
     }
 
diff --git a/lib/src/phy/ue/ue_ul.c b/lib/src/phy/ue/ue_ul.c
index 82f0eec91..6d669b5c3 100644
--- a/lib/src/phy/ue/ue_ul.c
+++ b/lib/src/phy/ue/ue_ul.c
@@ -269,6 +269,14 @@ void pucch_encode_bits(srslte_uci_data_t *uci_data, srslte_pucch_format_t format
     pucch_bits[1] = uci_data->uci_ack_2; // this will be ignored in format 1a 
   }
   if (format >= SRSLTE_PUCCH_FORMAT_2) {
+    /* Append Differential CQI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_dif_cqi, uci_data->uci_dif_cqi_len);
+    uci_data->uci_cqi_len += uci_data->uci_dif_cqi_len;
+
+    /* Append PMI */
+    memcpy(&uci_data->uci_cqi[uci_data->uci_cqi_len], uci_data->uci_pmi, uci_data->uci_pmi_len);
+    uci_data->uci_cqi_len += uci_data->uci_pmi_len;
+
     srslte_uci_encode_cqi_pucch(uci_data->uci_cqi, uci_data->uci_cqi_len, pucch_bits);
     if (format > SRSLTE_PUCCH_FORMAT_2) {
       pucch2_bits[0] = uci_data->uci_ack; 
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 8d3e2c2c1..0290c0745 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -254,11 +254,23 @@ void phch_worker::work_imp()
       }
 
       /* Select Rank Indicator by computing Condition Number */
-      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3 ||
-          phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4) {
+      if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3) {
         float cn = 0.0f;
         srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
         uci_data.uci_ri_len = 1;
+      } else if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4){
+        float sinr = 0.0f;
+        uint8 packed_pmi = 0;
+        srslte_ue_dl_ri_pmi_select(&ue_dl, &uci_data.uci_ri, &packed_pmi, &sinr);
+        srslte_bit_unpack_vector(&packed_pmi, uci_data.uci_pmi, 2);
+        uci_data.uci_ri_len = 1;
+        if (uci_data.uci_ri == 0) {
+          uci_data.uci_pmi_len = 2;
+          uci_data.uci_dif_cqi_len = 0;
+        } else {
+          uci_data.uci_pmi_len = 1;
+          uci_data.uci_dif_cqi_len = 3;
+        }
       }
     }
   }
@@ -740,7 +752,16 @@ void phch_worker::set_uci_periodic_cqi()
   int cqi_max       = phy->args->cqi_max;
   
   if (period_cqi.configured && rnti_is_set) {
-    if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
+    if (period_cqi.ri_idx_present && srslte_ri_send(period_cqi.pmi_idx, period_cqi.ri_idx, (tti+4)%10240)) {
+      if (uci_data.uci_ri_len) {
+        uci_data.uci_cqi[0] = uci_data.uci_ri;
+        uci_data.uci_cqi_len = uci_data.uci_ri_len;
+        uci_data.uci_ri_len = 0;
+        uci_data.uci_dif_cqi_len = 0;
+        uci_data.uci_pmi_len = 0;
+        Info("PUCCH: Periodic RI=%d\n", uci_data.uci_cqi[0]);
+      }
+    } else if (srslte_cqi_send(period_cqi.pmi_idx, (tti+4)%10240)) {
       srslte_cqi_value_t cqi_report;
       if (period_cqi.format_is_subband) {
         // TODO: Implement subband periodic reports
@@ -902,11 +923,12 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, sr=%s, cfo=%.1f Hz%s\n",
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
        uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
        uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+       uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
        uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   
@@ -1037,12 +1059,19 @@ void phch_worker::set_ul_params(bool pregen_disabled)
   /* CQI configuration */
   bzero(&period_cqi, sizeof(srslte_cqi_periodic_cfg_t));
   period_cqi.configured        = dedicated->cqi_report_cnfg.report_periodic_setup_present;
-  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx; 
+  period_cqi.pmi_idx           = dedicated->cqi_report_cnfg.report_periodic.pmi_cnfg_idx;
   period_cqi.simul_cqi_ack     = dedicated->cqi_report_cnfg.report_periodic.simult_ack_nack_and_cqi;
   period_cqi.format_is_subband = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic ==
                                  LIBLTE_RRC_CQI_FORMAT_INDICATOR_PERIODIC_SUBBAND_CQI;
   period_cqi.subband_size      = dedicated->cqi_report_cnfg.report_periodic.format_ind_periodic_subband_k;
-  
+
+  if (dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx_present) {
+    period_cqi.ri_idx = dedicated->cqi_report_cnfg.report_periodic.ri_cnfg_idx;
+    period_cqi.ri_idx_present = true;
+  } else {
+    period_cqi.ri_idx_present = false;
+  }
+
   /* SR configuration */
   I_sr                         = dedicated->sched_request_cnfg.sr_cnfg_idx;
   

From 8a7e37641092653ca354e65d0a9e8feeccac393b Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 13 Sep 2017 14:53:47 +0200
Subject: [PATCH 182/269] fix CMake's UHD find script for newer UHD binaries

---
 cmake/modules/FindUHD.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/modules/FindUHD.cmake b/cmake/modules/FindUHD.cmake
index a5b8834cd..0c0eb742e 100644
--- a/cmake/modules/FindUHD.cmake
+++ b/cmake/modules/FindUHD.cmake
@@ -16,6 +16,7 @@ FIND_LIBRARY(
     HINTS $ENV{UHD_DIR}/lib
     PATHS /usr/local/lib
           /usr/lib
+          /usr/lib/x86_64-linux-gnu
           /usr/local/lib64
           /usr/local/lib32
 )

From 313e12e3e9abfcde40b9355e84465350192801c6 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 13 Sep 2017 17:21:23 +0200
Subject: [PATCH 183/269] restored CQI table

---
 lib/src/phy/phch/cqi.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/phch/cqi.c b/lib/src/phy/phch/cqi.c
index 675f32897..0041c3fcb 100644
--- a/lib/src/phy/phch/cqi.c
+++ b/lib/src/phy/phch/cqi.c
@@ -272,10 +272,10 @@ float srslte_cqi_to_coderate(uint32_t cqi) {
  * Table III. 
 */
 // From paper
-//static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 19.9, 21.5, 23.45, 25.0, 27.30, 29};
+static float cqi_to_snr_table[15] = { 1.95, 4, 6, 8, 10, 11.95, 14.05, 16, 17.9, 19.9, 21.5, 23.45, 25.0, 27.30, 29};
 
 // From experimental measurements @ 5 MHz 
-static float cqi_to_snr_table[15] = { 1, 1.75, 3, 4, 5, 6, 7.5, 9, 11.5, 13.0, 15.0, 18, 20, 22.5, 26.5};
+//static float cqi_to_snr_table[15] = { 1, 1.75, 3, 4, 5, 6, 7.5, 9, 11.5, 13.0, 15.0, 18, 20, 22.5, 26.5};
 
 uint8_t srslte_cqi_from_snr(float snr)
 {

From 120544baa002e66e2a675aa746e0969fdc5898c9 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 14 Sep 2017 10:09:52 +0200
Subject: [PATCH 184/269] Remove LV_HAVE_SSE from turbodecoder interface. Fixes
 #99

---
 lib/include/srslte/phy/fec/turbodecoder.h     | 16 +++++-----------
 .../srslte/phy/fec/turbodecoder_simd.h        | 19 ++-----------------
 2 files changed, 7 insertions(+), 28 deletions(-)

diff --git a/lib/include/srslte/phy/fec/turbodecoder.h b/lib/include/srslte/phy/fec/turbodecoder.h
index d42a20ae6..4a24804fa 100644
--- a/lib/include/srslte/phy/fec/turbodecoder.h
+++ b/lib/include/srslte/phy/fec/turbodecoder.h
@@ -50,20 +50,14 @@
 #define SRSLTE_TCOD_MAX_LEN_CODED  (SRSLTE_TCOD_RATE*SRSLTE_TCOD_MAX_LEN_CB+SRSLTE_TCOD_TOTALTAIL)
 
 #include "srslte/phy/fec/turbodecoder_gen.h"
-
-#ifdef LV_HAVE_SSE
 #include "srslte/phy/fec/turbodecoder_simd.h"
-#else
-#define SRSLTE_TDEC_NPAR 1
-#endif
 
 typedef struct SRSLTE_API {
-#ifdef LV_HAVE_SSE
-  srslte_tdec_simd_t tdec_simd;
-#else
-  float *input_conv; 
-  srslte_tdec_gen_t tdec_gen;
-#endif  
+  float *input_conv;
+  union {
+    srslte_tdec_simd_t tdec_simd;
+    srslte_tdec_gen_t  tdec_gen;
+  };
 } srslte_tdec_t;
 
 SRSLTE_API int srslte_tdec_init(srslte_tdec_t * h, 
diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd.h b/lib/include/srslte/phy/fec/turbodecoder_simd.h
index d7bc284d2..8cafbc5f8 100644
--- a/lib/include/srslte/phy/fec/turbodecoder_simd.h
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd.h
@@ -43,23 +43,8 @@
 #include "srslte/phy/fec/tc_interl.h"
 #include "srslte/phy/fec/cbsegm.h"
 
-//#define ENABLE_SIMD_INTER
-
-// The constant SRSLTE_TDEC_NPAR defines the maximum number of parallel CB supported by all SIMD decoders 
-#ifdef ENABLE_SIMD_INTER
-  #include "srslte/phy/fec/turbodecoder_simd_inter.h"
-  #ifdef LV_HAVE_AVX2
-    #define SRSLTE_TDEC_NPAR_INTRA 2
-  #else
-    #define SRSLTE_TDEC_NPAR_INTRA 1
-  #endif
-#else
-  #ifdef LV_HAVE_AVX2
-    #define SRSLTE_TDEC_NPAR 2
-  #else
-    #define SRSLTE_TDEC_NPAR 1
-  #endif
-#endif
+// Define maximum number of CB decoded in parallel (2 for AVX2)
+#define SRSLTE_TDEC_NPAR 2
 
 #define SRSLTE_TCOD_RATE 3
 #define SRSLTE_TCOD_TOTALTAIL 12

From a22d9aaaa3a2c13a4dad866a3946b6ca7dd8a7db Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 14 Sep 2017 13:00:27 +0200
Subject: [PATCH 185/269] Solved mat.h include in precoding.c

---
 lib/src/phy/mimo/precoding.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index f1aab3b5d..bf9047977 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -36,16 +36,15 @@
 
 #ifdef LV_HAVE_SSE
 #include <immintrin.h>
-#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 int srslte_predecoding_diversity2_sse(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], cf_t *x[SRSLTE_MAX_LAYERS], int nof_rxant, int nof_symbols);
 #endif
 
 #ifdef LV_HAVE_AVX
 #include <immintrin.h>
-#include "srslte/phy/utils/mat.h"
 int srslte_predecoding_single_avx(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_PORTS], cf_t *x, int nof_rxant, int nof_symbols, float noise_estimate);
 #endif
+#include "srslte/phy/utils/mat.h"
 
 
 static srslte_mimo_decoder_t mimo_decoder = SRSLTE_MIMO_DECODER_MMSE;

From 826667361e4f676c199615b097576dfd60c7a888 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 15 Sep 2017 17:49:31 +0200
Subject: [PATCH 186/269] Reduced chances of uhd demuxer error. Reset device
 when it occurs

---
 lib/include/srslte/interfaces/ue_interfaces.h |   4 +-
 lib/include/srslte/radio/radio.h              |   7 +-
 lib/src/phy/rf/rf_uhd_imp.c                   |   4 +-
 lib/src/phy/ue/ue_cell_search.c               |   2 +-
 lib/src/phy/ue/ue_mib.c                       |   2 +-
 lib/src/radio/radio.cc                        |  16 +-
 srsue/hdr/phy/phch_recv.h                     |  16 +-
 srsue/hdr/phy/phy.h                           |   5 +-
 srsue/src/phy/phch_recv.cc                    | 220 +++++++++++-------
 srsue/src/phy/phy.cc                          |  16 +-
 srsue/src/upper/rrc.cc                        |   4 +-
 11 files changed, 180 insertions(+), 116 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index cf8a7c861..1315b4c82 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -518,9 +518,9 @@ public:
 
   /* Cell search and selection procedures */
   virtual void cell_search_start() = 0;
+  virtual void cell_search_stop() = 0;
   virtual void cell_search_next() = 0;
   virtual bool cell_select(uint32_t earfcn, srslte_cell_t cell) = 0;
-  virtual bool sync_stop() = 0;
 
   /* Is the PHY downlink synchronized? */
   virtual bool sync_status() = 0;
@@ -529,8 +529,6 @@ public:
   virtual void configure_ul_params(bool pregen_disabled = false) = 0;
 
   virtual void reset() = 0;
-  
-  virtual void resync_sfn() = 0;   
 
 };
   
diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index 30d274f44..866bc4697 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -78,7 +78,8 @@ namespace srslte {
       };
       
       bool init(char *args = NULL, char *devname = NULL);
-      void stop(); 
+      void stop();
+      void reset();
       bool start_agc(bool tx_gain_same_rx);
       
       void set_burst_preamble(double preamble_us);
@@ -170,6 +171,10 @@ namespace srslte {
       bool agc_enabled;
       int offset;
       uint32_t sf_len;
+
+      char saved_args[128];
+      char saved_devname[128];
+
   }; 
 }
 
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 16fb2e07e..b4aa23da5 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -628,9 +628,11 @@ int rf_uhd_recv_with_time_multi(void *h,
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_LATE_COMMAND) {
         log_late(handler);
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_TIMEOUT) {
-        fprintf(stderr, "Error timed out while receiving asynchronoous messages from UHD.\n");
+        fprintf(stderr, "Error timed out while receiving samples from UHD.\n");
+        return -1;
       } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE ) {
         fprintf(stderr, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
+        return -1;
       }
       
     } while (n < nsamples && trials < 100);
diff --git a/lib/src/phy/ue/ue_cell_search.c b/lib/src/phy/ue/ue_cell_search.c
index 51b0858bb..dc35fb48c 100644
--- a/lib/src/phy/ue/ue_cell_search.c
+++ b/lib/src/phy/ue/ue_cell_search.c
@@ -297,7 +297,7 @@ int srslte_ue_cellsearch_scan_N_id_2(srslte_ue_cellsearch_t * q,
       ret = srslte_ue_sync_zerocopy_multi(&q->ue_sync, q->sf_buffer);
       if (ret < 0) {
         fprintf(stderr, "Error calling srslte_ue_sync_work()\n");       
-        break; 
+        return -1;
       } else if (ret == 1) {
         /* This means a peak was found and ue_sync is now in tracking state */
         ret = srslte_sync_get_cell_id(&q->ue_sync.strack);
diff --git a/lib/src/phy/ue/ue_mib.c b/lib/src/phy/ue/ue_mib.c
index 49ab30657..23fec7f6e 100644
--- a/lib/src/phy/ue/ue_mib.c
+++ b/lib/src/phy/ue/ue_mib.c
@@ -271,7 +271,7 @@ int srslte_ue_mib_sync_decode(srslte_ue_mib_sync_t * q,
       ret = srslte_ue_sync_zerocopy_multi(&q->ue_sync, q->sf_buffer);
       if (ret < 0) {
         fprintf(stderr, "Error calling srslte_ue_sync_work()\n");       
-        break; 
+        return -1;
       } else if (srslte_ue_sync_get_sfidx(&q->ue_sync) == 0) {
         if (ret == 1) {
           mib_ret = srslte_ue_mib_decode(&q->ue_mib, q->sf_buffer[0], bch_payload, nof_tx_ports, sfn_offset);                    
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index c3592f168..3aaa8cba1 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -30,6 +30,7 @@ extern "C" {
 }
 #include "srslte/radio/radio.h"
 #include <string.h>
+#include <unistd.h>
 
 namespace srslte {
 
@@ -60,7 +61,10 @@ bool radio::init(char *args, char *devname)
   } else {
     printf("\nWarning burst preamble is not calibrated for device %s. Set a value manually\n\n", srslte_rf_name(&rf_device));
   }
-  
+
+  strncpy(saved_args,    args, 128);
+  strncpy(saved_devname, devname, 128);
+
   return true;    
 }
 
@@ -69,6 +73,16 @@ void radio::stop()
   srslte_rf_close(&rf_device);
 }
 
+void radio::reset()
+{
+  printf("Resetting Radio...\n");
+  srslte_rf_close(&rf_device);
+  sleep(3);
+  if (srslte_rf_open_devname(&rf_device, saved_devname, saved_args)) {
+    fprintf(stderr, "Error opening RF device\n");
+  }
+}
+
 void radio::set_manual_calibration(rf_cal_t* calibration)
 {
   srslte_rf_cal_t tx_cal; 
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 784d99abc..82abfd304 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -52,13 +52,12 @@ public:
   void stop();
   void set_agc_enable(bool enable);
 
-  void     resync_sfn();
-
   void    set_earfcn(std::vector<uint32_t> earfcn);
 
   bool    stop_sync();
   void    cell_search_start();
-  void    cell_search_next();
+  void    cell_search_stop();
+  void    cell_search_next(bool reset = false);
   bool    cell_select(uint32_t earfcn, srslte_cell_t cell);
 
   uint32_t get_current_tti();
@@ -79,11 +78,16 @@ private:
 
   void   set_sampling_rate();
   bool   set_frequency();
+  void   resync_sfn();
 
   void   cell_search_inc();
 
-  bool init_cell();
-  void free_cell();
+  bool   init_cell();
+  void   free_cell();
+
+  void   stop_rx();
+  void   start_rx();
+  bool   radio_is_rx;
 
   bool   running; 
   
@@ -153,7 +157,7 @@ private:
 
   int    cell_sync_sfn();
   int    cell_meas_rsrp();
-  bool   cell_search(int force_N_id_2 = -1);
+  int    cell_search(int force_N_id_2 = -1);
   bool   set_cell();
 };
 
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index c8d88ee44..1d500e7f7 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -80,18 +80,17 @@ public:
   /********** RRC INTERFACE ********************/
   void    reset();
   void    configure_ul_params(bool pregen_disabled = false);
-  void    resync_sfn();
   void    cell_search_start();
+  void    cell_search_stop();
   void    cell_search_next();
   bool    cell_select(uint32_t earfcn, srslte_cell_t phy_cell);
 
   /********** MAC INTERFACE ********************/
   /* Functions to synchronize with a cell */
   bool    sync_status(); // this is also RRC interface
-  bool    sync_stop();
 
   /* Sets a C-RNTI allowing the PHY to pregenerate signals if necessary */
-  void set_crnti(uint16_t rnti);
+  void    set_crnti(uint16_t rnti);
   
   /* Instructs the PHY to configure using the parameters written by set_param() */
   void    configure_prach_params();
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 5b725d49f..bdfafb167 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -41,6 +41,7 @@ namespace srsue {
 
 int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
   srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
+
   if (radio_h->rx_now(data, nsamples, rx_time)) {
     int offset = nsamples - radio_h->get_tti_len();
     if (abs(offset) < 10 && offset != 0) {
@@ -226,7 +227,7 @@ bool phch_recv::set_cell() {
   return cell_is_set;
 }
 
-bool phch_recv::cell_search(int force_N_id_2) {
+int phch_recv::cell_search(int force_N_id_2) {
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
 
   srslte_ue_cellsearch_result_t found_cells[3];
@@ -238,7 +239,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
     srate_mode = SRATE_FIND;
     radio_h->set_rx_srate(1.92e6);
   }
-  radio_h->start_rx();
+  start_rx();
 
   /* Find a cell in the given N_id_2 or go through the 3 of them to find the strongest */
   uint32_t max_peak_cell = 0;
@@ -257,13 +258,12 @@ bool phch_recv::cell_search(int force_N_id_2) {
   last_gain = srslte_agc_get_gain(&cs.ue_sync.agc);
 
   if (ret < 0) {
-    radio_h->stop_rx();
     Error("SYNC:  Error decoding MIB: Error searching PSS\n");
-    return false;
+    return -1;
   } else if (ret == 0) {
-    radio_h->stop_rx();
+    stop_rx();
     Info("SYNC:  Could not find any cell in this frequency\n");
-    return false;
+    return 0;
   }
   // Save result
   cell.id = found_cells[max_peak_cell].cell_id;
@@ -294,7 +294,7 @@ bool phch_recv::cell_search(int force_N_id_2) {
   ret = srslte_ue_mib_sync_decode(&ue_mib_sync,
                                   40,
                                   bch_payload, &cell.nof_ports, &sfn_offset);
-  radio_h->stop_rx();
+  stop_rx();
   last_gain = srslte_agc_get_gain(&ue_mib_sync.ue_sync.agc);
   cellsearch_cfo = srslte_ue_sync_get_cfo(&ue_mib_sync.ue_sync);
 
@@ -311,9 +311,12 @@ bool phch_recv::cell_search(int force_N_id_2) {
          cell.id, cell.nof_prb, cell.nof_ports, cellsearch_cfo/1000);
 
     return true;
-  } else {
+  } else if (ret == 0) {
     Warning("SYNC:  Found PSS but could not decode PBCH\n");
-    return false;
+    return 0;
+  } else {
+    Error("SYNC:  Receiving MIB\n");
+    return -1;
   }
 }
 
@@ -391,8 +394,8 @@ int phch_recv::cell_meas_rsrp() {
 }
 
 void phch_recv::resync_sfn() {
-  radio_h->stop_rx();
-  radio_h->start_rx();
+  stop_rx();
+  start_rx();
   srslte_ue_mib_reset(&ue_mib);
   Info("SYNC:  Starting SFN synchronization\n");
   sync_sfn_cnt = 0;
@@ -417,24 +420,28 @@ bool phch_recv::stop_sync() {
 void phch_recv::cell_search_inc()
 {
   cur_earfcn_index++;
-  Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
   if (cur_earfcn_index >= 0) {
     if (cur_earfcn_index >= (int) earfcn.size() - 1) {
       cur_earfcn_index = 0;
     }
   }
+  usleep(100000);
+  Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
   if (current_earfcn != earfcn[cur_earfcn_index]) {
     current_earfcn = earfcn[cur_earfcn_index];
     set_frequency();
   }
 }
 
-void phch_recv::cell_search_next() {
-  if (cell_search_in_progress) {
+void phch_recv::cell_search_next(bool reset) {
+  if (cell_search_in_progress || reset) {
     cell_search_in_progress = false;
     if (!stop_sync()) {
       log_h->warning("SYNC:  Couldn't stop PHY\n");
     }
+    if (reset) {
+      cur_earfcn_index = -1;
+    }
     cell_search_inc();
     phy_state = CELL_SEARCH;
     cell_search_in_progress = true;
@@ -443,16 +450,22 @@ void phch_recv::cell_search_next() {
 
 void phch_recv::cell_search_start() {
   if (earfcn.size() > 0) {
-    cell_search_in_progress = true;
-    cur_earfcn_index = -1;
-    cell_search_next();
-    log_h->info("SYNC:  Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+    Info("SYNC:  Starting Cell Search procedure in %d EARFCNs...\n", earfcn.size());
+    cell_search_next(true);
   } else {
-    log_h->info("SYNC:  Empty EARFCN list. Stopping cell search...\n");
+    Info("SYNC:  Empty EARFCN list. Stopping cell search...\n");
     log_h->console("Empty EARFCN list. Stopping cell search...\n");
   }
 }
 
+void phch_recv::cell_search_stop() {
+  Info("SYNC:  Stopping Cell Search procedure...\n");
+  if (!stop_sync()) {
+    Error("SYNC:  Stopping cell search\n");
+  }
+  cell_search_in_progress = false;
+}
+
 bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
   // Check if we are already camping in this cell
@@ -476,6 +489,8 @@ bool phch_recv::cell_select(uint32_t earfcn, srslte_cell_t cell) {
 
     current_earfcn = earfcn;
 
+    printf("cell select called set frequency\n");
+
     if (set_frequency()) {
       this->cell = cell;
       log_h->info("Cell Select: Configuring cell...\n");
@@ -536,7 +551,6 @@ void phch_recv::set_sampling_rate()
 }
 
 void phch_recv::run_thread() {
-  int sync_res;
   phch_worker *worker = NULL;
   cf_t *buffer[SRSLTE_MAX_PORTS];
   phy_state  = IDLE;
@@ -549,20 +563,28 @@ void phch_recv::run_thread() {
     }
     switch (phy_state) {
       case CELL_SEARCH:
-        if (cell_search() && cell_search_in_progress) {
-          if (!srslte_cell_isvalid(&cell)) {
-            Error("SYNC:  Detected invalid cell\n");
-            phy_state = IDLE;
+        if (cell_search_in_progress) {
+          switch(cell_search()) {
+          case 1:
+            if (!srslte_cell_isvalid(&cell)) {
+              Error("SYNC:  Detected invalid cell\n");
+              phy_state = IDLE;
+              break;
+            }
+            if (set_cell()) {
+              set_sampling_rate();
+              resync_sfn();
+            }
             break;
-          }
-          if (set_cell()) {
-            set_sampling_rate();
-            resync_sfn();
-          }
-
-        } else {
-          if (cell_search_in_progress) {
-            cell_search_inc();
+          case 0:
+            if (cell_search_in_progress) {
+              cell_search_inc();
+            }
+            break;
+          default:
+            log_h->error("SYNC:  Receiving frorm radio.\n");
+            phy_state = IDLE;
+            radio_h->reset();
           }
         }
         break;
@@ -571,10 +593,6 @@ void phch_recv::run_thread() {
         srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
 
         switch (cell_sync_sfn()) {
-          default:
-            log_h->console("SYNC:  Going IDLE\n");
-            phy_state = IDLE;
-            break;
           case 1:
             srslte_ue_sync_set_agc_period(&ue_sync, 20);
             if (!cell_search_in_progress) {
@@ -588,6 +606,11 @@ void phch_recv::run_thread() {
             break;
           case 0:
             break;
+          default:
+            log_h->error("SYNC:  Receiving frorm radio.\n");
+            phy_state = IDLE;
+            radio_h->reset();
+            break;
         }
         sync_sfn_cnt++;
         if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
@@ -604,10 +627,13 @@ void phch_recv::run_thread() {
             rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
             break;
           case 0:
-            break;
-          default:
             log_h->error("SYNC:  Getting RSRP cell measurement.\n");
             cell_search_next();
+            break;
+          default:
+            log_h->error("SYNC:  Receiving frorm radio.\n");
+            phy_state = IDLE;
+            radio_h->reset();
         }
         break;
       case CELL_CAMP:
@@ -618,53 +644,57 @@ void phch_recv::run_thread() {
             buffer[i] = worker->get_buffer(i);
           }
 
-          sync_res = srslte_ue_sync_zerocopy_multi(&ue_sync, buffer);
-          if (sync_res == 1) {
-
-            log_h->step(tti);
-
-            Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
-
-            metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
-            metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
-            worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
-            worker_com->set_sync_metrics(metrics);
-
-            float sample_offset = (float) srslte_ue_sync_get_sfo(&ue_sync) / 1000;
-            worker->set_sample_offset(sample_offset);
-
-            /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
-            srslte_timestamp_t rx_time, tx_time, tx_time_prach;
-            srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
-            srslte_timestamp_copy(&tx_time, &rx_time);
-            srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
-            worker->set_tx_time(tx_time);
-
-            Debug("SYNC:  Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
-            worker->set_tti(tti, tx_mutex_cnt);
-            tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
-
-            // Check if we need to TX a PRACH
-            if (prach_buffer->is_ready_to_send(tti)) {
-              srslte_timestamp_copy(&tx_time_prach, &rx_time);
-              srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
-              prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
-              radio_h->tx_end();
-              worker_com->p0_preamble = prach_buffer->get_p0_preamble();
-              worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
-            }
-            workers_pool->start_worker(worker);
-            // Notify RRC in-sync every 1 frame
-            if ((tti % 10) == 0) {
-              rrc->in_sync();
-              log_h->debug("SYNC:  Sending in-sync to RRC\n");
-            }
-          } else {
-            log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
-            // Notify RRC of out-of-sync frame
-            rrc->out_of_sync();
-            worker->release();
-            worker_com->reset_ul();
+          switch(srslte_ue_sync_zerocopy_multi(&ue_sync, buffer)) {
+            case 1:
+
+              log_h->step(tti);
+
+              Debug("SYNC:  Worker %d synchronized\n", worker->get_id());
+
+              metrics.sfo = srslte_ue_sync_get_sfo(&ue_sync);
+              metrics.cfo = srslte_ue_sync_get_cfo(&ue_sync);
+              worker->set_cfo(ul_dl_factor * metrics.cfo / 15000);
+              worker_com->set_sync_metrics(metrics);
+
+              worker->set_sample_offset(srslte_ue_sync_get_sfo(&ue_sync)/1000);
+
+              /* Compute TX time: Any transmission happens in TTI4 thus advance 4 ms the reception time */
+              srslte_timestamp_t rx_time, tx_time, tx_time_prach;
+              srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
+              srslte_timestamp_copy(&tx_time, &rx_time);
+              srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
+              worker->set_tx_time(tx_time);
+
+              Debug("SYNC:  Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
+              worker->set_tti(tti, tx_mutex_cnt);
+              tx_mutex_cnt = (tx_mutex_cnt+1) % nof_tx_mutex;
+
+              // Check if we need to TX a PRACH
+              if (prach_buffer->is_ready_to_send(tti)) {
+                srslte_timestamp_copy(&tx_time_prach, &rx_time);
+                srslte_timestamp_add(&tx_time_prach, 0, prach::tx_advance_sf * 1e-3);
+                prach_buffer->send(radio_h, ul_dl_factor * metrics.cfo / 15000, worker_com->pathloss, tx_time_prach);
+                radio_h->tx_end();
+                worker_com->p0_preamble = prach_buffer->get_p0_preamble();
+                worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
+              }
+              workers_pool->start_worker(worker);
+              // Notify RRC in-sync every 1 frame
+              if ((tti % 10) == 0) {
+                rrc->in_sync();
+                log_h->debug("SYNC:  Sending in-sync to RRC\n");
+              }
+            case 0:
+              log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
+              // Notify RRC of out-of-sync frame
+              rrc->out_of_sync();
+              worker->release();
+              worker_com->reset_ul();
+              break;
+            default:
+              log_h->error("SYNC:  Receiving frorm radio.\n");
+              phy_state = IDLE;
+              radio_h->reset();
           }
         } else {
           // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
@@ -673,7 +703,7 @@ void phch_recv::run_thread() {
         break;
       case IDLE:
         if (!is_in_idle) {
-          radio_h->stop_rx();
+          stop_rx();
         }
         is_in_idle = true;
         usleep(1000);
@@ -682,6 +712,22 @@ void phch_recv::run_thread() {
   }
 }
 
+void phch_recv::stop_rx() {
+  if (radio_is_rx) {
+    Info("SYNC:  Stopping RX streaming\n");
+    radio_h->stop_rx();
+  }
+  radio_is_rx = false;
+}
+
+void phch_recv::start_rx() {
+  if (!radio_is_rx) {
+    Info("SYNC:  Starting RX streaming\n");
+    radio_h->start_rx();
+  }
+  radio_is_rx = true;
+}
+
 uint32_t phch_recv::get_current_tti() {
   return tti;
 }
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index d3933df13..828ecbf0e 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -232,6 +232,11 @@ void phy::cell_search_start()
   sf_recv.cell_search_start();
 }
 
+void phy::cell_search_stop()
+{
+  sf_recv.cell_search_stop();
+}
+
 void phy::cell_search_next()
 {
   sf_recv.cell_search_next();
@@ -293,7 +298,7 @@ int phy::prach_tx_tti()
 
 void phy::reset()
 {
-  // TODO 
+  sf_recv.stop_sync();
   n_ta = 0; 
   pdcch_dl_search_reset();
   for(uint32_t i=0;i<nof_workers;i++) {
@@ -317,10 +322,6 @@ int phy::sr_last_tx_tti()
   return workers_common.sr_last_tx_tti;
 }
 
-void phy::resync_sfn() {
-  sf_recv.resync_sfn();
-}
-
 void phy::set_earfcn(vector< uint32_t > earfcns)
 {
   sf_recv.set_earfcn(earfcns);
@@ -331,11 +332,6 @@ bool phy::sync_status()
   return sf_recv.status_is_sync();
 }
 
-bool phy::sync_stop()
-{
-  return sf_recv.stop_sync();
-}
-
 void phy::set_rar_grant(uint32_t tti, uint8_t grant_payload[SRSLTE_RAR_GRANT_LEN])
 {
   workers_common.set_rar_grant(tti, grant_payload);
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index cb7c117a3..e284e4569 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -430,6 +430,7 @@ void rrc::run_thread() {
         plmn_select_timeout++;
         if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
           rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
+          phy->cell_search_stop();
           sleep(1);
           rrc_log->console("\nRRC PLMN Search: timeout expired. Searching again\n");
           plmn_select_timeout = 0;
@@ -714,7 +715,6 @@ void rrc::send_con_restablish_request() {
   mac->reset();
 
   // FIXME: Cell selection should be different??
-  phy->resync_sfn();
 
   // Wait for cell re-synchronization
   uint32_t timeout_cnt = 0;
@@ -1097,7 +1097,7 @@ void rrc::rrc_connection_release() {
   pthread_mutex_lock(&mutex);
   drb_up = false;
   state = RRC_STATE_IDLE;
-  phy->sync_stop();
+  phy->reset();
   set_phy_default();
   set_mac_default();
   mac_timers->get(t311)->run();

From 68b1782c86d0e2038a111f228da42ad99558ba92 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 18 Sep 2017 14:02:31 +0200
Subject: [PATCH 187/269] Fixed Paging and reattachment in new state machine.
 Moved GW to srsue

---
 .../srslte/interfaces/enb_metrics_interface.h |    2 +-
 lib/include/srslte/interfaces/ue_interfaces.h |    8 +
 lib/include/srslte/upper/pdcp.h               |    3 +
 lib/src/upper/pdcp.cc                         |    9 +
 srsue/hdr/ue.h                                |    7 +-
 srsue/hdr/ue_metrics_interface.h              |    4 +-
 {lib/include/srslte => srsue/hdr}/upper/gw.h  |   25 +-
 .../srslte => srsue/hdr}/upper/gw_metrics.h   |    2 +-
 srsue/hdr/upper/nas.h                         |    6 +-
 srsue/hdr/upper/rrc.h                         |    9 +-
 srsue/hdr/upper/rrc_common.h                  |    6 +-
 srsue/src/phy/phch_recv.cc                    |   24 +-
 srsue/src/phy/phy.cc                          |    4 +-
 srsue/src/ue.cc                               |    6 +-
 {lib => srsue}/src/upper/gw.cc                |  232 ++--
 srsue/src/upper/nas.cc                        |    8 +
 srsue/src/upper/rrc.cc                        | 1132 +++++++++--------
 17 files changed, 816 insertions(+), 671 deletions(-)
 rename {lib/include/srslte => srsue/hdr}/upper/gw.h (77%)
 rename {lib/include/srslte => srsue/hdr}/upper/gw_metrics.h (98%)
 rename {lib => srsue}/src/upper/gw.cc (51%)

diff --git a/lib/include/srslte/interfaces/enb_metrics_interface.h b/lib/include/srslte/interfaces/enb_metrics_interface.h
index 6893b4c2f..ee00e3453 100644
--- a/lib/include/srslte/interfaces/enb_metrics_interface.h
+++ b/lib/include/srslte/interfaces/enb_metrics_interface.h
@@ -32,7 +32,7 @@
 #include "upper/common_enb.h"
 #include "upper/s1ap_metrics.h"
 #include "upper/rrc_metrics.h"
-#include "srslte/upper/gw_metrics.h"
+#include "../../../../srsue/hdr/upper/gw_metrics.h"
 #include "srslte/upper/rlc_metrics.h"
 #include "mac/mac_metrics.h"
 #include "phy/phy_metrics.h"
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 1315b4c82..c15a09804 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -113,6 +113,13 @@ public:
   virtual void      deattach_request() = 0;
 };
 
+// NAS interface for UE
+class nas_interface_gw
+{
+public:
+  virtual void      attach_request() = 0;
+};
+
 // RRC interface for MAC
 class rrc_interface_mac_common
 {
@@ -172,6 +179,7 @@ class pdcp_interface_gw
 {
 public:
   virtual void write_sdu(uint32_t lcid, srslte::byte_buffer_t *sdu) = 0;
+  virtual bool is_drb_enabled(uint32_t lcid) = 0;
 };
 
 // PDCP interface for RRC
diff --git a/lib/include/srslte/upper/pdcp.h b/lib/include/srslte/upper/pdcp.h
index abdbeeaea..ce6ece151 100644
--- a/lib/include/srslte/upper/pdcp.h
+++ b/lib/include/srslte/upper/pdcp.h
@@ -50,6 +50,9 @@ public:
             uint8_t direction_);
   void stop();
 
+  // GW interface
+  bool is_drb_enabled(uint32_t lcid);
+
   // RRC interface
   void reset();
   void write_sdu(uint32_t lcid, byte_buffer_t *sdu);
diff --git a/lib/src/upper/pdcp.cc b/lib/src/upper/pdcp.cc
index 5f4afbd62..0acc7f6bd 100644
--- a/lib/src/upper/pdcp.cc
+++ b/lib/src/upper/pdcp.cc
@@ -65,6 +65,15 @@ void pdcp::reset()
 /*******************************************************************************
   RRC/GW interface
 *******************************************************************************/
+bool pdcp::is_drb_enabled(uint32_t lcid)
+{
+  if(lcid >= SRSLTE_N_RADIO_BEARERS) {
+    pdcp_log->error("Radio bearer id must be in [0:%d] - %d\n", SRSLTE_N_RADIO_BEARERS, lcid);
+    return false;
+  }
+  return pdcp_array[lcid].is_active();
+}
+
 void pdcp::write_sdu(uint32_t lcid, byte_buffer_t *sdu)
 {
   if(valid_lcid(lcid))
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 11565ec00..d5f5f9fc5 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -45,7 +45,7 @@
 #include "srslte/upper/pdcp.h"
 #include "upper/rrc.h"
 #include "upper/nas.h"
-#include "srslte/upper/gw.h"
+#include "upper/gw.h"
 #include "upper/usim.h"
 
 #include "srslte/common/buffer_pool.h"
@@ -82,9 +82,6 @@ public:
 
   void pregenerate_signals(bool enable);
 
-  // Testing
-  void test_con_restablishment();
-
 
 private:
   virtual ~ue();
@@ -97,7 +94,7 @@ private:
   srslte::pdcp       pdcp;
   srsue::rrc         rrc;
   srsue::nas         nas;
-  srslte::gw         gw;
+  srsue::gw          gw;
   srsue::usim        usim;
 
 #ifdef LOG_STDOUT
diff --git a/srsue/hdr/ue_metrics_interface.h b/srsue/hdr/ue_metrics_interface.h
index 70688863e..9555574f3 100644
--- a/srsue/hdr/ue_metrics_interface.h
+++ b/srsue/hdr/ue_metrics_interface.h
@@ -29,7 +29,7 @@
 
 #include <stdint.h>
 
-#include "srslte/upper/gw_metrics.h"
+#include "upper/gw_metrics.h"
 #include "srslte/upper/rlc_metrics.h"
 #include "mac/mac_metrics.h"
 #include "phy/phy_metrics.h"
@@ -48,7 +48,7 @@ typedef struct {
   phy_metrics_t         phy;
   mac_metrics_t         mac;
   srslte::rlc_metrics_t rlc;
-  srslte::gw_metrics_t  gw;
+  gw_metrics_t          gw;
 }ue_metrics_t;
 
 // UE interface
diff --git a/lib/include/srslte/upper/gw.h b/srsue/hdr/upper/gw.h
similarity index 77%
rename from lib/include/srslte/upper/gw.h
rename to srsue/hdr/upper/gw.h
index aa92ddc32..800b31624 100644
--- a/lib/include/srslte/upper/gw.h
+++ b/srsue/hdr/upper/gw.h
@@ -33,39 +33,40 @@
 #include "srslte/common/msg_queue.h"
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/threads.h"
-#include "srslte/upper/gw_metrics.h"
+#include "gw_metrics.h"
 
 #include <linux/if.h>
 
-namespace srslte {
+namespace srsue {
 
 class gw
-    :public srsue::gw_interface_pdcp
-    ,public srsue::gw_interface_nas
+    :public gw_interface_pdcp
+    ,public gw_interface_nas
     ,public thread
 {
 public:
   gw();
-  void init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_);
+  void init(pdcp_interface_gw *pdcp_, nas_interface_gw *nas_, srslte::log *gw_log_, uint32_t lcid_);
   void stop();
 
   void get_metrics(gw_metrics_t &m);
 
   // PDCP interface
-  void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
+  void write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu);
 
   // NAS interface
-  error_t setup_if_addr(uint32_t ip_addr, char *err_str);
+  srslte::error_t setup_if_addr(uint32_t ip_addr, char *err_str);
 
 private:
 
   static const int GW_THREAD_PRIO = 7;
 
-  srsue::pdcp_interface_gw  *pdcp;
-  srsue::ue_interface       *ue;
+  pdcp_interface_gw  *pdcp;
+  nas_interface_gw   *nas;
 
-  byte_buffer_pool   *pool;
-  log                *gw_log;
+  srslte::byte_buffer_pool   *pool;
+
+  srslte::log                *gw_log;
   bool                running;
   bool                run_enable;
   int32               tun_fd;
@@ -81,7 +82,7 @@ private:
   struct timeval      metrics_time[3];
 
   void                run_thread();
-  error_t     init_if(char *err_str);
+  srslte::error_t     init_if(char *err_str);
 };
 
 } // namespace srsue
diff --git a/lib/include/srslte/upper/gw_metrics.h b/srsue/hdr/upper/gw_metrics.h
similarity index 98%
rename from lib/include/srslte/upper/gw_metrics.h
rename to srsue/hdr/upper/gw_metrics.h
index b5d8eaf23..e596046c9 100644
--- a/lib/include/srslte/upper/gw_metrics.h
+++ b/srsue/hdr/upper/gw_metrics.h
@@ -28,7 +28,7 @@
 #define UE_GW_METRICS_H
 
 
-namespace srslte {
+namespace srsue {
 
 struct gw_metrics_t
 {
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 0f9b36bf8..1aa0ab3d6 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -63,7 +63,10 @@ typedef enum {
 } plmn_selection_state_t;
 
 class nas
-  : public nas_interface_rrc, public nas_interface_ue {
+  : public nas_interface_rrc,
+    public nas_interface_ue,
+    public nas_interface_gw
+{
 public:
   nas();
   void init(usim_interface_nas  *usim_,
@@ -106,6 +109,7 @@ private:
 
   plmn_selection_state_t plmn_selection;
   LIBLTE_RRC_PLMN_IDENTITY_STRUCT current_plmn;
+  LIBLTE_RRC_PLMN_IDENTITY_STRUCT selecting_plmn;
   LIBLTE_RRC_PLMN_IDENTITY_STRUCT home_plmn;
 
   std::vector<LIBLTE_RRC_PLMN_IDENTITY_STRUCT > known_plmns;
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 81514b559..8dfa7f70d 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -78,8 +78,6 @@ public:
   // Timeout callback interface
   void timer_expired(uint32_t timeout_id);
 
-  void test_con_restablishment();
-
   void liblte_rrc_log(char *str);
 
 private:
@@ -130,7 +128,6 @@ private:
   uint32_t n310_cnt, N310;
   uint32_t n311_cnt, N311;
   uint32_t t301, t310, t311;
-  uint32_t safe_reset_timer;
   int ue_category;
 
   typedef struct {
@@ -155,6 +152,10 @@ private:
   } si_acquire_state_t;
 
   si_acquire_state_t si_acquire_state;
+  void               run_si_acquisition_procedure();
+  uint32_t           sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
+  uint32_t           nof_sib1_trials;
+  uint32_t           last_win_start;
 
   void select_next_cell_in_plmn();
   LIBLTE_RRC_PLMN_IDENTITY_STRUCT selected_plmn_id;
@@ -242,12 +243,10 @@ private:
   void          parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
 
   // Helpers
-  void          reset_ue();
   void          rrc_connection_release();
   void          radio_link_failure(); 
   static void*  start_sib_thread(void *rrc_);
   void          sib_search();
-  uint32_t      sib_start_tti(uint32_t tti, uint32_t period, uint32_t x);
   void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
diff --git a/srsue/hdr/upper/rrc_common.h b/srsue/hdr/upper/rrc_common.h
index 40edfb467..95b909620 100644
--- a/srsue/hdr/upper/rrc_common.h
+++ b/srsue/hdr/upper/rrc_common.h
@@ -38,14 +38,16 @@ typedef enum {
   RRC_STATE_CELL_SELECTED,
   RRC_STATE_CONNECTING,
   RRC_STATE_CONNECTED,
+  RRC_STATE_LEAVE_CONNECTED,
   RRC_STATE_N_ITEMS,
 } rrc_state_t;
 static const char rrc_state_text[RRC_STATE_N_ITEMS][100] = {"IDLE",
                                                             "PLMN SELECTION",
-                                                            "CELL SELECTION",
+                                                            "CELL SELECTING",
+                                                            "CELL SELECTED",
                                                             "CONNECTING",
                                                             "CONNECTED",
-                                                            "RRC CONNECTED"};
+                                                            "LEAVE CONNECTED"};
 
 } // namespace srsue
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index bdfafb167..25bfb89b2 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -407,14 +407,18 @@ void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
 }
 
 bool phch_recv::stop_sync() {
-  Info("SYNC:  Going to IDLE\n");
-  phy_state = IDLE;
-  int cnt=0;
-  while(!is_in_idle && cnt<100) {
-    usleep(10000);
-    cnt++;
+  if (phy_state == IDLE && is_in_idle) {
+    return true;
+  } else {
+    Info("SYNC:  Going to IDLE\n");
+    phy_state = IDLE;
+    int cnt = 0;
+    while (!is_in_idle && cnt < 100) {
+      usleep(10000);
+      cnt++;
+    }
+    return is_in_idle;
   }
-  return is_in_idle;
 }
 
 void phch_recv::cell_search_inc()
@@ -425,7 +429,6 @@ void phch_recv::cell_search_inc()
       cur_earfcn_index = 0;
     }
   }
-  usleep(100000);
   Info("SYNC:  Cell Search idx %d/%d\n", cur_earfcn_index, earfcn.size());
   if (current_earfcn != earfcn[cur_earfcn_index]) {
     current_earfcn = earfcn[cur_earfcn_index];
@@ -627,8 +630,6 @@ void phch_recv::run_thread() {
             rrc->cell_found(earfcn[cur_earfcn_index], cell, 10*log10(measure_rsrp/1000));
             break;
           case 0:
-            log_h->error("SYNC:  Getting RSRP cell measurement.\n");
-            cell_search_next();
             break;
           default:
             log_h->error("SYNC:  Receiving frorm radio.\n");
@@ -684,6 +685,7 @@ void phch_recv::run_thread() {
                 rrc->in_sync();
                 log_h->debug("SYNC:  Sending in-sync to RRC\n");
               }
+              break;
             case 0:
               log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
               // Notify RRC of out-of-sync frame
@@ -692,7 +694,7 @@ void phch_recv::run_thread() {
               worker_com->reset_ul();
               break;
             default:
-              log_h->error("SYNC:  Receiving frorm radio.\n");
+              log_h->error("SYNC:  Receiving from radio.\n");
               phy_state = IDLE;
               radio_h->reset();
           }
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 828ecbf0e..6ae745d84 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -298,8 +298,8 @@ int phy::prach_tx_tti()
 
 void phy::reset()
 {
-  sf_recv.stop_sync();
-  n_ta = 0; 
+  Info("Resetting PHY\n");
+  n_ta = 0;
   pdcch_dl_search_reset();
   for(uint32_t i=0;i<nof_workers;i++) {
     workers[i].reset();
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 55e0c7d92..4619b43ce 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -167,7 +167,7 @@ bool ue::init(all_args_t *args_)
   pdcp.init(&rlc, &rrc, &gw, &pdcp_log, 0 /* RB_ID_SRB0 */, SECURITY_DIRECTION_UPLINK);
 
   nas.init(&usim, &rrc, &gw, &nas_log, 1 /* RB_ID_SRB1 */);
-  gw.init(&pdcp, this, &gw_log, 3 /* RB_ID_DRB1 */);
+  gw.init(&pdcp, &nas, &gw_log, 3 /* RB_ID_DRB1 */);
 
   usim.init(&args->usim, &usim_log);
 
@@ -195,10 +195,6 @@ void ue::pregenerate_signals(bool enable)
   phy.enable_pregen_signals(enable);
 }
 
-void ue::test_con_restablishment() {
-  rrc.test_con_restablishment();
-}
-
 void ue::stop()
 {
   if(started)
diff --git a/lib/src/upper/gw.cc b/srsue/src/upper/gw.cc
similarity index 51%
rename from lib/src/upper/gw.cc
rename to srsue/src/upper/gw.cc
index 321d8d9e9..19c4f38d2 100644
--- a/lib/src/upper/gw.cc
+++ b/srsue/src/upper/gw.cc
@@ -25,7 +25,7 @@
  */
 
 
-#include "srslte/upper/gw.h"
+#include "../../hdr/upper/gw.h"
 
 #include <errno.h>
 #include <unistd.h>
@@ -38,7 +38,7 @@
 #include <sys/socket.h>
 
 
-namespace srslte {
+namespace srsue {
 
 gw::gw()
   :if_up(false)
@@ -46,11 +46,11 @@ gw::gw()
   current_ip_addr = 0;
 }
 
-void gw::init(srsue::pdcp_interface_gw *pdcp_, srsue::ue_interface *ue_, log *gw_log_, uint32_t lcid_)
+void gw::init(pdcp_interface_gw *pdcp_, nas_interface_gw *nas_, srslte::log *gw_log_, uint32_t lcid_)
 {
-  pool    = byte_buffer_pool::get_instance();
+  pool    = srslte::byte_buffer_pool::get_instance();
   pdcp    = pdcp_;
-  ue      = ue_;
+  nas     = nas_;
   gw_log  = gw_log_;
   lcid    = lcid_;
   run_enable = true;
@@ -107,7 +107,7 @@ void gw::get_metrics(gw_metrics_t &m)
 /*******************************************************************************
   PDCP interface
 *******************************************************************************/
-void gw::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
+void gw::write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu)
 {
   gw_log->info_hex(pdu->msg, pdu->N_bytes, "RX PDU");
   gw_log->info("RX PDU. Stack latency: %ld us\n", pdu->get_latency_us());
@@ -128,7 +128,7 @@ void gw::write_pdu(uint32_t lcid, byte_buffer_t *pdu)
 /*******************************************************************************
   NAS interface
 *******************************************************************************/
-error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
+srslte::error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
 {
   if (ip_addr != current_ip_addr) {
     if(!if_up)
@@ -136,7 +136,7 @@ error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
       if(init_if(err_str))
       {
         gw_log->error("init_if failed\n");
-        return(ERROR_CANT_START);
+        return(srslte::ERROR_CANT_START);
       }
     }
 
@@ -149,7 +149,7 @@ error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket address: %s\n", err_str);
       close(tun_fd);
-      return(ERROR_CANT_START);
+      return(srslte::ERROR_CANT_START);
     }
     ifr.ifr_netmask.sa_family                                 = AF_INET;
     ((struct sockaddr_in *)&ifr.ifr_netmask)->sin_addr.s_addr = inet_addr("255.255.255.0");
@@ -158,7 +158,7 @@ error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
       err_str = strerror(errno);
       gw_log->debug("Failed to set socket netmask: %s\n", err_str);
       close(tun_fd);
-      return(ERROR_CANT_START);
+      return(srslte::ERROR_CANT_START);
     }
 
     current_ip_addr = ip_addr;
@@ -167,59 +167,59 @@ error_t gw::setup_if_addr(uint32_t ip_addr, char *err_str)
     start(GW_THREAD_PRIO);
   }
 
-  return(ERROR_NONE);
+  return(srslte::ERROR_NONE);
 }
 
-error_t gw::init_if(char *err_str)
+srslte::error_t gw::init_if(char *err_str)
 {
-    if(if_up)
-    {
-      return(ERROR_ALREADY_STARTED);
-    }
+  if(if_up)
+  {
+    return(srslte::ERROR_ALREADY_STARTED);
+  }
 
-    char dev[IFNAMSIZ] = "tun_srsue";
+  char dev[IFNAMSIZ] = "tun_srsue";
 
-    // Construct the TUN device
-    tun_fd = open("/dev/net/tun", O_RDWR);
-    gw_log->info("TUN file descriptor = %d\n", tun_fd);
-    if(0 > tun_fd)
-    {
-        err_str = strerror(errno);
-        gw_log->debug("Failed to open TUN device: %s\n", err_str);
-        return(ERROR_CANT_START);
-    }
-    memset(&ifr, 0, sizeof(ifr));
-    ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
-    strncpy(ifr.ifr_ifrn.ifrn_name, dev, IFNAMSIZ);
-    if(0 > ioctl(tun_fd, TUNSETIFF, &ifr))
-    {
-        err_str = strerror(errno);
-        gw_log->debug("Failed to set TUN device name: %s\n", err_str);
-        close(tun_fd);
-        return(ERROR_CANT_START);
-    }
+  // Construct the TUN device
+  tun_fd = open("/dev/net/tun", O_RDWR);
+  gw_log->info("TUN file descriptor = %d\n", tun_fd);
+  if(0 > tun_fd)
+  {
+      err_str = strerror(errno);
+      gw_log->debug("Failed to open TUN device: %s\n", err_str);
+      return(srslte::ERROR_CANT_START);
+  }
+  memset(&ifr, 0, sizeof(ifr));
+  ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
+  strncpy(ifr.ifr_ifrn.ifrn_name, dev, IFNAMSIZ);
+  if(0 > ioctl(tun_fd, TUNSETIFF, &ifr))
+  {
+      err_str = strerror(errno);
+      gw_log->debug("Failed to set TUN device name: %s\n", err_str);
+      close(tun_fd);
+      return(srslte::ERROR_CANT_START);
+  }
 
-    // Bring up the interface
-    sock = socket(AF_INET, SOCK_DGRAM, 0);
-    if(0 > ioctl(sock, SIOCGIFFLAGS, &ifr))
-    {
-        err_str = strerror(errno);
-        gw_log->debug("Failed to bring up socket: %s\n", err_str);
-        close(tun_fd);
-        return(ERROR_CANT_START);
-    }
-    ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
-    if(0 > ioctl(sock, SIOCSIFFLAGS, &ifr))
-    {
-        err_str = strerror(errno);
-        gw_log->debug("Failed to set socket flags: %s\n", err_str);
-        close(tun_fd);
-        return(ERROR_CANT_START);
-    }
+  // Bring up the interface
+  sock = socket(AF_INET, SOCK_DGRAM, 0);
+  if(0 > ioctl(sock, SIOCGIFFLAGS, &ifr))
+  {
+      err_str = strerror(errno);
+      gw_log->debug("Failed to bring up socket: %s\n", err_str);
+      close(tun_fd);
+      return(srslte::ERROR_CANT_START);
+  }
+  ifr.ifr_flags |= IFF_UP | IFF_RUNNING;
+  if(0 > ioctl(sock, SIOCSIFFLAGS, &ifr))
+  {
+      err_str = strerror(errno);
+      gw_log->debug("Failed to set socket flags: %s\n", err_str);
+      close(tun_fd);
+      return(srslte::ERROR_CANT_START);
+  }
 
-    if_up = true;
+  if_up = true;
 
-    return(ERROR_NONE);
+  return(srslte::ERROR_NONE);
 }
 
 /********************/
@@ -227,65 +227,81 @@ error_t gw::init_if(char *err_str)
 /********************/
 void gw::run_thread()
 {
-    struct iphdr   *ip_pkt;
-    uint32          idx = 0;
-    int32           N_bytes;
-    byte_buffer_t  *pdu = pool_allocate;
+  struct iphdr   *ip_pkt;
+  uint32          idx = 0;
+  int32           N_bytes;
+  srslte::byte_buffer_t  *pdu = pool_allocate;
+
+  const static uint32_t ATTACH_TIMEOUT_MS = 2000;
+  uint32_t attach_cnt = 0;
 
-    gw_log->info("GW IP packet receiver thread run_enable\n");
+  gw_log->info("GW IP packet receiver thread run_enable\n");
 
-    running = true; 
-    while(run_enable)
+  running = true;
+  while(run_enable)
+  {
+    if (SRSLTE_MAX_BUFFER_SIZE_BYTES-SRSLTE_BUFFER_HEADER_OFFSET > idx) {
+      N_bytes = read(tun_fd, &pdu->msg[idx], SRSLTE_MAX_BUFFER_SIZE_BYTES-SRSLTE_BUFFER_HEADER_OFFSET - idx);
+    } else {
+      gw_log->error("GW pdu buffer full - gw receive thread exiting.\n");
+      gw_log->console("GW pdu buffer full - gw receive thread exiting.\n");
+      break;
+    }
+    gw_log->debug("Read %d bytes from TUN fd=%d, idx=%d\n", N_bytes, tun_fd, idx);
+    if(N_bytes > 0)
     {
-      if (SRSLTE_MAX_BUFFER_SIZE_BYTES-SRSLTE_BUFFER_HEADER_OFFSET > idx) {
-        N_bytes = read(tun_fd, &pdu->msg[idx], SRSLTE_MAX_BUFFER_SIZE_BYTES-SRSLTE_BUFFER_HEADER_OFFSET - idx);
-      } else {
-        gw_log->error("GW pdu buffer full - gw receive thread exiting.\n");
-        gw_log->console("GW pdu buffer full - gw receive thread exiting.\n");
-        break; 
-      }
-      gw_log->debug("Read %d bytes from TUN fd=%d, idx=%d\n", N_bytes, tun_fd, idx);
-      if(N_bytes > 0)
-      {
-        pdu->N_bytes = idx + N_bytes;
-        ip_pkt       = (struct iphdr*)pdu->msg;
-
-        // Warning: Accept only IPv4 packets
-        if (ip_pkt->version == 4) {
-          // Check if entire packet was received
-          if(ntohs(ip_pkt->tot_len) == pdu->N_bytes)
-          {
-            gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
-
-            if (!run_enable) {
-              break;
+      pdu->N_bytes = idx + N_bytes;
+      ip_pkt       = (struct iphdr*)pdu->msg;
+
+      // Warning: Accept only IPv4 packets
+      if (ip_pkt->version == 4) {
+        // Check if entire packet was received
+        if(ntohs(ip_pkt->tot_len) == pdu->N_bytes)
+        {
+          gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
+
+          while(run_enable && !pdcp->is_drb_enabled(lcid)) {
+            if (attach_cnt == 0) {
+              gw_log->info("LCID=%d not active, requesting NAS attach\n", lcid);
+              nas->attach_request();
+            }
+            attach_cnt++;
+            if (attach_cnt == ATTACH_TIMEOUT_MS) {
+              attach_cnt = 0;
             }
-            
-            // Send PDU directly to PDCP
-            pdu->set_timestamp();
-            ul_tput_bytes += pdu->N_bytes;
-            pdcp->write_sdu(lcid, pdu);
-            
-            do {
-              pdu = pool_allocate;
-              if (!pdu) {
-                printf("Not enough buffers in pool\n");
-                usleep(100000);
-              }
-            } while(!pdu); 
-            idx = 0;
-          }else{
-            idx += N_bytes;
-          }            
-        } 
-      }else{
-        gw_log->error("Failed to read from TUN interface - gw receive thread exiting.\n");
-        gw_log->console("Failed to read from TUN interface - gw receive thread exiting.\n");
-        break;
+            usleep(1000);
+          }
+          attach_cnt = 0;
+
+          if (!run_enable) {
+            break;
+          }
+
+          // Send PDU directly to PDCP
+          pdu->set_timestamp();
+          ul_tput_bytes += pdu->N_bytes;
+          pdcp->write_sdu(lcid, pdu);
+
+          do {
+            pdu = pool_allocate;
+            if (!pdu) {
+              printf("Not enough buffers in pool\n");
+              usleep(100000);
+            }
+          } while(!pdu);
+          idx = 0;
+        }else{
+          idx += N_bytes;
+        }
       }
+    }else{
+      gw_log->error("Failed to read from TUN interface - gw receive thread exiting.\n");
+      gw_log->console("Failed to read from TUN interface - gw receive thread exiting.\n");
+      break;
     }
-    running = false; 
-    gw_log->info("GW IP receiver thread exiting.\n");
+  }
+  running = false;
+  gw_log->info("GW IP receiver thread exiting.\n");
 }
 
 } // namespace srsue
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index f3b3708af..f7f0598d3 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -74,7 +74,12 @@ void nas::attach_request() {
     } else if (plmn_selection == PLMN_SELECTED) {
       nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
       rrc->plmn_select(current_plmn);
+      selecting_plmn = current_plmn;
     }
+  } else if (state == EMM_STATE_REGISTERED) {
+    nas_log->info("NAS state is registered, connecting to same PLMN\n");
+    rrc->plmn_select(current_plmn);
+    selecting_plmn = current_plmn;
   } else {
     nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
   }
@@ -97,6 +102,7 @@ void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_
       nas_log->info("Detected known PLMN %s\n", plmn_id_to_c_str(plmn_id).c_str());
       if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
         rrc->plmn_select(plmn_id);
+        selecting_plmn = plmn_id;
       }
       return;
     }
@@ -107,6 +113,7 @@ void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_
                 tracking_area_code);
   if (plmn_id.mcc == home_plmn.mcc && plmn_id.mnc == home_plmn.mnc) {
     rrc->plmn_select(plmn_id);
+    selecting_plmn = plmn_id;
   }
 }
 
@@ -317,6 +324,7 @@ void nas::parse_attach_accept(uint32_t lcid, byte_buffer_t *pdu) {
     // FIXME: Setup the default EPS bearer context
 
     state = EMM_STATE_REGISTERED;
+    current_plmn = selecting_plmn;
 
     // Send EPS bearer context accept and attach complete
     count_ul++;
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index e284e4569..a55148c59 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -31,6 +31,7 @@
 #include "upper/rrc.h"
 #include <boost/assign.hpp>
 #include <upper/rrc.h>
+#include <srslte/asn1/liblte_rrc.h>
 #include "srslte/common/security.h"
 #include "srslte/common/bcd_helpers.h"
 
@@ -95,13 +96,15 @@ void rrc::init(phy_interface_rrc *phy_,
   t301 = mac_timers->get_unique_id();
   t310 = mac_timers->get_unique_id();
   t311 = mac_timers->get_unique_id();
-  safe_reset_timer = mac_timers->get_unique_id();
 
   transaction_id = 0;
 
   // Register logging handler with liblte_rrc
   liblte_rrc_log_register_handler(this, liblte_rrc_handler);
 
+  nof_sib1_trials = 0;
+  last_win_start  = 0;
+
   // Set default values for all layers
   set_rrc_default();
   set_phy_default();
@@ -117,6 +120,14 @@ rrc_state_t rrc::get_state() {
   return state;
 }
 
+bool rrc::is_connected() {
+  return (RRC_STATE_CONNECTED == state);
+}
+
+bool rrc::have_drb() {
+  return drb_up;
+}
+
 void rrc::set_ue_category(int category) {
   if (category >= 1 && category <= 5) {
     ue_category = category;
@@ -125,20 +136,194 @@ void rrc::set_ue_category(int category) {
   }
 }
 
+/*
+ *
+ * RRC State Machine
+ *
+ */
+void rrc::run_thread() {
+
+  while (thread_running) {
+
+    if (state >= RRC_STATE_IDLE && state < RRC_STATE_CONNECTING) {
+      run_si_acquisition_procedure();
+    }
+
+    switch(state) {
+      /* Procedures in IDLE state 36.304 Sec 4 */
+      case RRC_STATE_IDLE:
+        // If camping on the cell, it will receive SI and paging from PLMN
+        if (phy->sync_status()) {
+
+        // If not camping on a cell
+        } else {
+          // If NAS is attached, perform cell reselection on current PLMN
+          if (nas->is_attached()) {
+            rrc_log->info("RRC IDLE: NAS is attached, PHY not synchronized. Re-selecting cell...\n");
+            plmn_select(selected_plmn_id);
+          }
+          // If not attached, PLMN selection will be triggered from higher layers
+        }
+        break;
+      case RRC_STATE_PLMN_SELECTION:
+        plmn_select_timeout++;
+        if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
+          rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
+          phy->cell_search_stop();
+          sleep(1);
+          rrc_log->console("\nRRC PLMN Search: timeout expired. Searching again\n");
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTING:
+        if (phy->sync_status()) {
+          if (!current_cell->has_valid_sib1) {
+            si_acquire_state = SI_ACQUIRE_SIB1;
+          } else if (!current_cell->has_valid_sib2) {
+            si_acquire_state = SI_ACQUIRE_SIB2;
+          } else {
+            apply_sib2_configs(&current_cell->sib2);
+            si_acquire_state = SI_ACQUIRE_IDLE;
+            state = RRC_STATE_CELL_SELECTED;
+          }
+        }
+        select_cell_timeout++;
+        if (select_cell_timeout >= RRC_SELECT_CELL_TIMEOUT) {
+          rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
+          state = RRC_STATE_PLMN_SELECTION;
+          plmn_select_timeout = 0;
+          phy->cell_search_start();
+        }
+        break;
+      case RRC_STATE_CELL_SELECTED:
+        rrc_log->info("RRC Cell Selected: Sending connection request...\n");
+        send_con_request();
+        state = RRC_STATE_CONNECTING;
+        connecting_timeout = 0;
+        break;
+      case RRC_STATE_CONNECTING:
+        connecting_timeout++;
+        if (connecting_timeout >= RRC_CONNECTING_TIMEOUT) {
+          // Select another cell
+          rrc_log->info("RRC Connecting: timeout expired. Selecting next cell\n");
+          state = RRC_STATE_CELL_SELECTING;
+        }
+        break;
+      case RRC_STATE_CONNECTED:
+        // Take measurements, cell reselection, etc
+        break;
+      case RRC_STATE_LEAVE_CONNECTED:
+        usleep(60000);
+        rrc_log->info("Leaving RRC_CONNECTED state\n");
+        drb_up = false;
+        pdcp->reset();
+        rlc->reset();
+        phy->reset();
+        mac->reset();
+        set_phy_default();
+        set_mac_default();
+        mac->pcch_start_rx();
+        mac_timers->get(t311)->run();
+        mac_timers->get(t310)->stop();
+        mac_timers->get(t311)->stop();
+        state = RRC_STATE_IDLE;
+        break;
+      default:
+        break;
+    }
+    usleep(1000);
+  }
+}
+
+
+
+
+
 
 
 /*******************************************************************************
 *
 *
 *
-* PLMN selection, cell selection/reselection and acquisition of SI procedures
+* System Information Acquisition procedure
 *
 *
 *
 *******************************************************************************/
 
+
+// Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
+uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
+  return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
+}
+
+void rrc::run_si_acquisition_procedure()
+{
+  uint32_t tti;
+  uint32_t si_win_start=0, si_win_len=0;
+  uint16_t period;
+  const int SIB1_SEARCH_TIMEOUT = 30;
+
+  switch (si_acquire_state) {
+    case SI_ACQUIRE_SIB1:
+      // Instruct MAC to look for SIB1
+      tti = mac->get_current_tti();
+      si_win_start = sib_start_tti(tti, 2, 5);
+      if (tti > last_win_start + 10) {
+        last_win_start = si_win_start;
+        mac->bcch_start_rx(si_win_start, 1);
+        rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
+                       si_win_start, 1);
+        nof_sib1_trials++;
+        if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
+          if (state == RRC_STATE_CELL_SELECTING) {
+            select_next_cell_in_plmn();
+            si_acquire_state = SI_ACQUIRE_IDLE;
+          } else if (state == RRC_STATE_PLMN_SELECTION) {
+            phy->cell_search_next();
+          }
+          nof_sib1_trials = 0;
+        }
+      }
+      break;
+    case SI_ACQUIRE_SIB2:
+      // Instruct MAC to look for SIB2 only when selecting a cell
+      tti = mac->get_current_tti();
+      period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
+      si_win_start = sib_start_tti(tti, period, 0);
+      if (tti > last_win_start + 10) {
+        last_win_start = si_win_start;
+        si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
+
+        mac->bcch_start_rx(si_win_start, si_win_len);
+        rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
+                       si_win_start, si_win_len);
+      }
+      break;
+    default:
+      break;
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
 /*******************************************************************************
-NAS interface
+*
+*
+*
+* PLMN selection, cell selection/reselection and acquisition of SI procedures
+*
+*
+*
 *******************************************************************************/
 
 uint16_t rrc::get_mcc() {
@@ -167,17 +352,26 @@ void rrc::plmn_search() {
 }
 
 void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
-  rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
-
 
-  // Sort cells according to RSRP
+  // If already camping on the selected PLMN, select this cell
+  if (state == RRC_STATE_IDLE || state == RRC_STATE_CONNECTED || state == RRC_STATE_PLMN_SELECTION) {
+    if (phy->sync_status() && selected_plmn_id.mcc == plmn_id.mcc && selected_plmn_id.mnc == plmn_id.mnc) {
+      rrc_log->info("Already camping on selected PLMN, connecting...\n");
+      state = RRC_STATE_CELL_SELECTING;
+    } else {
+      rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
+      // Sort cells according to RSRP
 
-  selected_plmn_id = plmn_id;
-  last_selected_cell = -1;
-  select_cell_timeout = 0;
+      selected_plmn_id = plmn_id;
+      last_selected_cell = -1;
+      select_cell_timeout = 0;
 
-  state = RRC_STATE_CELL_SELECTING;
-  select_next_cell_in_plmn();
+      state = RRC_STATE_CELL_SELECTING;
+      select_next_cell_in_plmn();
+    }
+  } else {
+    rrc_log->warning("Requested PLMN select in incorrect state %s\n", rrc_state_text[state]);
+  }
 }
 
 void rrc::select_next_cell_in_plmn() {
@@ -210,10 +404,6 @@ void rrc::select_next_cell_in_plmn() {
   rrc_log->info("No more known cells...\n");
 }
 
-/*******************************************************************************
-PHY interface
-*******************************************************************************/
-
 void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
   // find if cell_id-earfcn combination already exists
@@ -254,6 +444,24 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
                 cell.earfcn, cell.rsrp);
 }
 
+
+
+
+
+
+
+
+
+/*******************************************************************************
+*
+*
+*
+* Detection of Radio-Link Failures
+*
+*
+*
+*******************************************************************************/
+
 // Detection of physical layer problems (5.3.11.1)
 void rrc::out_of_sync() {
     current_cell->in_sync = false;
@@ -273,352 +481,78 @@ void rrc::in_sync() {
   current_cell->in_sync = true;
   if (mac_timers->get(t310)->is_running()) {
     n311_cnt++;
-    if (n311_cnt == N311) {
-      mac_timers->get(t310)->stop();
-      n311_cnt = 0;
-      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
-    }
-  }
-}
-
-/*******************************************************************************
-PDCP interface
-*******************************************************************************/
-void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
-  pool->deallocate(pdu);
-  if (state == RRC_STATE_PLMN_SELECTION) {
-    // Do we need to do something with BCH?
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
-  } else {
-    rrc_log->warning("Received BCCH BCH in incorrect state\n");
-  }
-}
-
-void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
-  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
-  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
-  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-  bit_buf.N_bits = pdu->N_bytes * 8;
-  pool->deallocate(pdu);
-  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
-
-  if (dlsch_msg.N_sibs > 0) {
-    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
-      mac->bcch_stop_rx();
-
-      // Handle SIB1
-      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
-
-      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
-                    current_cell->sib1.cell_id & 0xfff,
-                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
-                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
-
-
-      // Set TDD Config
-      if (current_cell->sib1.tdd) {
-        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
-      }
-
-      current_cell->has_valid_sib1 = true;
-
-      // Send PLMN and TAC to NAS
-      std::stringstream ss;
-      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
-        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
-      }
-
-      // Jump to next state
-      switch(state) {
-        case RRC_STATE_CELL_SELECTING:
-          si_acquire_state = SI_ACQUIRE_SIB2;
-          break;
-        case RRC_STATE_PLMN_SELECTION:
-          si_acquire_state = SI_ACQUIRE_IDLE;
-          rrc_log->info("SI Acquisition done. Searching next cell...\n");
-          usleep(5000);
-          phy->cell_search_next();
-          break;
-        default:
-          si_acquire_state = SI_ACQUIRE_IDLE;
-      }
-
-    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
-               SI_ACQUIRE_SIB2 == si_acquire_state) {
-      mac->bcch_stop_rx();
-
-      // Handle SIB2
-      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
-      rrc_log->info("SIB2 received\n");
-
-      apply_sib2_configs(&current_cell->sib2);
-
-      current_cell->has_valid_sib2 = true;
-
-      // Jump to next state
-      switch(state) {
-        case RRC_STATE_CELL_SELECTING:
-          si_acquire_state = SI_ACQUIRE_IDLE;
-          state = RRC_STATE_CELL_SELECTED;
-          break;
-        default:
-          si_acquire_state = SI_ACQUIRE_IDLE;
-      }
-    }
-  }
-}
-
-
-// Right now, this thread only controls System Information acquisition procedure
-void rrc::run_thread() {
-  uint32_t tti;
-  uint32_t si_win_start=0, si_win_len=0, last_win_start=0;
-  uint16_t period;
-  uint32_t nof_sib1_trials = 0;
-  const int SIB1_SEARCH_TIMEOUT = 30;
-
-  while (thread_running) {
-    switch (si_acquire_state) {
-      case SI_ACQUIRE_SIB1:
-        // Instruct MAC to look for SIB1
-        tti = mac->get_current_tti();
-        si_win_start = sib_start_tti(tti, 2, 5);
-        if (tti > last_win_start + 10) {
-          last_win_start = si_win_start;
-          mac->bcch_start_rx(si_win_start, 1);
-          rrc_log->debug("Instructed MAC to search for SIB1, win_start=%d, win_len=%d\n",
-                         si_win_start, 1);
-          nof_sib1_trials++;
-          if (nof_sib1_trials >= SIB1_SEARCH_TIMEOUT) {
-            if (state == RRC_STATE_CELL_SELECTING) {
-              select_next_cell_in_plmn();
-              si_acquire_state = SI_ACQUIRE_IDLE;
-            } else if (state == RRC_STATE_PLMN_SELECTION) {
-              phy->cell_search_next();
-            }
-            nof_sib1_trials = 0;
-          }
-        }
-        break;
-      case SI_ACQUIRE_SIB2:
-        // Instruct MAC to look for SIB2 only when selecting a cell
-        tti = mac->get_current_tti();
-        period = liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity];
-        si_win_start = sib_start_tti(tti, period, 0);
-        if (tti > last_win_start + 10) {
-          last_win_start = si_win_start;
-          si_win_len = liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length];
-
-          mac->bcch_start_rx(si_win_start, si_win_len);
-          rrc_log->debug("Instructed MAC to search for SIB2, win_start=%d, win_len=%d\n",
-                         si_win_start, si_win_len);
-        }
-        break;
-      default:
-        break;
-    }
-    switch(state) {
-      case RRC_STATE_IDLE:
-        if (nas->is_attached()) {
-          usleep(10000);
-          rrc_log->info("RRC IDLE: NAS is attached, re-selecting cell...\n");
-          plmn_select(selected_plmn_id);
-        }
-        break;
-      case RRC_STATE_PLMN_SELECTION:
-        plmn_select_timeout++;
-        if (plmn_select_timeout >= RRC_PLMN_SELECT_TIMEOUT) {
-          rrc_log->info("RRC PLMN Search: timeout expired. Searching again\n");
-          phy->cell_search_stop();
-          sleep(1);
-          rrc_log->console("\nRRC PLMN Search: timeout expired. Searching again\n");
-          plmn_select_timeout = 0;
-          phy->cell_search_start();
-        }
-        break;
-      case RRC_STATE_CELL_SELECTING:
-        if (phy->sync_status()) {
-          if (!current_cell->has_valid_sib1) {
-            si_acquire_state = SI_ACQUIRE_SIB1;
-          } else if (!current_cell->has_valid_sib2) {
-            si_acquire_state = SI_ACQUIRE_SIB2;
-          } else {
-            apply_sib2_configs(&current_cell->sib2);
-            si_acquire_state = SI_ACQUIRE_IDLE;
-            state = RRC_STATE_CELL_SELECTED;
-          }
-        }
-        select_cell_timeout++;
-        if (select_cell_timeout >= RRC_SELECT_CELL_TIMEOUT) {
-          rrc_log->info("RRC Cell Selecting: timeout expired. Starting Cell Search...\n");
-          state = RRC_STATE_PLMN_SELECTION;
-          plmn_select_timeout = 0;
-          phy->cell_search_start();
-        }
-        break;
-      case RRC_STATE_CELL_SELECTED:
-        rrc_log->info("RRC Cell Selected: Sending connection request...\n");
-        send_con_request();
-        state = RRC_STATE_CONNECTING;
-        connecting_timeout = 0;
-        break;
-      case RRC_STATE_CONNECTING:
-        connecting_timeout++;
-        if (connecting_timeout >= RRC_CONNECTING_TIMEOUT) {
-          // Select another cell
-          rrc_log->info("RRC Connecting: timeout expired. Selecting next cell\n");
-          state = RRC_STATE_CELL_SELECTING;
-        }
-        break;
-      case RRC_STATE_CONNECTED:
-        // Take measurements, cell reselection, etc
-        break;
-      default:
-        break;
-    }
-    usleep(1000);
-  }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-/*******************************************************************************
-NAS interface
-*******************************************************************************/
-
-void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
-  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", get_rb_name(lcid).c_str());
-  switch (state) {
-    case RRC_STATE_CONNECTING:
-      send_con_setup_complete(sdu);
-      break;
-    case RRC_STATE_CONNECTED:
-      send_ul_info_transfer(lcid, sdu);
-      break;
-    default:
-      rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
-      break;
-  }
-}
-
-
-/*******************************************************************************
-MAC interface
-*******************************************************************************/
-/* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
-void rrc::release_pucch_srs() {
-  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
-  set_phy_default_pucch_srs();
-
-  // Configure RX signals without pregeneration because default option is release
-  phy->configure_ul_params(true);
-}
-
-void rrc::ra_problem() {
-  radio_link_failure();
-}
-
-/*******************************************************************************
-GW interface
-*******************************************************************************/
-
-bool rrc::is_connected() {
-  return (RRC_STATE_CONNECTED == state);
-}
-
-bool rrc::have_drb() {
-  return drb_up;
-}
-
-/*******************************************************************************
-PDCP interface
-*******************************************************************************/
-
-void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
-  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", get_rb_name(lcid).c_str());
-  rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
-
-  switch (lcid) {
-    case RB_ID_SRB0:
-      parse_dl_ccch(pdu);
-      break;
-    case RB_ID_SRB1:
-    case RB_ID_SRB2:
-      parse_dl_dcch(lcid, pdu);
-      break;
-    default:
-      rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
-      break;
-  }
-}
-
-void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
-  if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
-    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
-    rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
-    rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
-
-    LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
-    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
-    bit_buf.N_bits = pdu->N_bytes * 8;
-    pool->deallocate(pdu);
-    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
-
-    if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
-      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
-    }
-
-    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
-    if (!nas->get_s_tmsi(&s_tmsi)) {
-      rrc_log->info("No S-TMSI present in NAS\n");
-      return;
-    }
-
-    LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
-    for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
-      s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
-      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
-                    pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
-                       pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
-      if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
-        rrc_log->info("S-TMSI match in paging message\n");
-        rrc_log->console("S-TMSI match in paging message\n");
-        mac->pcch_stop_rx();
-        if (RRC_STATE_IDLE == state) {
-          rrc_log->info("RRC in IDLE state - sending connection request.\n");
-          state = RRC_STATE_CONNECTING;
-          send_con_request();
-        }
-      }
+    if (n311_cnt == N311) {
+      mac_timers->get(t310)->stop();
+      n311_cnt = 0;
+      rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
     }
   }
 }
 
-/*******************************************************************************
-RLC interface
-*******************************************************************************/
+/* Detection of radio link failure (5.3.11.3)
+ * Upon T310 expiry, RA problem or RLC max retx
+ */
+void rrc::radio_link_failure() {
+  // TODO: Generate and store failure report
+
+  rrc_log->warning("Detected Radio-Link Failure\n");
+  rrc_log->console("Warning: Detected Radio-Link Failure\n");
+  if (state != RRC_STATE_CONNECTED) {
+    state = RRC_STATE_LEAVE_CONNECTED;
+  } else {
+    send_con_restablish_request();
+  }
+}
+
+/* Reception of PUCCH/SRS release procedure (Section 5.3.13) */
+void rrc::release_pucch_srs() {
+  // Apply default configuration for PUCCH (CQI and SR) and SRS (release)
+  set_phy_default_pucch_srs();
+
+  // Configure RX signals without pregeneration because default option is release
+  phy->configure_ul_params(true);
+}
+
+void rrc::ra_problem() {
+  radio_link_failure();
+}
 
 void rrc::max_retx_attempted() {
   //TODO: Handle the radio link failure
   rrc_log->warning("Max RLC reTx attempted\n");
-  //radio_link_failure();
+  radio_link_failure();
+}
+
+void rrc::timer_expired(uint32_t timeout_id) {
+  if (timeout_id == t310) {
+    rrc_log->info("Timer T310 expired: Radio Link Failure\n");
+    radio_link_failure();
+  } else if (timeout_id == t311) {
+    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
+    state = RRC_STATE_LEAVE_CONNECTED;
+  } else if (timeout_id == t301) {
+    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
+    state = RRC_STATE_LEAVE_CONNECTED;
+  } else {
+    rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
+  }
 }
 
+
+
+
+
+
+
+
 /*******************************************************************************
-Senders
+*
+*
+*
+* Connection Control: Establishment, Reconfiguration, Reestablishment and Release
+*
+*
+*
 *******************************************************************************/
 
 void rrc::send_con_request() {
@@ -879,93 +813,246 @@ void rrc::send_rrc_con_reconfig_complete(uint32_t lcid, byte_buffer_t *pdu) {
   pdcp->write_sdu(lcid, pdu);
 }
 
-void rrc::enable_capabilities() {
-  bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
-  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
-  phy->set_config_64qam_en(enable_ul_64);
+
+void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
+                                  byte_buffer_t *pdu) {
+  uint32_t i;
+
+  if (reconfig->rr_cnfg_ded_present) {
+    apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
+  } else {
+    printf("received con reconfig no rr confg present\n");
+  }
+  if (reconfig->meas_cnfg_present) {
+    //TODO: handle meas_cnfg
+  }
+  if (reconfig->mob_ctrl_info_present) {
+    //TODO: handle mob_ctrl_info
+  }
+
+  send_rrc_con_reconfig_complete(lcid, pdu);
+
+  byte_buffer_t *nas_sdu;
+  for (i = 0; i < reconfig->N_ded_info_nas; i++) {
+    nas_sdu = pool_allocate;;
+    memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
+    nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
+    nas->write_pdu(lcid, nas_sdu);
+  }
 }
 
-void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
-  rrc_log->debug("Preparing UE Capability Info\n");
-  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
+  /* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
+void rrc::rrc_connection_release() {
+  // Save idleModeMobilityControlInfo, etc.
+  state = RRC_STATE_LEAVE_CONNECTED;
+  rrc_log->console("Received RRC Connection Release\n");
+}
 
-  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
-  ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
 
-  LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
-  info->N_ue_caps = 1;
-  info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
 
-  LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
-  cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
-  cap->ue_category = ue_category;
 
-  cap->pdcp_params.max_rohc_ctxts_present = false;
-  cap->pdcp_params.supported_rohc_profiles[0] = false;
-  cap->pdcp_params.supported_rohc_profiles[1] = false;
-  cap->pdcp_params.supported_rohc_profiles[2] = false;
-  cap->pdcp_params.supported_rohc_profiles[3] = false;
-  cap->pdcp_params.supported_rohc_profiles[4] = false;
-  cap->pdcp_params.supported_rohc_profiles[5] = false;
-  cap->pdcp_params.supported_rohc_profiles[6] = false;
-  cap->pdcp_params.supported_rohc_profiles[7] = false;
-  cap->pdcp_params.supported_rohc_profiles[8] = false;
 
-  cap->phy_params.specific_ref_sigs_supported = false;
-  cap->phy_params.tx_antenna_selection_supported = false;
 
-  //TODO: Generate this from user input?
-  cap->rf_params.N_supported_band_eutras = 3;
-  cap->rf_params.supported_band_eutra[0].band_eutra = 3;
-  cap->rf_params.supported_band_eutra[0].half_duplex = false;
-  cap->rf_params.supported_band_eutra[1].band_eutra = 7;
-  cap->rf_params.supported_band_eutra[1].half_duplex = false;
-  cap->rf_params.supported_band_eutra[2].band_eutra = 20;
-  cap->rf_params.supported_band_eutra[2].half_duplex = false;
 
-  cap->meas_params.N_band_list_eutra = 3;
-  cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
-  cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
-  cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
-  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
 
-  cap->feature_group_indicator_present = true;
-  cap->feature_group_indicator = 0x62001000;
-  cap->inter_rat_params.utra_fdd_present = false;
-  cap->inter_rat_params.utra_tdd128_present = false;
-  cap->inter_rat_params.utra_tdd384_present = false;
-  cap->inter_rat_params.utra_tdd768_present = false;
-  cap->inter_rat_params.geran_present = false;
-  cap->inter_rat_params.cdma2000_hrpd_present = false;
-  cap->inter_rat_params.cdma2000_1xrtt_present = false;
+/*******************************************************************************
+*
+*
+*
+* Reception of Broadcast messages (MIB and SIBs)
+*
+*
+*
+*******************************************************************************/
+void rrc::write_pdu_bcch_bch(byte_buffer_t *pdu) {
+  pool->deallocate(pdu);
+  if (state == RRC_STATE_PLMN_SELECTION) {
+    // Do we need to do something with BCH?
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH BCH message received.");
+  } else {
+    rrc_log->warning("Received BCCH BCH in incorrect state\n");
+  }
+}
+
+void rrc::write_pdu_bcch_dlsch(byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "BCCH DLSCH message received.");
+  rrc_log->info("BCCH DLSCH message Stack latency: %ld us\n", pdu->get_latency_us());
+  LIBLTE_RRC_BCCH_DLSCH_MSG_STRUCT dlsch_msg;
+  srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+  bit_buf.N_bits = pdu->N_bytes * 8;
+  pool->deallocate(pdu);
+  liblte_rrc_unpack_bcch_dlsch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &dlsch_msg);
+
+  if (dlsch_msg.N_sibs > 0) {
+    if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1 == dlsch_msg.sibs[0].sib_type && SI_ACQUIRE_SIB1 == si_acquire_state) {
+      mac->bcch_stop_rx();
+
+      // Handle SIB1
+      memcpy(&current_cell->sib1, &dlsch_msg.sibs[0].sib.sib1, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_1_STRUCT));
+
+      rrc_log->info("SIB1 received, CellID=%d, si_window=%d, sib2_period=%d\n",
+                    current_cell->sib1.cell_id & 0xfff,
+                    liblte_rrc_si_window_length_num[current_cell->sib1.si_window_length],
+                    liblte_rrc_si_periodicity_num[current_cell->sib1.sched_info[0].si_periodicity]);
+
+
+      // Set TDD Config
+      if (current_cell->sib1.tdd) {
+        phy->set_config_tdd(&current_cell->sib1.tdd_cnfg);
+      }
+
+      current_cell->has_valid_sib1 = true;
+
+      // Send PLMN and TAC to NAS
+      std::stringstream ss;
+      for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
+        nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
+      }
+
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_SIB2;
+          break;
+        case RRC_STATE_PLMN_SELECTION:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          rrc_log->info("SI Acquisition done. Searching next cell...\n");
+          usleep(5000);
+          phy->cell_search_next();
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+      }
+
+    } else if (LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2 == dlsch_msg.sibs[0].sib_type &&
+               SI_ACQUIRE_SIB2 == si_acquire_state) {
+      mac->bcch_stop_rx();
+
+      // Handle SIB2
+      memcpy(&current_cell->sib2, &dlsch_msg.sibs[0].sib.sib2, sizeof(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT));
+      rrc_log->info("SIB2 received\n");
+
+      apply_sib2_configs(&current_cell->sib2);
+
+      current_cell->has_valid_sib2 = true;
+
+      // Jump to next state
+      switch(state) {
+        case RRC_STATE_CELL_SELECTING:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+          state = RRC_STATE_CELL_SELECTED;
+          break;
+        default:
+          si_acquire_state = SI_ACQUIRE_IDLE;
+      }
+    }
+  }
+}
+
+
+
+/*******************************************************************************
+*
+*
+*
+* Reception of Paging messages
+*
+*
+*
+*******************************************************************************/
+void rrc::write_pdu_pcch(byte_buffer_t *pdu) {
+  if (pdu->N_bytes > 0 && pdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BITS) {
+    rrc_log->info_hex(pdu->msg, pdu->N_bytes, "PCCH message received %d bytes\n", pdu->N_bytes);
+    rrc_log->info("PCCH message Stack latency: %ld us\n", pdu->get_latency_us());
+    rrc_log->console("PCCH message received %d bytes\n", pdu->N_bytes);
+
+    LIBLTE_RRC_PCCH_MSG_STRUCT pcch_msg;
+    srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
+    bit_buf.N_bits = pdu->N_bytes * 8;
+    pool->deallocate(pdu);
+    liblte_rrc_unpack_pcch_msg((LIBLTE_BIT_MSG_STRUCT *) &bit_buf, &pcch_msg);
+
+    if (pcch_msg.paging_record_list_size > LIBLTE_RRC_MAX_PAGE_REC) {
+      pcch_msg.paging_record_list_size = LIBLTE_RRC_MAX_PAGE_REC;
+    }
+
+    LIBLTE_RRC_S_TMSI_STRUCT s_tmsi;
+    if (!nas->get_s_tmsi(&s_tmsi)) {
+      rrc_log->info("No S-TMSI present in NAS\n");
+      return;
+    }
+
+    LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi_paged;
+    for (uint32_t i = 0; i < pcch_msg.paging_record_list_size; i++) {
+      s_tmsi_paged = &pcch_msg.paging_record_list[i].ue_identity.s_tmsi;
+      rrc_log->info("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                    pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+      rrc_log->console("Received paging (%d/%d) for UE 0x%x\n", i + 1, pcch_msg.paging_record_list_size,
+                       pcch_msg.paging_record_list[i].ue_identity.s_tmsi);
+      if (s_tmsi.mmec == s_tmsi_paged->mmec && s_tmsi.m_tmsi == s_tmsi_paged->m_tmsi) {
+        rrc_log->info("S-TMSI match in paging message\n");
+        rrc_log->console("S-TMSI match in paging message\n");
+        mac->pcch_stop_rx();
+        if (RRC_STATE_IDLE == state) {
+          rrc_log->info("RRC in IDLE state - sending connection request.\n");
+          state = RRC_STATE_CELL_SELECTING;
+        }
+      }
+    }
+  }
+}
+
+
+
+
+
+
+
 
-  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
 
-  // Byte align and pack the message bits for PDCP
-  if ((bit_buf.N_bits % 8) != 0) {
-    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
-      bit_buf.msg[bit_buf.N_bits + i] = 0;
-    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
-  }
-  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
-  pdu->N_bytes = bit_buf.N_bits / 8;
-  pdu->set_timestamp();
 
-  rrc_log->info("Sending UE Capability Info\n");
-  pdcp->write_sdu(lcid, pdu);
-}
 
 /*******************************************************************************
-Parsers
+*
+*
+*
+* Packet processing
+*
+*
+*
 *******************************************************************************/
+void rrc::write_sdu(uint32_t lcid, byte_buffer_t *sdu) {
+  rrc_log->info_hex(sdu->msg, sdu->N_bytes, "RX %s SDU", get_rb_name(lcid).c_str());
+  switch (state) {
+    case RRC_STATE_CONNECTING:
+      send_con_setup_complete(sdu);
+      break;
+    case RRC_STATE_CONNECTED:
+      send_ul_info_transfer(lcid, sdu);
+      break;
+    default:
+      rrc_log->error("SDU received from NAS while RRC state = %s", rrc_state_text[state]);
+      break;
+  }
+}
+
+void rrc::write_pdu(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->info_hex(pdu->msg, pdu->N_bytes, "TX %s PDU", get_rb_name(lcid).c_str());
+  rrc_log->info("TX PDU Stack latency: %ld us\n", pdu->get_latency_us());
+
+  switch (lcid) {
+    case RB_ID_SRB0:
+      parse_dl_ccch(pdu);
+      break;
+    case RB_ID_SRB1:
+    case RB_ID_SRB2:
+      parse_dl_dcch(lcid, pdu);
+      break;
+    default:
+      rrc_log->error("TX PDU with invalid bearer id: %s", lcid);
+      break;
+  }
+}
 
 void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
   srslte_bit_unpack_vector(pdu->msg, bit_buf.msg, pdu->N_bytes * 8);
@@ -996,11 +1083,11 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
       transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
       handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
       break;
+      /* Reception of RRCConnectionReestablishmentReject 5.3.7.8 */
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST_REJ:
       rrc_log->info("Connection Reestablishment Reject received\n");
       rrc_log->console("Reestablishment Reject\n");
-      usleep(50000);
-      rrc_connection_release();
+      state = RRC_STATE_LEAVE_CONNECTED;
       break;
     default:
       break;
@@ -1058,80 +1145,125 @@ void rrc::parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu) {
   }
 }
 
+
+
+
+
+
+
+
+
 /*******************************************************************************
-Timer expiration callback
+*
+*
+*
+* Capabilities Message
+*
+*
+*
 *******************************************************************************/
-void rrc::timer_expired(uint32_t timeout_id) {
-  if (timeout_id == t310) {
-    rrc_log->info("Timer T310 expired: Radio Link Failure\n");
-    radio_link_failure();
-  } else if (timeout_id == t311) {
-    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
-    rrc_connection_release();
-  } else if (timeout_id == t301) {
-    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
-    rrc_connection_release();
-  } else if (timeout_id == safe_reset_timer) {
-    reset_ue();
-  } else {
-    rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
-  }
+void rrc::enable_capabilities() {
+  bool enable_ul_64 = ue_category >= 5 && current_cell->sib2.rr_config_common_sib.pusch_cnfg.enable_64_qam;
+  rrc_log->info("%s 64QAM PUSCH\n", enable_ul_64 ? "Enabling" : "Disabling");
+  phy->set_config_64qam_en(enable_ul_64);
 }
 
-/*******************************************************************************
-Helpers
-*******************************************************************************/
+void rrc::send_rrc_ue_cap_info(uint32_t lcid, byte_buffer_t *pdu) {
+  rrc_log->debug("Preparing UE Capability Info\n");
+  LIBLTE_RRC_UL_DCCH_MSG_STRUCT ul_dcch_msg;
 
-void rrc::reset_ue() {
-  phy->reset();
-  mac->reset();
-  pdcp->reset();
-  rlc->reset();
-  mac->pcch_start_rx();
-  mac_timers->get(safe_reset_timer)->stop();
-  mac_timers->get(safe_reset_timer)->reset();
-  rrc_log->console("RRC Connection Released.\n");
-}
+  ul_dcch_msg.msg_type = LIBLTE_RRC_UL_DCCH_MSG_TYPE_UE_CAPABILITY_INFO;
+  ul_dcch_msg.msg.ue_capability_info.rrc_transaction_id = transaction_id;
 
-void rrc::rrc_connection_release() {
-  pthread_mutex_lock(&mutex);
-  drb_up = false;
-  state = RRC_STATE_IDLE;
-  phy->reset();
-  set_phy_default();
-  set_mac_default();
-  mac_timers->get(t311)->run();
-  mac_timers->get(t310)->stop();
-  mac_timers->get(t311)->stop();
-  mac_timers->get(safe_reset_timer)->stop();
-  mac_timers->get(safe_reset_timer)->reset();
-  mac_timers->get(safe_reset_timer)->run();
-  pthread_mutex_unlock(&mutex);
-}
+  LIBLTE_RRC_UE_CAPABILITY_INFORMATION_STRUCT *info = &ul_dcch_msg.msg.ue_capability_info;
+  info->N_ue_caps = 1;
+  info->ue_capability_rat[0].rat_type = LIBLTE_RRC_RAT_TYPE_EUTRA;
 
-void rrc::test_con_restablishment() {
-  printf("Testing connection Reestablishment\n");
-  send_con_restablish_request();
-}
+  LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *cap = &info->ue_capability_rat[0].eutra_capability;
+  cap->access_stratum_release = LIBLTE_RRC_ACCESS_STRATUM_RELEASE_REL8;
+  cap->ue_category = ue_category;
 
-/* Detection of radio link failure (5.3.11.3) */
-void rrc::radio_link_failure() {
-  // TODO: Generate and store failure report
+  cap->pdcp_params.max_rohc_ctxts_present = false;
+  cap->pdcp_params.supported_rohc_profiles[0] = false;
+  cap->pdcp_params.supported_rohc_profiles[1] = false;
+  cap->pdcp_params.supported_rohc_profiles[2] = false;
+  cap->pdcp_params.supported_rohc_profiles[3] = false;
+  cap->pdcp_params.supported_rohc_profiles[4] = false;
+  cap->pdcp_params.supported_rohc_profiles[5] = false;
+  cap->pdcp_params.supported_rohc_profiles[6] = false;
+  cap->pdcp_params.supported_rohc_profiles[7] = false;
+  cap->pdcp_params.supported_rohc_profiles[8] = false;
 
-  rrc_log->warning("Detected Radio-Link Failure\n");
-  rrc_log->console("Warning: Detected Radio-Link Failure\n");
-  if (state != RRC_STATE_CONNECTED) {
-    rrc_connection_release();
-  } else {
-    send_con_restablish_request();
+  cap->phy_params.specific_ref_sigs_supported = false;
+  cap->phy_params.tx_antenna_selection_supported = false;
+
+  //TODO: Generate this from user input?
+  cap->rf_params.N_supported_band_eutras = 3;
+  cap->rf_params.supported_band_eutra[0].band_eutra = 3;
+  cap->rf_params.supported_band_eutra[0].half_duplex = false;
+  cap->rf_params.supported_band_eutra[1].band_eutra = 7;
+  cap->rf_params.supported_band_eutra[1].half_duplex = false;
+  cap->rf_params.supported_band_eutra[2].band_eutra = 20;
+  cap->rf_params.supported_band_eutra[2].half_duplex = false;
+
+  cap->meas_params.N_band_list_eutra = 3;
+  cap->meas_params.band_list_eutra[0].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[0].inter_freq_need_for_gaps[2] = true;
+  cap->meas_params.band_list_eutra[1].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[1].inter_freq_need_for_gaps[2] = true;
+  cap->meas_params.band_list_eutra[2].N_inter_freq_need_for_gaps = 3;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[0] = true;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[1] = true;
+  cap->meas_params.band_list_eutra[2].inter_freq_need_for_gaps[2] = true;
+
+  cap->feature_group_indicator_present = true;
+  cap->feature_group_indicator = 0x62001000;
+  cap->inter_rat_params.utra_fdd_present = false;
+  cap->inter_rat_params.utra_tdd128_present = false;
+  cap->inter_rat_params.utra_tdd384_present = false;
+  cap->inter_rat_params.utra_tdd768_present = false;
+  cap->inter_rat_params.geran_present = false;
+  cap->inter_rat_params.cdma2000_hrpd_present = false;
+  cap->inter_rat_params.cdma2000_1xrtt_present = false;
+
+  liblte_rrc_pack_ul_dcch_msg(&ul_dcch_msg, (LIBLTE_BIT_MSG_STRUCT *) &bit_buf);
+
+  // Byte align and pack the message bits for PDCP
+  if ((bit_buf.N_bits % 8) != 0) {
+    for (uint32_t i = 0; i < 8 - (bit_buf.N_bits % 8); i++)
+      bit_buf.msg[bit_buf.N_bits + i] = 0;
+    bit_buf.N_bits += 8 - (bit_buf.N_bits % 8);
   }
+  srslte_bit_pack_vector(bit_buf.msg, pdu->msg, bit_buf.N_bits);
+  pdu->N_bytes = bit_buf.N_bits / 8;
+  pdu->set_timestamp();
+
+  rrc_log->info("Sending UE Capability Info\n");
+  pdcp->write_sdu(lcid, pdu);
 }
 
 
-// Determine SI messages scheduling as in 36.331 5.2.3 Acquisition of an SI message
-uint32_t rrc::sib_start_tti(uint32_t tti, uint32_t period, uint32_t x) {
-  return (period * 10 * (1 + tti / (period * 10)) + x) % 10240; // the 1 means next opportunity
-}
+
+
+
+
+
+
+
+
+/*******************************************************************************
+*
+*
+*
+* PHY and MAC Radio Resource configuration
+*
+*
+*
+*******************************************************************************/
 
 void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
 
@@ -1453,33 +1585,6 @@ void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup)
 }
 
 
-void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig,
-                                  byte_buffer_t *pdu) {
-  uint32_t i;
-
-  if (reconfig->rr_cnfg_ded_present) {
-    apply_rr_config_dedicated(&reconfig->rr_cnfg_ded);
-  } else {
-    printf("received con reconfig no rr confg present\n");
-  }
-  if (reconfig->meas_cnfg_present) {
-    //TODO: handle meas_cnfg
-  }
-  if (reconfig->mob_ctrl_info_present) {
-    //TODO: handle mob_ctrl_info
-  }
-
-  send_rrc_con_reconfig_complete(lcid, pdu);
-
-  byte_buffer_t *nas_sdu;
-  for (i = 0; i < reconfig->N_ded_info_nas; i++) {
-    nas_sdu = pool_allocate;;
-    memcpy(nas_sdu->msg, &reconfig->ded_info_nas_list[i].msg, reconfig->ded_info_nas_list[i].N_bytes);
-    nas_sdu->N_bytes = reconfig->ded_info_nas_list[i].N_bytes;
-    nas->write_pdu(lcid, nas_sdu);
-  }
-}
-
 void rrc::add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg) {
   // Setup PDCP
   pdcp->add_bearer(srb_cnfg->srb_id, srslte_pdcp_config_t(true)); // Set PDCP config control flag
@@ -1588,10 +1693,6 @@ void rrc::release_drb(uint8_t lcid) {
   // TODO
 }
 
-/************************** 
-*  DEFAULT VALUES Section 9
-****************************/
-
 // PHY CONFIG DEDICATED Defaults (3GPP 36.331 v10 9.2.4)
 void rrc::set_phy_default_pucch_srs() {
 
@@ -1630,7 +1731,6 @@ void rrc::set_rrc_default() {
   N311 = 1;
   mac_timers->get(t310)->set(this, 1000);
   mac_timers->get(t311)->set(this, 1000);
-  mac_timers->get(safe_reset_timer)->set(this, 10);
 }
 
 

From c29af35158ada977f8a74b21506962878c02e37c Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 15 Sep 2017 15:59:40 +0200
Subject: [PATCH 188/269] Solved bug: when a retransmission of a succesfully
 decoded block was received

---
 lib/include/srslte/interfaces/ue_interfaces.h |  4 ++--
 lib/src/phy/phch/pdsch.c                      |  5 +++--
 lib/src/phy/phch/test/pdsch_test.c            |  4 ++++
 srsue/hdr/mac/dl_harq.h                       | 13 +++++++------
 srsue/src/mac/mac.cc                          |  4 ++--
 srsue/src/mac/proc_ra.cc                      |  8 +++++---
 srsue/src/phy/phch_worker.cc                  | 14 +++++++-------
 srsue/test/phy/ue_itf_test_prach.cc           |  4 ++--
 srsue/test/phy/ue_itf_test_sib1.cc            |  4 ++--
 9 files changed, 34 insertions(+), 26 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index c15a09804..03edd0b88 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -303,11 +303,11 @@ public:
   } mac_grant_t; 
   
   typedef struct {
-    bool                    decode_enabled;
+    bool                    decode_enabled[SRSLTE_MAX_TB];
     int                     rv[SRSLTE_MAX_TB];
     uint16_t                rnti; 
     bool                    generate_ack; 
-    bool                    default_ack; 
+    bool                    default_ack[SRSLTE_MAX_TB];
     // If non-null, called after tb_decoded_ok to determine if ack needs to be sent
     bool                  (*generate_ack_callback)(void*); 
     void                   *generate_ack_callback_arg;
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 0908793db..16f72a37e 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -613,7 +613,7 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
       ret = SRSLTE_SUCCESS;
     }
   } else {
-    ERROR("Detected NULL pointer");
+    ERROR("Detected NULL pointer in TB%d &softbuffer=%p &data=%p &ack=%p", codeword_idx, softbuffer, (void*)data, ack);
   }
 
   return ret;
@@ -689,7 +689,8 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
 
     // Codeword decoding
     for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
-      if (cfg->grant.tb_en[tb]) {
+      /* Decode only if transport block is enabled and the default ACK is not true */
+      if (cfg->grant.tb_en[tb] && !acks[tb]) {
         int ret = srslte_pdsch_codeword_decode(q, cfg, softbuffers[tb], rnti, data[tb], tb, &acks[tb]);
 
         /* Check if there has been any execution error */
diff --git a/lib/src/phy/phch/test/pdsch_test.c b/lib/src/phy/phch/test/pdsch_test.c
index c5b55b48d..0c98079b9 100644
--- a/lib/src/phy/phch/test/pdsch_test.c
+++ b/lib/src/phy/phch/test/pdsch_test.c
@@ -460,6 +460,10 @@ int main(int argc, char **argv) {
         srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
       }
     }
+
+    /* Set ACKs to zero, otherwise will not decode if there are positive ACKs*/
+    bzero(acks, sizeof(acks));
+
     r = srslte_pdsch_decode(&pdsch_rx, &pdsch_cfg, softbuffers_rx, rx_slot_symbols, ce, 0, rnti, data_rx, acks);
   }
   gettimeofday(&t[2], NULL);
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index e897ac76f..7c6dbac1e 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -168,12 +168,14 @@ private:
     void new_grant_dl(Tgrant grant, Taction *action) {
       /* Fill action structure */
       bzero(action, sizeof(Taction));
-      action->default_ack = false;
       action->generate_ack = true;
-      action->decode_enabled = false;
+      action->rnti = grant.rnti;
 
       /* For each subprocess... */
       for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
+        action->default_ack[tb] = false;
+        action->decode_enabled[tb] = false;
+        action->phy_grant.dl.tb_en[tb] = grant.tb_en[tb];
         if (grant.tb_en[tb]) {
           subproc[tb].new_grant_dl(grant, action);
         }
@@ -258,20 +260,19 @@ private:
                                                                        cur_grant.n_bytes[tid]);
           action->payload_ptr[tid] = payload_buffer_ptr;
           if (!action->payload_ptr) {
-            action->decode_enabled = false;
+            action->decode_enabled[tid] = false;
             Error("Can't get a buffer for TBS=%d\n", cur_grant.n_bytes[tid]);
             return;
           }
-          action->decode_enabled = true;
+          action->decode_enabled[tid]= true;
           action->rv[tid] = cur_grant.rv[tid];
-          action->rnti = cur_grant.rnti;
           action->softbuffers[tid] = &softbuffer;
           memcpy(&action->phy_grant, &cur_grant.phy_grant, sizeof(Tphygrant));
           n_retx++;
 
         } else {
+          action->default_ack[tid] = true;
           Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
-          action->phy_grant.dl.tb_en[tid] = false;
         }
 
         if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index f28e43553..5c0a00b4a 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -268,7 +268,7 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
 
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
     action->generate_ack = false; 
-    action->decode_enabled = true; 
+    action->decode_enabled[0] = true;
     srslte_softbuffer_rx_reset_cb(&pch_softbuffer, 1);
     action->payload_ptr[0] = pch_payload_buffer;
     action->softbuffers[0]  = &pch_softbuffer;
@@ -276,7 +276,7 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
     action->rv[0]   = grant.rv[0];
     if (grant.n_bytes[0] > pch_payload_buffer_sz) {
       Error("Received grant for PCH (%d bytes) exceeds buffer (%d bytes)\n", grant.n_bytes[0], pch_payload_buffer_sz);
-      action->decode_enabled = false; 
+      action->decode_enabled[0] = false;
     }
   } else {
     // If PDCCH for C-RNTI and RA procedure in Contention Resolution, notify it
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 28a2b47e3..3c5adacd9 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -275,8 +275,9 @@ void ra_proc::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_p
 {
   if (grant.n_bytes[0] < MAX_RAR_PDU_LEN) {
     rDebug("DL grant found RA-RNTI=%d\n", ra_rnti);        
-    action->decode_enabled = true; 
-    action->default_ack = false; 
+    action->decode_enabled[0] = true;
+    action->decode_enabled[1] = false;
+    action->default_ack[0] = false;
     action->generate_ack = false; 
     action->payload_ptr[0] = rar_pdu_buffer;
     action->rnti = grant.rnti; 
@@ -290,7 +291,8 @@ void ra_proc::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_p
     }
   } else {
     rError("Received RAR grant exceeds buffer length (%d>%d)\n", grant.n_bytes[0], MAX_RAR_PDU_LEN);
-    action->decode_enabled = false; 
+    action->decode_enabled[0] = false;
+    action->decode_enabled[1] = false;
     state = RESPONSE_ERROR;
   }
 }
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 0290c0745..2f99c2699 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -230,18 +230,18 @@ void phch_worker::work_imp()
 
       /* Set DL ACKs to default */
       for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
-        dl_ack[tb] = dl_action.default_ack;
+        dl_ack[tb] = dl_action.default_ack[tb];
       }
 
       /* Decode PDSCH if instructed to do so */
-      if (dl_action.decode_enabled) {
+      if (dl_action.decode_enabled[0] || dl_action.decode_enabled[1]) {
         decode_pdsch(&dl_action.phy_grant.dl, dl_action.payload_ptr,
                       dl_action.softbuffers, dl_action.rv, dl_action.rnti,
                       dl_mac_grant.pid, dl_ack);
       }
-      if (dl_action.generate_ack_callback && dl_action.decode_enabled) {
+      if (dl_action.generate_ack_callback) {
         for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
-          if (dl_mac_grant.tb_en[tb]) {
+          if (dl_action.decode_enabled[tb]) {
             phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
             dl_ack[tb] = dl_action.generate_ack_callback(dl_action.generate_ack_callback_arg);
             Debug("Calling generate ACK callback for TB %d returned=%d\n", tb, dl_ack[tb]);
@@ -332,12 +332,12 @@ void phch_worker::work_imp()
   
   phy->worker_end(tx_tti, signal_ready, signal_buffer[0], SRSLTE_SF_LEN_PRB(cell.nof_prb), tx_time);
   
-  if (dl_action.decode_enabled && !dl_action.generate_ack_callback) {
-    if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH) {
+  if (!dl_action.generate_ack_callback) {
+    if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH && dl_action.decode_enabled[0]) {
       phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
     } else {
       for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
-        if (dl_mac_grant.tb_en[tb]) {
+        if (dl_action.decode_enabled[tb]) {
           phy->mac->tb_decoded(dl_ack[tb], tb, dl_mac_grant.rnti_type, dl_mac_grant.pid);
         }
       }
diff --git a/srsue/test/phy/ue_itf_test_prach.cc b/srsue/test/phy/ue_itf_test_prach.cc
index 42e04c495..810278c7c 100644
--- a/srsue/test/phy/ue_itf_test_prach.cc
+++ b/srsue/test/phy/ue_itf_test_prach.cc
@@ -277,8 +277,8 @@ public:
   }
 
   void new_grant_dl(mac_grant_t grant, tb_action_dl_t *action) {
-    action->decode_enabled = true; 
-    action->default_ack = false; 
+    action->decode_enabled[0] = true;
+    action->default_ack[0] = false;
     if (grant.rnti == 2) {
       action->generate_ack = false; 
     } else {
diff --git a/srsue/test/phy/ue_itf_test_sib1.cc b/srsue/test/phy/ue_itf_test_sib1.cc
index 90f8b5b43..23f8566be 100644
--- a/srsue/test/phy/ue_itf_test_sib1.cc
+++ b/srsue/test/phy/ue_itf_test_sib1.cc
@@ -116,8 +116,8 @@ public:
     total_dci++; 
     
 
-    action->decode_enabled = true; 
-    action->default_ack = false; 
+    action->decode_enabled[0] = true;
+    action->default_ack[0] = false;
     action->generate_ack = false;
     for (uint32_t tb = 0; tb < SRSLTE_MAX_TB; tb++) {
       action->payload_ptr[tb] = payload[tb];

From 54974c5d64bb0178596b4fb263c6dbeae90b3e89 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Fri, 15 Sep 2017 16:07:17 +0200
Subject: [PATCH 189/269] Fixed over the wire bits to 12 (MIMO only)

---
 lib/src/phy/rf/rf_uhd_imp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index b4aa23da5..ad69c0642 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -395,7 +395,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     size_t channel[4] = {0, 1, 2, 3};
     uhd_stream_args_t stream_args = {
           .cpu_format = "fc32",
-          .otw_format = "sc16",
+          .otw_format = (nof_channels > 1) ? "sc12" : "sc16",
           .args = "",
           .channel_list = channel,
           .n_channels = nof_channels,

From 0844274147e968ab7941033fb594d9e2542c409c Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 18 Sep 2017 16:19:42 +0200
Subject: [PATCH 190/269] Removed UHD decimation warnings for 2 antennas

---
 lib/src/phy/rf/rf_uhd_imp.c  | 39 +++++++++++++++++++++++++++---------
 lib/src/radio/radio.cc       |  8 ++++++--
 lib/src/radio/radio_multi.cc |  9 ++++++++-
 3 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index ad69c0642..3966ef56b 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -290,6 +290,13 @@ int rf_uhd_open(char *args, void **h)
   return rf_uhd_open_multi(args, h, 1);
 }
 
+static void remove_substring(char *s,const char *toremove)
+{
+  while((s=strstr(s,toremove))) {
+    memmove(s,s+strlen(toremove),1+strlen(s+strlen(toremove)));
+  }
+}
+
 int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
 {
   if (h) {
@@ -324,7 +331,21 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     handler->uhd_error_handler = NULL;
     
     bzero(zero_mem, sizeof(cf_t)*64*1024);
-    
+
+    enum {DEFAULT, EXTERNAL, GPSDO} clock_src;
+
+    // Set external clock reference
+    if (strstr(args, "clock=external")) {
+      remove_substring(args, "clock=external");
+      clock_src = EXTERNAL;
+    } else if (strstr(args, "clock=gpsdo")) {
+      printf("Using GPSDO clock\n");
+      remove_substring(args, "clock=gpsdo");
+      clock_src = GPSDO;
+    } else {
+      clock_src = DEFAULT;
+    }
+
     /* If device type or name not given in args, choose a B200 */
     if (args[0]=='\0') {
       if (find_string(devices_str, "type=b200") && !strstr(args, "recv_frame_size")) {
@@ -379,14 +400,12 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     }
     
     // Set external clock reference   
-    if (strstr(args, "clock=external")) {
+    if (clock_src == EXTERNAL) {
       uhd_usrp_set_clock_source(handler->usrp, "external", 0);       
-    } else if (strstr(args, "clock=gpsdo")) {
-      printf("Using GPSDO clock\n");
-      uhd_usrp_set_clock_source(handler->usrp, "gpsdo", 0);       
+    } else if (clock_src == GPSDO) {
+      uhd_usrp_set_clock_source(handler->usrp, "gpsdo", 0);
     }
 
-      
     handler->has_rssi = get_has_rssi(handler);  
     if (handler->has_rssi) {        
       uhd_sensor_value_make_from_realnum(&handler->rssi_value, "rssi", 0, "dBm", "%f");      
@@ -405,9 +424,11 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     handler->nof_tx_channels = nof_channels;
 
     /* Set default rate to avoid decimation warnings */
-    uhd_usrp_set_rx_rate(handler->usrp, 1.92e6, 0);
-    uhd_usrp_set_tx_rate(handler->usrp, 1.92e6, 0);
-    
+    for (int i=0;i<nof_channels;i++) {
+      uhd_usrp_set_rx_rate(handler->usrp, 1.92e6, i);
+      uhd_usrp_set_tx_rate(handler->usrp, 1.92e6, i);
+    }
+
     /* Initialize rx and tx stremers */
     uhd_rx_streamer_make(&handler->rx_stream);
     error = uhd_usrp_get_rx_stream(handler->usrp, &stream_args, handler->rx_stream);
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index 3aaa8cba1..6cd2d9dd3 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -62,8 +62,12 @@ bool radio::init(char *args, char *devname)
     printf("\nWarning burst preamble is not calibrated for device %s. Set a value manually\n\n", srslte_rf_name(&rf_device));
   }
 
-  strncpy(saved_args,    args, 128);
-  strncpy(saved_devname, devname, 128);
+  if (args) {
+    strncpy(saved_args, args, 128);
+  }
+  if (devname) {
+    strncpy(saved_devname, devname, 128);
+  }
 
   return true;    
 }
diff --git a/lib/src/radio/radio_multi.cc b/lib/src/radio/radio_multi.cc
index 0bb538c8d..018a675ac 100644
--- a/lib/src/radio/radio_multi.cc
+++ b/lib/src/radio/radio_multi.cc
@@ -29,7 +29,14 @@ bool radio_multi::init_multi(uint32_t nof_rx_antennas, char* args, char* devname
   } else {
     printf("\nWarning burst preamble is not calibrated for device %s. Set a value manually\n\n", srslte_rf_name(&rf_device));
   }
-  
+
+  if (args) {
+    strncpy(saved_args, args, 128);
+  }
+  if (devname) {
+    strncpy(saved_devname, devname, 128);
+  }
+
   return true;    
 }
 

From f58f74b1024852c718fabc98614e47487bc4ac2e Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 18 Sep 2017 16:45:12 +0200
Subject: [PATCH 191/269] Solved PHICH Segmentation fault for MIMO

---
 lib/include/srslte/phy/phch/phich.h     | 14 ++------------
 lib/src/phy/phch/phich.c                | 23 +++--------------------
 lib/src/phy/phch/test/phich_file_test.c |  2 +-
 lib/src/phy/phch/test/phich_test.c      | 24 ++++++++++++++----------
 lib/src/phy/ue/ue_dl.c                  |  8 +-------
 5 files changed, 21 insertions(+), 50 deletions(-)

diff --git a/lib/include/srslte/phy/phch/phich.h b/lib/include/srslte/phy/phch/phich.h
index f560fdc80..0645e22b1 100644
--- a/lib/include/srslte/phy/phch/phich.h
+++ b/lib/include/srslte/phy/phch/phich.h
@@ -101,8 +101,8 @@ SRSLTE_API void srslte_phich_calc(srslte_phich_t *q,
                                   uint32_t *nseq); 
 
 SRSLTE_API int srslte_phich_decode(srslte_phich_t *q, 
-                                   cf_t *slot_symbols, 
-                                   cf_t *ce[SRSLTE_MAX_PORTS],
+                                   cf_t *slot_symbols[SRSLTE_MAX_PORTS],
+                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                    float noise_estimate, 
                                    uint32_t ngroup, 
                                    uint32_t nseq, 
@@ -110,16 +110,6 @@ SRSLTE_API int srslte_phich_decode(srslte_phich_t *q,
                                    uint8_t *ack, 
                                    float *distance);
 
-SRSLTE_API int srslte_phich_decode_multi(srslte_phich_t *q, 
-                                         cf_t *slot_symbols[SRSLTE_MAX_PORTS], 
-                                         cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                         float noise_estimate, 
-                                         uint32_t ngroup, 
-                                         uint32_t nseq, 
-                                         uint32_t nsubframe, 
-                                         uint8_t *ack, 
-                                         float *distance);
-
 SRSLTE_API int srslte_phich_encode(srslte_phich_t *q, 
                                    uint8_t ack, 
                                    uint32_t ngroup, 
diff --git a/lib/src/phy/phch/phich.c b/lib/src/phy/phch/phich.c
index a7cb282fd..14a0d5426 100644
--- a/lib/src/phy/phch/phich.c
+++ b/lib/src/phy/phch/phich.c
@@ -173,26 +173,9 @@ void srslte_phich_ack_encode(uint8_t ack, uint8_t bits[SRSLTE_PHICH_NBITS]) {
   memset(bits, ack, 3 * sizeof(uint8_t));
 }
 
-int srslte_phich_decode(srslte_phich_t *q, cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate,
-    uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) 
-{
-  cf_t *_sf_symbols[SRSLTE_MAX_PORTS]; 
-  cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
-  _sf_symbols[0] = sf_symbols; 
-  for (int i=0;i<q->cell.nof_ports;i++) {
-    _ce[i][0] = ce[i]; 
-  }
-
-  return srslte_phich_decode_multi(q, _sf_symbols, _ce, noise_estimate, ngroup, nseq, subframe, ack, distance);
-}
-/* Decodes the phich channel and saves the CFI in the cfi pointer.
- *
- * Returns 1 if successfully decoded the CFI, 0 if not and -1 on error
- */
-int srslte_phich_decode_multi(srslte_phich_t *q, cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
-                              uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) 
-{
+int srslte_phich_decode(srslte_phich_t *q, cf_t *sf_symbols[SRSLTE_MAX_PORTS],
+                        cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
+                        uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) {
 
   /* Set pointers for layermapping & precoding */
   int i, j;
diff --git a/lib/src/phy/phch/test/phich_file_test.c b/lib/src/phy/phch/test/phich_file_test.c
index 0250c6f0b..968aa5cbc 100644
--- a/lib/src/phy/phch/test/phich_file_test.c
+++ b/lib/src/phy/phch/test/phich_file_test.c
@@ -263,7 +263,7 @@ int main(int argc, char **argv) {
   for (ngroup=0;ngroup<srslte_phich_ngroups(&phich);ngroup++) {
     for (nseq=0;nseq<max_nseq;nseq++) {
 
-      if (srslte_phich_decode(&phich, fft_buffer, ce, srslte_chest_dl_get_noise_estimate(&chest), ngroup, nseq, numsubframe, &ack_rx, &distance)<0) {
+      if (srslte_phich_decode(&phich, &fft_buffer, &ce, srslte_chest_dl_get_noise_estimate(&chest), ngroup, nseq, numsubframe, &ack_rx, &distance)<0) {
         printf("Error decoding ACK\n");
         exit(-1);
       }
diff --git a/lib/src/phy/phch/test/phich_test.c b/lib/src/phy/phch/test/phich_test.c
index 5334b210f..cffb85663 100644
--- a/lib/src/phy/phch/test/phich_test.c
+++ b/lib/src/phy/phch/test/phich_test.c
@@ -103,7 +103,7 @@ int main(int argc, char **argv) {
   srslte_phich_t phich;
   srslte_regs_t regs;
   int i, j;
-  cf_t *ce[SRSLTE_MAX_PORTS];
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   int nof_re;
   cf_t *slot_symbols[SRSLTE_MAX_PORTS];
   uint8_t ack[50][SRSLTE_PHICH_NORM_NSEQUENCES], ack_rx;
@@ -120,13 +120,15 @@ int main(int argc, char **argv) {
 
   /* init memory */
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
-    ce[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!ce[i]) {
-      perror("malloc");
-      exit(-1);
-    }
-    for (j=0;j<nof_re;j++) {
-      ce[i][j] = 1;
+    for (int k=0;k<SRSLTE_MAX_PORTS;k++) {
+      ce[k][i] = malloc(sizeof(cf_t) * nof_re);
+      if (!ce[k][i]) {
+        perror("malloc");
+        exit(-1);
+      }
+      for (j = 0; j < nof_re; j++) {
+        ce[k][i][j] = 1;
+      }
     }
     slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
     if (!slot_symbols[i]) {
@@ -185,7 +187,7 @@ int main(int argc, char **argv) {
       for (ngroup=0;ngroup<srslte_phich_ngroups(&phich);ngroup++) {
         for (nseq=0;nseq<max_nseq;nseq++) {
 
-          if (srslte_phich_decode(&phich, slot_symbols[0], ce, 0, ngroup, nseq, nsf, &ack_rx, &distance)<0) {
+          if (srslte_phich_decode(&phich, slot_symbols, ce, 0, ngroup, nseq, nsf, &ack_rx, &distance)<0) {
             printf("Error decoding ACK\n");
             exit(-1);
           }
@@ -208,7 +210,9 @@ int main(int argc, char **argv) {
   srslte_phich_free(&phich);
 
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
-    free(ce[i]);
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      free(ce[i][j]);
+    }
     free(slot_symbols[i]);
   }
   printf("OK\n");
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 46dfea566..cbeb6ce87 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -776,13 +776,7 @@ bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t n_pr
     sf_idx, n_prb_lowest, n_dmrs, ngroup, nseq, 
     srslte_phich_ngroups(&q->phich), srslte_phich_nsf(&q->phich));
   
-  cf_t *ce0[SRSLTE_MAX_PORTS];
-  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
-    ce0[i] = q->ce_m[i][0];
-  }
-
-  
-  if (!srslte_phich_decode(&q->phich, q->sf_symbols_m[0], ce0, 0, ngroup, nseq, sf_idx, &ack_bit, &distance)) {
+  if (!srslte_phich_decode(&q->phich, q->sf_symbols_m, q->ce_m, 0, ngroup, nseq, sf_idx, &ack_bit, &distance)) {
     INFO("Decoded PHICH %d with distance %f\n", ack_bit, distance);    
   } else {
     fprintf(stderr, "Error decoding PHICH\n");

From fcbcf1ec7b41bdfd6d8556dba2a48c7da15beea0 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 18 Sep 2017 16:58:28 +0200
Subject: [PATCH 192/269] Removed grant size condition from
 calc_new_transmission

---
 srsue/hdr/mac/dl_harq.h | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index 7c6dbac1e..e2f5aeab0 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -212,6 +212,7 @@ private:
           return false;
         } else {
           pid = pid_;
+          is_first_tb = true;
           is_initiated = true;
           harq_entity = parent;
           log_h = harq_entity->log_h;
@@ -220,6 +221,7 @@ private:
       }
 
       void reset(void) {
+        is_first_tb = true;
         ack = false;
         payload_buffer_ptr = NULL;
         bzero(&cur_grant, sizeof(Tgrant));
@@ -241,7 +243,12 @@ private:
           }
         }
         calc_is_new_transmission(grant);
-        if (is_new_transmission) {
+        // If this is a new transmission or the size of the TB has changed
+        if (is_new_transmission || (cur_grant.n_bytes[tid] != grant.n_bytes[tid])) {
+          if (!is_new_transmission) {
+            Warning("DL PID %d: Size of grant changed during a retransmission %d!=%d\n", pid,
+                    cur_grant.n_bytes[tid], grant.n_bytes[tid]);
+          }
           ack = false;
           srslte_softbuffer_rx_reset_tbs(&softbuffer, cur_grant.n_bytes[tid] * 8);
           n_retx = 0;
@@ -337,17 +344,14 @@ private:
       int get_current_tbs(void) { return cur_grant.n_bytes[tid] * 8; }
 
     private:
+      // Determine if it's a new transmission 5.3.2.2
       bool calc_is_new_transmission(Tgrant grant) {
-        bool is_new_tb = true;
-        if ((srslte_tti_interval(grant.tti, cur_grant.tti) <= 8 && (grant.n_bytes[tid] == cur_grant.n_bytes[tid])) ||
-            pid == HARQ_BCCH_PID) {
-          is_new_tb = false;
-        }
 
-        if ((grant.ndi[tid] != cur_grant.ndi[tid] && !is_new_tb) || // NDI toggled for same TB
-            is_new_tb || // is new TB
-            (pid == HARQ_BCCH_PID && grant.rv[tid] == 0))      // Broadcast PID and 1st TX (RV=0)
+        if ((grant.ndi[tid] != cur_grant.ndi[tid])       || // 1st condition (NDI has changed)
+            (pid == HARQ_BCCH_PID && grant.rv[tid] == 0) || // 2nd condition (Broadcast and 1st transmission)
+             is_first_tb)                                   // 3rd condition (first TB)
         {
+          is_first_tb         = false;
           is_new_transmission = true;
           Debug("Set HARQ for new transmission\n");
         } else {
@@ -362,6 +366,7 @@ private:
       dl_harq_entity *harq_entity;
       srslte::log *log_h;
 
+      bool is_first_tb;
       bool is_new_transmission;
 
       uint32_t pid;     /* HARQ Proccess ID   */

From 4a869675305f2f55a78227ce2af9975bc883e91e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 18 Sep 2017 18:34:33 +0200
Subject: [PATCH 193/269] Improved stability after radio link failure and radio
 transport error

---
 lib/include/srslte/interfaces/ue_interfaces.h |  2 +
 srsue/hdr/phy/phch_recv.h                     |  9 +-
 srsue/hdr/phy/phy.h                           |  1 +
 srsue/hdr/upper/nas.h                         |  1 +
 srsue/src/phy/phch_recv.cc                    | 95 +++++++++++++------
 srsue/src/phy/phy.cc                          |  4 +
 srsue/src/upper/gw.cc                         | 41 +++++---
 srsue/src/upper/nas.cc                        |  4 +
 srsue/src/upper/rrc.cc                        | 15 ++-
 9 files changed, 124 insertions(+), 48 deletions(-)

diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 03edd0b88..2c29bd4d4 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -98,6 +98,7 @@ class nas_interface_rrc
 {
 public:
   virtual bool      is_attached() = 0;
+  virtual bool      is_attaching() = 0;
   virtual void      notify_connection_setup() = 0;
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
@@ -532,6 +533,7 @@ public:
 
   /* Is the PHY downlink synchronized? */
   virtual bool sync_status() = 0;
+  virtual void sync_reset()  = 0;
 
   /* Configure UL using parameters written with set_param() */
   virtual void configure_ul_params(bool pregen_disabled = false) = 0;
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 82abfd304..ac033f607 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -54,7 +54,7 @@ public:
 
   void    set_earfcn(std::vector<uint32_t> earfcn);
 
-  bool    stop_sync();
+  void    reset_sync();
   void    cell_search_start();
   void    cell_search_stop();
   void    cell_search_next(bool reset = false);
@@ -73,12 +73,16 @@ private:
 
   std::vector<uint32_t> earfcn;
 
+  void   reset();
+  void   radio_error();
+  bool   wait_radio_reset();
   void   set_ue_sync_opts(srslte_ue_sync_t *q); 
   void   run_thread();
 
   void   set_sampling_rate();
   bool   set_frequency();
   void   resync_sfn();
+  bool   stop_sync();
 
   void   cell_search_inc();
 
@@ -89,6 +93,8 @@ private:
   void   start_rx();
   bool   radio_is_rx;
 
+  bool   radio_is_resetting;
+
   bool   running; 
   
   srslte::radio_multi  *radio_h;
@@ -152,7 +158,6 @@ private:
   float    measure_rsrp;
   srslte_ue_dl_t ue_dl_measure;
 
-
   const static int RSRP_MEASURE_NOF_FRAMES = 5;
 
   int    cell_sync_sfn();
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index 1d500e7f7..2df93b07b 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -79,6 +79,7 @@ public:
 
   /********** RRC INTERFACE ********************/
   void    reset();
+  void    sync_reset();
   void    configure_ul_params(bool pregen_disabled = false);
   void    cell_search_start();
   void    cell_search_stop();
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 1aa0ab3d6..68d00ba06 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -86,6 +86,7 @@ public:
   uint32_t get_ul_count();
 
   bool is_attached();
+  bool is_attaching();
 
   bool get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi);
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 25bfb89b2..eac26d9ec 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -79,15 +79,7 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
   prach_buffer = _prach_buffer;
   nof_rx_antennas = nof_rx_antennas_;
 
-  tx_mutex_cnt = 0;
-  running = true;
-  phy_state = IDLE;
-  time_adv_sec = 0;
-  cell_is_set = false;
-  sync_sfn_cnt = 0;
-  srate_mode = SRATE_NONE;
-  cell_search_in_progress = false;
-  current_earfcn = 0;
+  reset();
 
   for (uint32_t i = 0; i < nof_rx_antennas; i++) {
     sf_buffer[i] = (cf_t *) srslte_vec_malloc(sizeof(cf_t) * 3 * SRSLTE_SF_LEN_PRB(100));
@@ -157,6 +149,37 @@ void phch_recv::stop() {
   wait_thread_finish();
 }
 
+void phch_recv::reset() {
+  tx_mutex_cnt = 0;
+  running = true;
+  phy_state = IDLE;
+  time_adv_sec = 0;
+  cell_is_set = false;
+  sync_sfn_cnt = 0;
+  srate_mode = SRATE_NONE;
+  cell_search_in_progress = false;
+  current_earfcn = 0;
+  radio_is_resetting = false;
+}
+
+void phch_recv::radio_error() {
+  log_h->error("SYNC:  Receiving from radio.\n");
+  phy_state = IDLE;
+  radio_is_resetting=true;
+  radio_h->reset();
+  reset();
+  radio_is_resetting=false;
+}
+
+bool phch_recv::wait_radio_reset() {
+  int cnt=0;
+  while(cnt < 20 && radio_is_resetting) {
+    sleep(1);
+    cnt++;
+  }
+  return radio_is_resetting;
+}
+
 void phch_recv::set_agc_enable(bool enable) {
   do_agc = enable;
 }
@@ -394,6 +417,9 @@ int phch_recv::cell_meas_rsrp() {
 }
 
 void phch_recv::resync_sfn() {
+
+  wait_radio_reset();
+
   stop_rx();
   start_rx();
   srslte_ue_mib_reset(&ue_mib);
@@ -407,6 +433,9 @@ void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
 }
 
 bool phch_recv::stop_sync() {
+
+  wait_radio_reset();
+
   if (phy_state == IDLE && is_in_idle) {
     return true;
   } else {
@@ -421,6 +450,16 @@ bool phch_recv::stop_sync() {
   }
 }
 
+void phch_recv::reset_sync() {
+
+  wait_radio_reset();
+
+  Info("SYNC:  Resetting sync\n");
+  srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
+  srslte_ue_sync_reset(&ue_sync);
+  resync_sfn();
+}
+
 void phch_recv::cell_search_inc()
 {
   cur_earfcn_index++;
@@ -540,17 +579,20 @@ bool phch_recv::set_frequency()
 void phch_recv::set_sampling_rate()
 {
   float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+  if (srate != -1) {
+    Info("SYNC:  Setting sampling rate %.2f MHz\n", srate/1000000);
 
-  Info("SYNC:  Setting sampling rate %.2f MHz\n", srate/1000000);
-
-  if (30720 % ((int) srate / 1000) == 0) {
-    radio_h->set_master_clock_rate(30.72e6);
+    if (30720 % ((int) srate / 1000) == 0) {
+      radio_h->set_master_clock_rate(30.72e6);
+    } else {
+      radio_h->set_master_clock_rate(23.04e6);
+    }
+    srate_mode = SRATE_CAMP;
+    radio_h->set_rx_srate(srate);
+    radio_h->set_tx_srate(srate);
   } else {
-    radio_h->set_master_clock_rate(23.04e6);
+    Error("Error setting sampling rate for cell with %d PRBs\n", cell.nof_prb);
   }
-  srate_mode = SRATE_CAMP;
-  radio_h->set_rx_srate(srate);
-  radio_h->set_tx_srate(srate);
 }
 
 void phch_recv::run_thread() {
@@ -585,9 +627,8 @@ void phch_recv::run_thread() {
             }
             break;
           default:
-            log_h->error("SYNC:  Receiving frorm radio.\n");
-            phy_state = IDLE;
-            radio_h->reset();
+            radio_error();
+            break;
           }
         }
         break;
@@ -610,9 +651,7 @@ void phch_recv::run_thread() {
           case 0:
             break;
           default:
-            log_h->error("SYNC:  Receiving frorm radio.\n");
-            phy_state = IDLE;
-            radio_h->reset();
+            radio_error();
             break;
         }
         sync_sfn_cnt++;
@@ -632,9 +671,8 @@ void phch_recv::run_thread() {
           case 0:
             break;
           default:
-            log_h->error("SYNC:  Receiving frorm radio.\n");
-            phy_state = IDLE;
-            radio_h->reset();
+            radio_error();
+            break;
         }
         break;
       case CELL_CAMP:
@@ -694,9 +732,8 @@ void phch_recv::run_thread() {
               worker_com->reset_ul();
               break;
             default:
-              log_h->error("SYNC:  Receiving from radio.\n");
-              phy_state = IDLE;
-              radio_h->reset();
+              radio_error();
+              break;
           }
         } else {
           // wait_worker() only returns NULL if it's being closed. Quit now to avoid unnecessary loops here
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 6ae745d84..afce102b3 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -242,6 +242,10 @@ void phy::cell_search_next()
   sf_recv.cell_search_next();
 }
 
+void phy::sync_reset() {
+  sf_recv.reset_sync();
+}
+
 bool phy::cell_select(uint32_t earfcn, srslte_cell_t phy_cell)
 {
   return sf_recv.cell_select(earfcn, phy_cell);
diff --git a/srsue/src/upper/gw.cc b/srsue/src/upper/gw.cc
index 19c4f38d2..07ac36989 100644
--- a/srsue/src/upper/gw.cc
+++ b/srsue/src/upper/gw.cc
@@ -232,8 +232,10 @@ void gw::run_thread()
   int32           N_bytes;
   srslte::byte_buffer_t  *pdu = pool_allocate;
 
-  const static uint32_t ATTACH_TIMEOUT_MS = 2000;
+  const static uint32_t ATTACH_TIMEOUT_MS   = 10000;
+  const static uint32_t ATTACH_MAX_ATTEMPTS = 3;
   uint32_t attach_cnt = 0;
+  uint32_t attach_attempts = 0;
 
   gw_log->info("GW IP packet receiver thread run_enable\n");
 
@@ -260,10 +262,11 @@ void gw::run_thread()
         {
           gw_log->info_hex(pdu->msg, pdu->N_bytes, "TX PDU");
 
-          while(run_enable && !pdcp->is_drb_enabled(lcid)) {
+          while(run_enable && !pdcp->is_drb_enabled(lcid) && attach_attempts < ATTACH_MAX_ATTEMPTS) {
             if (attach_cnt == 0) {
-              gw_log->info("LCID=%d not active, requesting NAS attach\n", lcid);
+              gw_log->info("LCID=%d not active, requesting NAS attach (%d/%d)\n", lcid, attach_attempts, ATTACH_MAX_ATTEMPTS);
               nas->attach_request();
+              attach_attempts++;
             }
             attach_cnt++;
             if (attach_cnt == ATTACH_TIMEOUT_MS) {
@@ -271,6 +274,12 @@ void gw::run_thread()
             }
             usleep(1000);
           }
+
+          if (attach_attempts == ATTACH_MAX_ATTEMPTS) {
+            gw_log->warning("LCID=%d was not active after %d attempts\n", lcid, ATTACH_MAX_ATTEMPTS);
+          }
+
+          attach_attempts = 0;
           attach_cnt = 0;
 
           if (!run_enable) {
@@ -278,18 +287,20 @@ void gw::run_thread()
           }
 
           // Send PDU directly to PDCP
-          pdu->set_timestamp();
-          ul_tput_bytes += pdu->N_bytes;
-          pdcp->write_sdu(lcid, pdu);
-
-          do {
-            pdu = pool_allocate;
-            if (!pdu) {
-              printf("Not enough buffers in pool\n");
-              usleep(100000);
-            }
-          } while(!pdu);
-          idx = 0;
+          if (pdcp->is_drb_enabled(lcid)) {
+            pdu->set_timestamp();
+            ul_tput_bytes += pdu->N_bytes;
+            pdcp->write_sdu(lcid, pdu);
+
+            do {
+              pdu = pool_allocate;
+              if (!pdu) {
+                printf("Not enough buffers in pool\n");
+                usleep(100000);
+              }
+            } while(!pdu);
+            idx = 0;
+          }
         }else{
           idx += N_bytes;
         }
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index f7f0598d3..91b35ce01 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -121,6 +121,10 @@ bool nas::is_attached() {
   return state == EMM_STATE_REGISTERED;
 }
 
+bool nas::is_attaching() {
+  return state == EMM_STATE_REGISTERED_INITIATED;
+}
+
 void nas::notify_connection_setup() {
   nas_log->debug("State = %s\n", emm_state_text[state]);
   if (EMM_STATE_REGISTERED_INITIATED == state) {
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index a55148c59..8f064dd3a 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -154,13 +154,22 @@ void rrc::run_thread() {
       case RRC_STATE_IDLE:
         // If camping on the cell, it will receive SI and paging from PLMN
         if (phy->sync_status()) {
-
+          // If attempting to attach, reselect cell
+          if (nas->is_attaching()) {
+            sleep(1);
+            rrc_log->info("RRC IDLE: NAS is attaching and camping on cell, reselecting...\n");
+            plmn_select(selected_plmn_id);
+          }
         // If not camping on a cell
         } else {
           // If NAS is attached, perform cell reselection on current PLMN
           if (nas->is_attached()) {
             rrc_log->info("RRC IDLE: NAS is attached, PHY not synchronized. Re-selecting cell...\n");
             plmn_select(selected_plmn_id);
+          } else if (nas->is_attaching()) {
+            sleep(1);
+            rrc_log->info("RRC IDLE: NAS is attaching, searching again PLMN\n");
+            plmn_search();
           }
           // If not attached, PLMN selection will be triggered from higher layers
         }
@@ -358,6 +367,7 @@ void rrc::plmn_select(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id) {
     if (phy->sync_status() && selected_plmn_id.mcc == plmn_id.mcc && selected_plmn_id.mnc == plmn_id.mnc) {
       rrc_log->info("Already camping on selected PLMN, connecting...\n");
       state = RRC_STATE_CELL_SELECTING;
+      select_cell_timeout = 0;
     } else {
       rrc_log->info("PLMN %s selected\n", plmn_id_to_c_str(plmn_id).c_str());
       // Sort cells according to RSRP
@@ -495,6 +505,7 @@ void rrc::in_sync() {
 void rrc::radio_link_failure() {
   // TODO: Generate and store failure report
 
+  phy->sync_reset();
   rrc_log->warning("Detected Radio-Link Failure\n");
   rrc_log->console("Warning: Detected Radio-Link Failure\n");
   if (state != RRC_STATE_CONNECTED) {
@@ -681,7 +692,7 @@ void rrc::send_con_restablish_request() {
   rrc_log->debug("Setting UE contention resolution ID: %d\n", uecri);
   mac->set_contention_id(uecri);
 
-  rrc_log->info("Sending RRC Connection Resetablishment Request on SRB0\n");
+  rrc_log->info("Sending RRC Connection Reestablishment Request on SRB0\n");
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
 

From 336a6b621796575171366a005bb744121d00bc8f Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 19 Sep 2017 10:32:29 +0200
Subject: [PATCH 194/269] Increased compatibility for TM3 and TM4 with one Rx
 antenna

---
 lib/src/phy/mimo/precoding.c |  4 +--
 lib/src/phy/phch/pdsch.c     | 19 +++++++++---
 lib/src/phy/ue/ue_dl.c       | 60 +++++++++++++++++-------------------
 srsue/src/phy/phch_worker.cc | 33 +++++++++++++++-----
 4 files changed, 71 insertions(+), 45 deletions(-)

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index bf9047977..6f3e43889 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -1367,7 +1367,7 @@ int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_P
                                  int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols,
                                  float noise_estimate)
 {
-  if (nof_ports == 2 && nof_rxant == 2) {
+  if (nof_ports == 2 && nof_rxant <= 2) {
     if (nof_layers == 2) {
       switch (mimo_decoder) {
         case SRSLTE_MIMO_DECODER_ZF:
@@ -1407,7 +1407,7 @@ int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_P
   } else if (nof_ports == 4) {
     ERROR("Error predecoding multiplex: not implemented for %d Tx ports", nof_ports);
   } else {
-    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennas %d\n", nof_ports, nof_rxant);
   }
   return SRSLTE_ERROR;
 }
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 16f72a37e..de553630e 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -246,7 +246,7 @@ static int pdsch_init(srslte_pdsch_t *q, uint32_t max_prb, bool is_ue, uint32_t
         goto clean;
       }
       if (q->is_ue) {
-        for (int j=0;j<q->nof_rx_antennas;j++) {
+        for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
           q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
           if (!q->ce[i][j]) {
             goto clean;
@@ -309,7 +309,7 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
       free(q->symbols[i]);
     }
     if (q->is_ue) {
-      for (int j=0;j<q->nof_rx_antennas;j++) {
+      for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
         if (q->ce[i][j]) {
           free(q->ce[i][j]);
         }
@@ -713,8 +713,9 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
                                   uint32_t pmi[SRSLTE_MAX_LAYERS], float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]) {
 
-  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
-    for (int nof_layers = 1; nof_layers <= 2; nof_layers++ ) {
+  if (q->cell.nof_ports == 2 && q->nof_rx_antennas <= 2) {
+    int nof_layers = 1;
+    for (; nof_layers <= q->nof_rx_antennas; nof_layers++ ) {
       if (sinr[nof_layers - 1] && pmi) {
         if (srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, nof_layers, &pmi[nof_layers - 1],
                                         sinr[nof_layers - 1]) < 0) {
@@ -723,6 +724,16 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
         }
       }
     }
+
+    /* FIXME: Set other layers to 0 */
+    for (; nof_layers <= SRSLTE_MAX_LAYERS; nof_layers++ ) {
+      if (sinr[nof_layers - 1] && pmi) {
+        for (int cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb++) {
+          sinr[nof_layers - 1][cb] = -INFINITY;
+        }
+        pmi[nof_layers - 1] = 0;
+      }
+    }
   } else {
     ERROR("Not implemented configuration");
     return SRSLTE_ERROR_INVALID_INPUTS;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index cbeb6ce87..fd04a4c85 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -115,7 +115,7 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     }
     srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft.symbol_sz);
     
-    for (int j=0;j<nof_rx_antennas;j++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
       q->sf_symbols_m[j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
       if (!q->sf_symbols_m[j]) {
         perror("malloc");
@@ -127,6 +127,7 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
           perror("malloc");
           goto clean_exit; 
         }
+        bzero(q->ce_m[i][j], MAX_SFLEN_RE * sizeof(cf_t));
       }
     }
     
@@ -163,7 +164,7 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
         free(q->softbuffers[i]);
       }
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
       }
@@ -516,10 +517,10 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
   float best_sinr = -INFINITY;
   uint8_t best_pmi = 0, best_ri = 0;
 
-  if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
+  if (q->cell.nof_ports < 2) {
     /* Do nothing */
     return SRSLTE_SUCCESS;
-  } else if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+  } else {
     if (srslte_pdsch_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
                                   SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), q->pmi, q->sinr)) {
       ERROR("SINR calculation error");
@@ -527,7 +528,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
     }
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
-    for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
+    for (uint32_t nof_layers = 1; nof_layers <= SRSLTE_MAX_LAYERS; nof_layers++) {
       float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers * nof_layers;
       if (_sinr > best_sinr + 0.1) {
         best_sinr = _sinr;
@@ -535,37 +536,34 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
         best_ri = (uint8_t) (nof_layers - 1);
       }
     }
+  }
 
-    /* Set RI */
-    if (ri != NULL) {
-      *ri = best_ri;
-    }
+  /* Print Trace */
+  if (ri != NULL && pmi != NULL && current_sinr != NULL) {
+    INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
+         10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+  }
 
-    /* Set PMI */
-    if (pmi != NULL) {
-      *pmi = best_pmi;
-    }
+  /* Set RI */
+  if (ri != NULL) {
+    *ri = best_ri;
+  }
 
-    /* Set current SINR */
-    if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-      if (q->pdsch_cfg.nof_layers == 1) {
-        *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
-      } else if (q->pdsch_cfg.nof_layers == 2) {
-        *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
-      } else {
-        ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
-        return SRSLTE_ERROR;
-      }
-    }
+  /* Set PMI */
+  if (pmi != NULL) {
+    *pmi = best_pmi;
+  }
 
-    /* Print Trace */
-    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
-      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
-           10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+  /* Set current SINR */
+  if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+    if (q->pdsch_cfg.nof_layers == 1) {
+      *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
+    } else if (q->pdsch_cfg.nof_layers == 2) {
+      *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
+    } else {
+      ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
+      return SRSLTE_ERROR;
     }
-  } else {
-    ERROR("Not implemented configuration");
-    return SRSLTE_ERROR_INVALID_INPUTS;
   }
 
   return SRSLTE_SUCCESS;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 2f99c2699..f1dff5f8d 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -255,9 +255,17 @@ void phch_worker::work_imp()
 
       /* Select Rank Indicator by computing Condition Number */
       if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3) {
-        float cn = 0.0f;
-        srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
-        uci_data.uci_ri_len = 1;
+        if (ue_dl.nof_rx_antennas > 1) {
+          /* If 2 ort more receiving antennas, select RI */
+          float cn = 0.0f;
+          srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
+          uci_data.uci_ri_len = 1;
+        } else {
+          /* If only one receiving antenna, force RI for 1 layer */
+          uci_data.uci_ri = 0;
+          uci_data.uci_ri_len = 1;
+          Warning("Only one receiving antenna with TM3. Forcing RI=1 layer.\n");
+        }
       } else if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4){
         float sinr = 0.0f;
         uint8 packed_pmi = 0;
@@ -271,6 +279,11 @@ void phch_worker::work_imp()
           uci_data.uci_pmi_len = 1;
           uci_data.uci_dif_cqi_len = 3;
         }
+
+        /* If only one antenna in TM4 print limitation warning */
+        if (ue_dl.nof_rx_antennas < 2) {
+          Warning("Only one receiving antenna with TM4. Forcing RI=1 layer (PMI=%d).\n", packed_pmi);
+        }
       }
     }
   }
@@ -508,20 +521,22 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
       if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM3\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+        Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
+              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
       if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM4\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+        Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
+              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
     break;
@@ -923,12 +938,14 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, sr=%s, cfo=%.1f Hz%s\n",
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
        uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
        uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
        uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
+       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[1]?"1":"0"):"no",
+       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[0]?"1":"0"):"",
        uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   

From 3c845b3653239534d1bef21e60ebc3cf381c5c21 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 19 Sep 2017 12:45:24 +0200
Subject: [PATCH 195/269] Disabled buffer pool debugging

---
 lib/include/srslte/common/common.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/include/srslte/common/common.h b/lib/include/srslte/common/common.h
index ad88d9dcc..7156fbfc9 100644
--- a/lib/include/srslte/common/common.h
+++ b/lib/include/srslte/common/common.h
@@ -50,7 +50,7 @@
 #define SRSLTE_MAX_BUFFER_SIZE_BYTES 12756
 #define SRSLTE_BUFFER_HEADER_OFFSET  1024
 
-#define SRSLTE_BUFFER_POOL_LOG_ENABLED
+//#define SRSLTE_BUFFER_POOL_LOG_ENABLED
 
 #ifdef SRSLTE_BUFFER_POOL_LOG_ENABLED
 #define pool_allocate (pool->allocate(__FUNCTION__))

From 5359c42b46a485e847847d66df2ba85fa0ec0bfd Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 19 Sep 2017 15:15:25 +0200
Subject: [PATCH 196/269] RLC UM release timer at stop()

---
 lib/include/srslte/common/interfaces_common.h |  3 +-
 lib/include/srslte/common/timers.h            |  2 +-
 lib/include/srslte/upper/rlc_am.h             |  1 +
 lib/include/srslte/upper/rlc_common.h         |  1 +
 lib/include/srslte/upper/rlc_entity.h         |  1 +
 lib/include/srslte/upper/rlc_tm.h             |  1 +
 lib/include/srslte/upper/rlc_um.h             |  1 +
 lib/src/upper/rlc.cc                          |  5 +-
 lib/src/upper/rlc_am.cc                       |  6 ++
 lib/src/upper/rlc_entity.cc                   |  5 ++
 lib/src/upper/rlc_tm.cc                       |  5 ++
 lib/src/upper/rlc_um.cc                       |  6 ++
 lib/test/upper/rlc_am_test.cc                 |  1 +
 lib/test/upper/rlc_um_test.cc                 |  2 +-
 srsenb/hdr/mac/mac.h                          | 25 ++----
 srsenb/src/mac/mac.cc                         | 18 +++-
 srsue/hdr/mac/mac.h                           | 14 +--
 srsue/src/mac/mac.cc                          | 85 +++++++++++--------
 18 files changed, 117 insertions(+), 65 deletions(-)

diff --git a/lib/include/srslte/common/interfaces_common.h b/lib/include/srslte/common/interfaces_common.h
index d919d9fe5..dcb9ee736 100644
--- a/lib/include/srslte/common/interfaces_common.h
+++ b/lib/include/srslte/common/interfaces_common.h
@@ -71,7 +71,8 @@ public:
   /* Timer services with ms resolution. 
    * timer_id must be lower than MAC_NOF_UPPER_TIMERS
    */
-  virtual timers::timer* get(uint32_t timer_id) = 0;
+  virtual timers::timer* get(uint32_t timer_id)  = 0;
+  virtual void           free(uint32_t timer_id) = 0;
   virtual uint32_t               get_unique_id() = 0;
 };
 
diff --git a/lib/include/srslte/common/timers.h b/lib/include/srslte/common/timers.h
index 377df7b1d..533a07a76 100644
--- a/lib/include/srslte/common/timers.h
+++ b/lib/include/srslte/common/timers.h
@@ -145,7 +145,7 @@ public:
   }
   uint32_t get_unique_id() {
     if (nof_used_timers >= nof_timers) {
-      fprintf(stderr, "Error getting uinque timer id: no more timers available\n");
+      fprintf(stderr, "Error getting unique timer id: no more timers available\n");
       return 0;
     } else {
       while(used_timers[next_timer]) {
diff --git a/lib/include/srslte/upper/rlc_am.h b/lib/include/srslte/upper/rlc_am.h
index a92c9a0ad..afbd18544 100644
--- a/lib/include/srslte/upper/rlc_am.h
+++ b/lib/include/srslte/upper/rlc_am.h
@@ -78,6 +78,7 @@ public:
             mac_interface_timers *mac_timers);
   void configure(srslte_rlc_config_t cnfg);
   void reset();
+  void stop();
   void empty_queue(); 
   
   rlc_mode_t    get_mode();
diff --git a/lib/include/srslte/upper/rlc_common.h b/lib/include/srslte/upper/rlc_common.h
index c68e3e6f3..494cc6174 100644
--- a/lib/include/srslte/upper/rlc_common.h
+++ b/lib/include/srslte/upper/rlc_common.h
@@ -158,6 +158,7 @@ public:
                     srslte::mac_interface_timers      *mac_timers_) = 0;
   virtual void configure(srslte_rlc_config_t cnfg) = 0;
   virtual void reset() = 0;
+  virtual void stop() = 0;
   virtual void empty_queue() = 0; 
 
   virtual rlc_mode_t    get_mode() = 0;
diff --git a/lib/include/srslte/upper/rlc_entity.h b/lib/include/srslte/upper/rlc_entity.h
index 651af72fb..e9c41fa30 100644
--- a/lib/include/srslte/upper/rlc_entity.h
+++ b/lib/include/srslte/upper/rlc_entity.h
@@ -56,6 +56,7 @@ public:
 
   void configure(srslte_rlc_config_t cnfg);
   void reset();
+  void stop();
   void empty_queue();
   bool active();
 
diff --git a/lib/include/srslte/upper/rlc_tm.h b/lib/include/srslte/upper/rlc_tm.h
index def968a92..4a55e33dd 100644
--- a/lib/include/srslte/upper/rlc_tm.h
+++ b/lib/include/srslte/upper/rlc_tm.h
@@ -48,6 +48,7 @@ public:
             mac_interface_timers *mac_timers);
   void configure(srslte_rlc_config_t cnfg);
   void reset();
+  void stop();
   void empty_queue(); 
 
   rlc_mode_t    get_mode();
diff --git a/lib/include/srslte/upper/rlc_um.h b/lib/include/srslte/upper/rlc_um.h
index 4325bd029..672a7c646 100644
--- a/lib/include/srslte/upper/rlc_um.h
+++ b/lib/include/srslte/upper/rlc_um.h
@@ -58,6 +58,7 @@ public:
             mac_interface_timers *mac_timers_);
   void configure(srslte_rlc_config_t cnfg);
   void reset();
+  void stop();
   void empty_queue(); 
 
   rlc_mode_t    get_mode();
diff --git a/lib/src/upper/rlc.cc b/lib/src/upper/rlc.cc
index 0cc551a1f..f506ebc63 100644
--- a/lib/src/upper/rlc.cc
+++ b/lib/src/upper/rlc.cc
@@ -65,7 +65,10 @@ void rlc::reset_metrics()
 
 void rlc::stop()
 {
-  reset();
+  for(uint32_t i=0; i<SRSLTE_N_RADIO_BEARERS; i++) {
+    if(rlc_array[i].active())
+      rlc_array[i].stop();
+  }
 }
 
 void rlc::get_metrics(rlc_metrics_t &m)
diff --git a/lib/src/upper/rlc_am.cc b/lib/src/upper/rlc_am.cc
index f0055a534..b62a7e88e 100644
--- a/lib/src/upper/rlc_am.cc
+++ b/lib/src/upper/rlc_am.cc
@@ -93,6 +93,12 @@ void rlc_am::empty_queue() {
   }
 }
 
+void rlc_am::stop()
+{
+  reset();
+  pthread_mutex_destroy(&mutex);
+}
+
 void rlc_am::reset()
 {
   // Empty tx_sdu_queue before locking the mutex 
diff --git a/lib/src/upper/rlc_entity.cc b/lib/src/upper/rlc_entity.cc
index 247bf4409..5a351162a 100644
--- a/lib/src/upper/rlc_entity.cc
+++ b/lib/src/upper/rlc_entity.cc
@@ -76,6 +76,11 @@ void rlc_entity::reset()
   rlc = NULL;
 }
 
+void rlc_entity::stop()
+{
+  rlc->stop();
+  rlc = NULL;
+}
 
 void rlc_entity::empty_queue()
 {
diff --git a/lib/src/upper/rlc_tm.cc b/lib/src/upper/rlc_tm.cc
index e3bef0a99..627752494 100644
--- a/lib/src/upper/rlc_tm.cc
+++ b/lib/src/upper/rlc_tm.cc
@@ -66,6 +66,11 @@ void rlc_tm::reset()
   empty_queue(); 
 }
 
+void rlc_tm::stop()
+{
+  reset();
+}
+
 rlc_mode_t rlc_tm::get_mode()
 {
   return RLC_MODE_TM;
diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index 3d6bb9553..725bc8925 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -102,6 +102,12 @@ void rlc_um::empty_queue() {
   }
 }
 
+void rlc_um::stop()
+{
+  reset();
+  mac_timers->free(reordering_timeout_id);
+}
+
 void rlc_um::reset()
 {
   
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index c7ab2aa74..35503707e 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -43,6 +43,7 @@ public:
     return &t;
   }
   uint32_t get_unique_id(){return 0;}
+  void free(uint32_t id){}
 
 private:
   srslte::timers::timer t;
diff --git a/lib/test/upper/rlc_um_test.cc b/lib/test/upper/rlc_um_test.cc
index 739c9ebf8..926c65561 100644
--- a/lib/test/upper/rlc_um_test.cc
+++ b/lib/test/upper/rlc_um_test.cc
@@ -47,7 +47,7 @@ public:
   {
     t.step();
   }
-
+  void free(uint32_t timer_id) {}
 private:
   srslte::timers::timer t;
 };
diff --git a/srsenb/hdr/mac/mac.h b/srsenb/hdr/mac/mac.h
index 97be1825b..948065cf3 100644
--- a/srsenb/hdr/mac/mac.h
+++ b/srsenb/hdr/mac/mac.h
@@ -106,24 +106,14 @@ public:
   void timer_expired(uint32_t timer_id); 
   
   srslte::timers::timer*   get(uint32_t timer_id);
+  void                     free(uint32_t timer_id);
   u_int32_t                get_unique_id();
-  
+
+  const static int NOF_MAC_TIMERS = 20;
+
   uint32_t get_current_tti();
   void get_metrics(mac_metrics_t metrics[ENB_METRICS_MAX_USERS]);
-      
-  enum {
-    HARQ_RTT, 
-    TIME_ALIGNMENT,
-    CONTENTION_TIMER,
-    BSR_TIMER_PERIODIC,
-    BSR_TIMER_RETX,
-    PHR_TIMER_PERIODIC,
-    PHR_TIMER_PROHIBIT,
-    NOF_MAC_TIMERS
-  } mac_timers_t; 
-  
-  static const int MAC_NOF_UPPER_TIMERS = 20; 
-  
+
 private:  
 
   void log_step_ul(uint32_t tti);
@@ -194,12 +184,13 @@ private:
   /* Class to run upper-layer timers with normal priority */
   class upper_timers : public thread {
   public: 
-    upper_timers() : timers_db(MAC_NOF_UPPER_TIMERS),ttisync(10240) {start();}
+    upper_timers() : timers_db(NOF_MAC_TIMERS),ttisync(10240) {start();}
     void tti_clock();
     void stop();
     void reset();
     srslte::timers::timer* get(uint32_t timer_id);
-    uint32_t get_unique_id();
+    void                   free(uint32_t timer_id);
+    uint32_t               get_unique_id();
   private:
     void run_thread();
     srslte::timers      timers_db;
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 284cef3e7..62719009f 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -124,6 +124,11 @@ uint32_t mac::get_unique_id()
   return upper_timers_thread.get_unique_id();
 }
 
+void mac::free(uint32_t timer_id)
+{
+  upper_timers_thread.free(timer_id);
+}
+
 /* Front-end to upper-layer timers */
 srslte::timers::timer* mac::get(uint32_t timer_id)
 {
@@ -658,9 +663,20 @@ void mac::upper_timers::run_thread()
     timers_db.step_all();
   }
 }
+
+void mac::upper_timers::free(uint32_t timer_id)
+{
+  timers_db.release_id(timer_id);
+}
+
 srslte::timers::timer* mac::upper_timers::get(uint32_t timer_id)
 {
-  return timers_db.get(timer_id%MAC_NOF_UPPER_TIMERS);
+  if (timer_id < NOF_MAC_TIMERS) {
+    return timers_db.get(timer_id);
+  } else {
+    fprintf(stderr, "Error requested invalid timer id=%d\n", timer_id);
+    return NULL;
+  }
 }
 
 uint32_t mac::upper_timers::get_unique_id()
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index 51bb467f2..6eacdbc78 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -93,14 +93,15 @@ public:
   
   void timer_expired(uint32_t timer_id); 
   void start_pcap(srslte::mac_pcap* pcap);
-  
+
   srslte::timers::timer*   get(uint32_t timer_id);
+  void                     free(uint32_t timer_id);
   u_int32_t                get_unique_id();
-  
+
   uint32_t get_current_tti();
   
-  static const int MAC_NOF_UPPER_TIMERS = 20; 
-  
+  static const int MAC_NOF_UPPER_TIMERS = 20;
+
 private:  
   void run_thread(); 
   
@@ -161,16 +162,17 @@ private:
 
   /* Class to run upper-layer timers with normal priority */
   class upper_timers : public periodic_thread {
-  public: 
+  public:
     upper_timers();
     void reset();
+    void free(uint32_t timer_id);
     srslte::timers::timer* get(uint32_t timer_id);
     uint32_t get_unique_id();
   private:
     void run_period();
     srslte::timers  timers_db;
   };
-  upper_timers   upper_timers_thread; 
+  upper_timers   upper_timers_thread;
 
 
 
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 5c0a00b4a..a74908db2 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -97,7 +97,7 @@ void mac::stop()
 
 void mac::start_pcap(srslte::mac_pcap* pcap_)
 {
-  pcap = pcap_; 
+  pcap = pcap_;
   dl_harq.start_pcap(pcap);
   ul_harq.start_pcap(pcap);
   ra_procedure.start_pcap(pcap);
@@ -113,28 +113,28 @@ void mac::reconfiguration()
 void mac::reset()
 {
   bzero(&metrics, sizeof(mac_metrics_t));
- 
+
   Info("Resetting MAC\n");
-  
+
   timers_db.stop_all();
   upper_timers_thread.reset();
-  
+
   ul_harq.reset_ndi();
-  
+
   mux_unit.msg3_flush();
   mux_unit.reset();
-  
-  ra_procedure.reset();    
+
+  ra_procedure.reset();
   sr_procedure.reset();
   bsr_procedure.reset();
   phr_procedure.reset();
-  
+
   dl_harq.reset();
   phy_h->pdcch_dl_search_reset();
   phy_h->pdcch_ul_search_reset();
-  
+
   is_first_ul_grant = true;
-  
+
   bzero(&uernti, sizeof(ue_rnti_t));
 }
 
@@ -158,13 +158,13 @@ void mac::run_thread() {
 
       log_h->step(tti);
       timers_db.step_all();
-      
-      // Step all procedures 
+
+      // Step all procedures
       bsr_procedure.step(tti);
       phr_procedure.step(tti);
-      
-      // Check if BSR procedure need to start SR 
-      
+
+      // Check if BSR procedure need to start SR
+
       if (bsr_procedure.need_to_send_sr(tti)) {
         Debug("Starting SR procedure by BSR request, PHY TTI=%d\n", tti);
         sr_procedure.start();
@@ -175,13 +175,13 @@ void mac::run_thread() {
       }
       sr_procedure.step(tti);
 
-      // Check SR if we need to start RA 
+      // Check SR if we need to start RA
       if (sr_procedure.need_random_access()) {
         ra_procedure.start_mac_order();
       }
       ra_procedure.step(tti);
     }
-  }  
+  }
 }
 
 void mac::bcch_start_rx()
@@ -197,7 +197,7 @@ void mac::bcch_start_rx(int si_window_start, int si_window_length)
   } else {
     phy_h->pdcch_dl_search(SRSLTE_RNTI_SI, SRSLTE_SIRNTI, si_window_start);
   }
-  Info("SCHED: Searching for DL grant for SI-RNTI window_st=%d, window_len=%d\n", si_window_start, si_window_length);  
+  Info("SCHED: Searching for DL grant for SI-RNTI window_st=%d, window_len=%d\n", si_window_start, si_window_length);
 }
 
 void mac::bcch_stop_rx()
@@ -208,7 +208,7 @@ void mac::bcch_stop_rx()
 void mac::pcch_start_rx()
 {
   phy_h->pdcch_dl_search(SRSLTE_RNTI_PCH, SRSLTE_PRNTI);
-  Info("SCHED: Searching for DL grant for P-RNTI\n");  
+  Info("SCHED: Searching for DL grant for P-RNTI\n");
 }
 
 void mac::pcch_stop_rx()
@@ -224,9 +224,9 @@ void mac::tti_clock(uint32_t tti)
 
 void mac::bch_decoded_ok(uint8_t* payload, uint32_t len)
 {
-  // Send MIB to RLC 
+  // Send MIB to RLC
   rlc_h->write_pdu_bcch_bch(payload, len);
-  
+
   if (pcap) {
     pcap->write_dl_bch(payload, len, true, phy_h->get_current_tti());
   }
@@ -234,9 +234,9 @@ void mac::bch_decoded_ok(uint8_t* payload, uint32_t len)
 
 void mac::pch_decoded_ok(uint32_t len)
 {
-  // Send PCH payload to RLC 
+  // Send PCH payload to RLC
   rlc_h->write_pdu_pcch(pch_payload_buffer, len);
-  
+
   if (pcap) {
     pcap->write_dl_pch(pch_payload_buffer, len, true, phy_h->get_current_tti());
   }
@@ -267,7 +267,7 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
   } else if (grant.rnti_type == SRSLTE_RNTI_PCH) {
 
     memcpy(&action->phy_grant, &grant.phy_grant, sizeof(srslte_phy_grant_t));
-    action->generate_ack = false; 
+    action->generate_ack = false;
     action->decode_enabled[0] = true;
     srslte_softbuffer_rx_reset_cb(&pch_softbuffer, 1);
     action->payload_ptr[0] = pch_payload_buffer;
@@ -281,7 +281,7 @@ void mac::new_grant_dl(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
   } else {
     // If PDCCH for C-RNTI and RA procedure in Contention Resolution, notify it
     if (grant.rnti_type == SRSLTE_RNTI_USER && ra_procedure.is_contention_resolution()) {
-      ra_procedure.pdcch_to_crnti(false);      
+      ra_procedure.pdcch_to_crnti(false);
     }
     dl_harq.new_grant_dl(grant, action);
   }
@@ -296,11 +296,11 @@ void mac::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
 {
   /* Start PHR Periodic timer on first UL grant */
   if (is_first_ul_grant) {
-    is_first_ul_grant = false; 
+    is_first_ul_grant = false;
     timers_db.get(PHR_TIMER_PERIODIC)->run();
   }
   if (grant.rnti_type == SRSLTE_RNTI_USER && ra_procedure.is_contention_resolution()) {
-    ra_procedure.pdcch_to_crnti(true);    
+    ra_procedure.pdcch_to_crnti(true);
   }
   ul_harq.new_grant_ul(grant, action);
   metrics.tx_pkts++;
@@ -353,13 +353,13 @@ void mac::timer_expired(uint32_t timer_id)
     case TIME_ALIGNMENT:
       timeAlignmentTimerExpire();
       break;
-    default: 
+    default:
       break;
   }
 }
 
 /* Function called on expiry of TimeAlignmentTimer */
-void mac::timeAlignmentTimerExpire() 
+void mac::timeAlignmentTimerExpire()
 {
   printf("timeAlignmentTimer has expired value=%d ms\n", timers_db.get(TIME_ALIGNMENT)->get_timeout());
   rrc_h->release_pucch_srs();
@@ -374,7 +374,7 @@ void mac::get_rntis(ue_rnti_t* rntis)
 
 void mac::set_contention_id(uint64_t uecri)
 {
-  uernti.contention_id = uecri; 
+  uernti.contention_id = uecri;
 }
 
 void mac::get_config(mac_cfg_t* mac_cfg)
@@ -407,7 +407,7 @@ void mac::set_config_sr(LIBLTE_RRC_SCHEDULING_REQUEST_CONFIG_STRUCT* sr_cfg)
 
 void mac::setup_lcid(uint32_t lcid, uint32_t lcg, uint32_t priority, int PBR_x_tti, uint32_t BSD)
 {
-  Info("Logical Channel Setup: LCID=%d, LCG=%d, priority=%d, PBR=%d, BSd=%d\n", 
+  Info("Logical Channel Setup: LCID=%d, LCG=%d, priority=%d, PBR=%d, BSd=%d\n",
        lcid, lcg, priority, PBR_x_tti, BSD);
   mux_unit.set_priority(lcid, priority, PBR_x_tti, BSD);
   bsr_procedure.setup_lcg(lcid, lcg);
@@ -425,8 +425,13 @@ srslte::timers::timer* mac::get(uint32_t timer_id)
   return upper_timers_thread.get(timer_id);
 }
 
+void mac::free(uint32_t timer_id)
+{
+  upper_timers_thread.free(timer_id);
+}
 
-void mac::get_metrics(mac_metrics_t &m)
+
+  void mac::get_metrics(mac_metrics_t &m)
 {
   Info("DL retx: %.2f \%%, perpkt: %.2f, UL retx: %.2f \%% perpkt: %.2f\n", 
        metrics.rx_pkts?((float) 100*metrics.rx_errors/metrics.rx_pkts):0.0, 
@@ -442,20 +447,20 @@ void mac::get_metrics(mac_metrics_t &m)
 
 /********************************************************
  *
- * Class to run upper-layer timers with normal priority 
+ * Class to run upper-layer timers with normal priority
  *
  *******************************************************/
 
-mac::upper_timers::upper_timers() : timers_db(MAC_NOF_UPPER_TIMERS) 
+mac::upper_timers::upper_timers() : timers_db(MAC_NOF_UPPER_TIMERS)
 {
-  start_periodic(1000, MAC_MAIN_THREAD_PRIO+1);  
+  start_periodic(1000, MAC_MAIN_THREAD_PRIO+1);
 }
 
 void mac::upper_timers::run_period()
 {
   timers_db.step_all();
 }
-  
+
 srslte::timers::timer* mac::upper_timers::get(uint32_t timer_id)
 {
   return timers_db.get(timer_id%MAC_NOF_UPPER_TIMERS);
@@ -472,9 +477,15 @@ void mac::upper_timers::reset()
 }
 
 
+void mac::upper_timers::free(uint32_t timer_id)
+{
+  timers_db.release_id(timer_id);
+}
+
+
 
 
-/********************************************************
+  /********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from
  * DEMUX unit

From 6556941e6d33685d5fd6f34dc16436abb9b9ab07 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 19 Sep 2017 17:51:35 +0200
Subject: [PATCH 197/269] simplified MAC timers. Unified mac and upper timers
 in same background task

---
 lib/include/srslte/common/interfaces_common.h |   6 +-
 lib/include/srslte/common/timers.h            |   2 +-
 lib/include/srslte/upper/rlc_um.h             |   3 +-
 lib/src/upper/rlc_um.cc                       |  27 ++--
 lib/test/upper/rlc_am_test.cc                 |   6 +-
 lib/test/upper/rlc_um_test.cc                 |  10 +-
 srsenb/hdr/mac/mac.h                          |  25 ++-
 srsenb/src/mac/mac.cc                         |  79 ++++------
 srsue/hdr/mac/demux.h                         |   4 +-
 srsue/hdr/mac/dl_harq.h                       |  11 +-
 srsue/hdr/mac/mac.h                           |  64 ++++----
 srsue/hdr/mac/mac_common.h                    |  45 ------
 srsue/hdr/mac/proc_bsr.h                      |   3 +
 srsue/hdr/mac/proc_phr.h                      |  12 +-
 srsue/hdr/mac/proc_ra.h                       |  90 ++++++-----
 srsue/hdr/mac/ul_harq.h                       |  16 +-
 srsue/src/mac/demux.cc                        |  10 +-
 srsue/src/mac/mac.cc                          | 149 +++++++++---------
 srsue/src/mac/proc_bsr.cc                     |  75 ++++-----
 srsue/src/mac/proc_phr.cc                     |  69 ++++----
 srsue/src/mac/proc_ra.cc                      |  30 ++--
 srsue/src/upper/rrc.cc                        |  50 +++---
 22 files changed, 373 insertions(+), 413 deletions(-)
 delete mode 100644 srsue/hdr/mac/mac_common.h

diff --git a/lib/include/srslte/common/interfaces_common.h b/lib/include/srslte/common/interfaces_common.h
index dcb9ee736..a027b6230 100644
--- a/lib/include/srslte/common/interfaces_common.h
+++ b/lib/include/srslte/common/interfaces_common.h
@@ -71,9 +71,9 @@ public:
   /* Timer services with ms resolution. 
    * timer_id must be lower than MAC_NOF_UPPER_TIMERS
    */
-  virtual timers::timer* get(uint32_t timer_id)  = 0;
-  virtual void           free(uint32_t timer_id) = 0;
-  virtual uint32_t               get_unique_id() = 0;
+  virtual timers::timer* timer_get(uint32_t timer_id)  = 0;
+  virtual void           timer_release_id(uint32_t timer_id) = 0;
+  virtual uint32_t       timer_get_unique_id() = 0;
 };
 
 class read_pdu_interface
diff --git a/lib/include/srslte/common/timers.h b/lib/include/srslte/common/timers.h
index 533a07a76..a8563af37 100644
--- a/lib/include/srslte/common/timers.h
+++ b/lib/include/srslte/common/timers.h
@@ -74,7 +74,7 @@ public:
     }
     void step() {
       if (running) {
-        counter++; 
+        counter++;
         if (is_expired()) {
           running = false; 
           if (callback) {
diff --git a/lib/include/srslte/upper/rlc_um.h b/lib/include/srslte/upper/rlc_um.h
index 672a7c646..0a2469a7c 100644
--- a/lib/include/srslte/upper/rlc_um.h
+++ b/lib/include/srslte/upper/rlc_um.h
@@ -125,7 +125,8 @@ private:
    * Timers
    * Ref: 3GPP TS 36.322 v10.0.0 Section 7
    ***************************************************************************/
-  uint32_t reordering_timeout_id;
+  srslte::timers::timer *reordering_timer;
+  uint32_t               reordering_timer_id;
 
   bool     pdu_lost;
 
diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index 725bc8925..c5399407f 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -62,7 +62,8 @@ void rlc_um::init(srslte::log                 *log_,
   pdcp                  = pdcp_;
   rrc                   = rrc_;
   mac_timers            = mac_timers_;
-  reordering_timeout_id = mac_timers->get_unique_id();
+  reordering_timer_id   = mac_timers->timer_get_unique_id();
+  reordering_timer      = mac_timers->timer_get(reordering_timer_id);
 }
 
 void rlc_um::configure(srslte_rlc_config_t cnfg_)
@@ -105,7 +106,7 @@ void rlc_um::empty_queue() {
 void rlc_um::stop()
 {
   reset();
-  mac_timers->free(reordering_timeout_id);
+  mac_timers->timer_release_id(reordering_timer_id);
 }
 
 void rlc_um::reset()
@@ -125,7 +126,7 @@ void rlc_um::reset()
   if(tx_sdu)
     tx_sdu->reset();
   if(mac_timers)
-    mac_timers->get(reordering_timeout_id)->stop();
+    reordering_timer->stop();
   
   // Drop all messages in RX window
   std::map<uint32_t, rlc_umd_pdu_t>::iterator it;
@@ -209,7 +210,7 @@ void rlc_um::write_pdu(uint8_t *payload, uint32_t nof_bytes)
 
 void rlc_um::timer_expired(uint32_t timeout_id)
 {
-  if(reordering_timeout_id == timeout_id)
+  if(reordering_timer_id == timeout_id)
   {
     pthread_mutex_lock(&mutex);
 
@@ -227,11 +228,11 @@ void rlc_um::timer_expired(uint32_t timeout_id)
       reassemble_rx_sdus();
       log->debug("Finished reassemble from timeout id=%d\n", timeout_id);
     }
-    mac_timers->get(reordering_timeout_id)->stop();
+    reordering_timer->stop();
     if(RX_MOD_BASE(vr_uh) > RX_MOD_BASE(vr_ur))
     {
-      mac_timers->get(reordering_timeout_id)->set(this, cfg.t_reordering);
-      mac_timers->get(reordering_timeout_id)->run();
+      reordering_timer->set(this, cfg.t_reordering);
+      reordering_timer->run();
       vr_ux = vr_uh;
     }
 
@@ -242,7 +243,7 @@ void rlc_um::timer_expired(uint32_t timeout_id)
 
 bool rlc_um::reordering_timeout_running()
 {
-  return mac_timers->get(reordering_timeout_id)->is_running();
+  return reordering_timer->is_running();
 }
 
 /****************************************************************************
@@ -404,20 +405,20 @@ void rlc_um::handle_data_pdu(uint8_t *payload, uint32_t nof_bytes)
   log->debug("Finished reassemble from received PDU\n");
   
   // Update reordering variables and timers
-  if(mac_timers->get(reordering_timeout_id)->is_running())
+  if(reordering_timer->is_running())
   {
     if(RX_MOD_BASE(vr_ux) <= RX_MOD_BASE(vr_ur) ||
        (!inside_reordering_window(vr_ux) && vr_ux != vr_uh))
     {
-      mac_timers->get(reordering_timeout_id)->stop();
+      reordering_timer->stop();
     }
   }
-  if(!mac_timers->get(reordering_timeout_id)->is_running())
+  if(!reordering_timer->is_running())
   {
     if(RX_MOD_BASE(vr_uh) > RX_MOD_BASE(vr_ur))
     {
-      mac_timers->get(reordering_timeout_id)->set(this, cfg.t_reordering);
-      mac_timers->get(reordering_timeout_id)->run();
+      reordering_timer->set(this, cfg.t_reordering);
+      reordering_timer->run();
       vr_ux = vr_uh;
     }
   }
diff --git a/lib/test/upper/rlc_am_test.cc b/lib/test/upper/rlc_am_test.cc
index 35503707e..7a37d0d5d 100644
--- a/lib/test/upper/rlc_am_test.cc
+++ b/lib/test/upper/rlc_am_test.cc
@@ -38,12 +38,12 @@ class mac_dummy_timers
     :public srslte::mac_interface_timers
 {
 public:
-  srslte::timers::timer* get(uint32_t timer_id)
+  srslte::timers::timer* timer_get(uint32_t timer_id)
   {
     return &t;
   }
-  uint32_t get_unique_id(){return 0;}
-  void free(uint32_t id){}
+  uint32_t timer_get_unique_id(){return 0;}
+  void timer_release_id(uint32_t id){}
 
 private:
   srslte::timers::timer t;
diff --git a/lib/test/upper/rlc_um_test.cc b/lib/test/upper/rlc_um_test.cc
index 926c65561..0894a2a8b 100644
--- a/lib/test/upper/rlc_um_test.cc
+++ b/lib/test/upper/rlc_um_test.cc
@@ -38,16 +38,16 @@ class mac_dummy_timers
     :public srslte::mac_interface_timers
 {
 public:
-  srslte::timers::timer* get(uint32_t timer_id)
+  srslte::timers::timer* timer_get(uint32_t timer_id)
   {
     return &t;
   }
-  uint32_t get_unique_id(){return 0;}
+  uint32_t timer_get_unique_id(){return 0;}
   void step()
   {
     t.step();
   }
-  void free(uint32_t timer_id) {}
+  void timer_release_id(uint32_t timer_id) {}
 private:
   srslte::timers::timer t;
 };
@@ -205,8 +205,8 @@ void loss_test()
   }
 
   // Step the reordering timer until expiry
-  while(!timers.get(1)->is_expired())
-    timers.get(1)->step();
+  while(!timers.timer_get(1)->is_expired())
+    timers.timer_get(1)->step();
 
   assert(NBUFS-1 == tester.n_sdus);
 }
diff --git a/srsenb/hdr/mac/mac.h b/srsenb/hdr/mac/mac.h
index 948065cf3..4620c9629 100644
--- a/srsenb/hdr/mac/mac.h
+++ b/srsenb/hdr/mac/mac.h
@@ -102,14 +102,11 @@ public:
   int rlc_buffer_state(uint16_t rnti, uint32_t lc_id, uint32_t tx_queue, uint32_t retx_queue);
     
   bool process_pdus(); 
-  
-  void timer_expired(uint32_t timer_id); 
-  
-  srslte::timers::timer*   get(uint32_t timer_id);
-  void                     free(uint32_t timer_id);
-  u_int32_t                get_unique_id();
 
-  const static int NOF_MAC_TIMERS = 20;
+  // Interface for upper-layer timers
+  srslte::timers::timer*   timer_get(uint32_t timer_id);
+  void                     timer_release_id(uint32_t timer_id);
+  u_int32_t                timer_get_unique_id();
 
   uint32_t get_current_tti();
   void get_metrics(mac_metrics_t metrics[ENB_METRICS_MAX_USERS]);
@@ -182,22 +179,18 @@ private:
   
 
   /* Class to run upper-layer timers with normal priority */
-  class upper_timers : public thread {
-  public: 
-    upper_timers() : timers_db(NOF_MAC_TIMERS),ttisync(10240) {start();}
+  class timer_thread : public thread {
+  public:
+    timer_thread(srslte::timers *t) : ttisync(10240),timers(t),running(false) {start();}
     void tti_clock();
     void stop();
-    void reset();
-    srslte::timers::timer* get(uint32_t timer_id);
-    void                   free(uint32_t timer_id);
-    uint32_t               get_unique_id();
   private:
     void run_thread();
-    srslte::timers      timers_db;
     srslte::tti_sync_cv ttisync;
+    srslte::timers     *timers;
     bool running; 
   };
-  upper_timers   upper_timers_thread; 
+  timer_thread   timers_thread;
 
   /* Class to process MAC PDUs from DEMUX unit */
   class pdu_process : public thread {
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 62719009f..87290a859 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -42,7 +42,8 @@
 
 namespace srsenb {
 
-mac::mac() : timers_db((uint32_t) NOF_MAC_TIMERS),
+mac::mac() : timers_db(128),
+             timers_thread(&timers_db),
              rar_pdu_msg(sched_interface::MAX_RAR_LIST),
              pdu_process_thread(this)
 {
@@ -99,7 +100,7 @@ void mac::stop()
   srslte_softbuffer_tx_free(&pcch_softbuffer_tx);
   srslte_softbuffer_tx_free(&rar_softbuffer_tx);
   started = false;   
-  upper_timers_thread.stop();
+  timers_thread.stop();
   pdu_process_thread.stop();
 }
 
@@ -109,8 +110,7 @@ void mac::reset()
   Info("Resetting MAC\n");
   
   timers_db.stop_all();
-  upper_timers_thread.reset();
-  
+
   tti = 0; 
   last_rnti = 70; 
   
@@ -119,23 +119,6 @@ void mac::reset()
   
 }
 
-uint32_t mac::get_unique_id()
-{
-  return upper_timers_thread.get_unique_id();
-}
-
-void mac::free(uint32_t timer_id)
-{
-  upper_timers_thread.free(timer_id);
-}
-
-/* Front-end to upper-layer timers */
-srslte::timers::timer* mac::get(uint32_t timer_id)
-{
-  return upper_timers_thread.get(timer_id);
-}
-
-
 void mac::start_pcap(srslte::mac_pcap* pcap_)
 {
   pcap = pcap_; 
@@ -645,57 +628,59 @@ void mac::log_step_dl(uint32_t tti)
 
 void mac::tti_clock()
 {
-  upper_timers_thread.tti_clock();
+  timers_thread.tti_clock();
 }
 
+
+
+
 /********************************************************
  *
- * Class to run upper-layer timers with normal priority 
+ * Interface for upper layer timers
  *
  *******************************************************/
-void mac::upper_timers::run_thread()
+uint32_t mac::timer_get_unique_id()
 {
-  running=true; 
-  ttisync.set_producer_cntr(0);
-  ttisync.resync();
-  while(running) {
-    ttisync.wait();
-    timers_db.step_all();
-  }
+  return timers_db.get_unique_id();
 }
 
-void mac::upper_timers::free(uint32_t timer_id)
+void mac::timer_release_id(uint32_t timer_id)
 {
   timers_db.release_id(timer_id);
 }
 
-srslte::timers::timer* mac::upper_timers::get(uint32_t timer_id)
+/* Front-end to upper-layer timers */
+srslte::timers::timer* mac::timer_get(uint32_t timer_id)
 {
-  if (timer_id < NOF_MAC_TIMERS) {
-    return timers_db.get(timer_id);
-  } else {
-    fprintf(stderr, "Error requested invalid timer id=%d\n", timer_id);
-    return NULL;
-  }
+  return timers_db.get(timer_id);
 }
 
-uint32_t mac::upper_timers::get_unique_id()
+
+
+/********************************************************
+ *
+ * Class to run timers with normal priority
+ *
+ *******************************************************/
+void mac::timer_thread::run_thread()
 {
-  return timers_db.get_unique_id();
+  running=true; 
+  ttisync.set_producer_cntr(0);
+  ttisync.resync();
+  while(running) {
+    ttisync.wait();
+    timers->step_all();
+  }
 }
 
-void mac::upper_timers::stop()
+void mac::timer_thread::stop()
 {
   running=false;
   ttisync.increase();
   wait_thread_finish();
 }
-void mac::upper_timers::reset()
-{
-  timers_db.stop_all();
-}
 
-void mac::upper_timers::tti_clock()
+void mac::timer_thread::tti_clock()
 {
   ttisync.increase();
 }
diff --git a/srsue/hdr/mac/demux.h b/srsue/hdr/mac/demux.h
index 2f38a4dfc..8b722e8c9 100644
--- a/srsue/hdr/mac/demux.h
+++ b/srsue/hdr/mac/demux.h
@@ -42,7 +42,7 @@ class demux : public srslte::pdu_queue::process_callback
 {
 public:
   demux(uint8_t nof_harq_proc_);
-  void init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc, srslte::log* log_h_, srslte::timers* timers_db_);
+  void init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc, srslte::log* log_h_, srslte::timers::timer* time_alignment_timer);
 
   bool     process_pdus();
   uint8_t* request_buffer(uint32_t pid, uint32_t len);
@@ -74,7 +74,7 @@ private:
     
   phy_interface_mac_common *phy_h;
   srslte::log              *log_h;
-  srslte::timers           *timers_db;
+  srslte::timers::timer    *time_alignment_timer;
   rlc_interface_mac        *rlc;
   uint8_t                   nof_harq_proc;
   
diff --git a/srsue/hdr/mac/dl_harq.h b/srsue/hdr/mac/dl_harq.h
index e2f5aeab0..ee925596f 100644
--- a/srsue/hdr/mac/dl_harq.h
+++ b/srsue/hdr/mac/dl_harq.h
@@ -35,7 +35,6 @@
 #include "srslte/common/log.h"
 #include "srslte/common/timers.h"
 #include "mac/demux.h"
-#include "mac/mac_common.h"
 #include "mac/dl_sps.h"
 #include "srslte/common/mac_pcap.h"
 
@@ -58,9 +57,9 @@ public:
     pcap = NULL;
   }
     
-  bool init(srslte::log *log_h_, srslte::timers *timers_, demux *demux_unit_)
+  bool init(srslte::log *log_h_, srslte::timers::timer *timer_aligment_timer_, demux *demux_unit_)
   {
-    timers_db  = timers_; 
+    timer_aligment_timer  = timer_aligment_timer_;
     demux_unit = demux_unit_; 
     si_window_start = 0; 
     log_h = log_h_; 
@@ -102,7 +101,7 @@ public:
       } else {
         if (grant.is_sps_release) {
           dl_sps_assig.clear();
-          if (timers_db->get(TIME_ALIGNMENT)->is_running()) {
+          if (timer_aligment_timer->is_running()) {
             //phy_h->send_sps_ack();
             Warning("PHY Send SPS ACK not implemented\n");
           }
@@ -282,7 +281,7 @@ private:
           Warning("DL PID %d: Received duplicate TB. Discarting and retransmitting ACK\n", pid);
         }
 
-        if (pid == HARQ_BCCH_PID || harq_entity->timers_db->get(TIME_ALIGNMENT)->is_expired()) {
+        if (pid == HARQ_BCCH_PID || harq_entity->timer_aligment_timer->is_expired()) {
           // Do not generate ACK
           Debug("Not generating ACK\n");
           action->generate_ack = false;
@@ -397,7 +396,7 @@ private:
   
 
   std::vector<dl_harq_process> proc;
-  srslte::timers   *timers_db;
+  srslte::timers::timer   *timer_aligment_timer;
   demux           *demux_unit; 
   srslte::log     *log_h;
   srslte::mac_pcap *pcap; 
diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index 6eacdbc78..ed5ad151a 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -48,9 +48,9 @@ namespace srsue {
 class mac
     :public mac_interface_phy
     ,public mac_interface_rrc
+    ,public srslte::timer_callback
     ,public srslte::mac_interface_timers
     ,public thread
-    ,public srslte::timer_callback
 {
 public:
   mac();
@@ -90,17 +90,19 @@ public:
   void set_contention_id(uint64_t uecri);
   
   void get_rntis(ue_rnti_t *rntis);
-  
-  void timer_expired(uint32_t timer_id); 
+
   void start_pcap(srslte::mac_pcap* pcap);
 
-  srslte::timers::timer*   get(uint32_t timer_id);
-  void                     free(uint32_t timer_id);
-  u_int32_t                get_unique_id();
+  // Timer callback interface
+  void timer_expired(uint32_t timer_id); 
 
   uint32_t get_current_tti();
-  
-  static const int MAC_NOF_UPPER_TIMERS = 20;
+
+  // Interface for upper-layer timers
+  srslte::timers::timer* timer_get(uint32_t timer_id);
+  void                   timer_release_id(uint32_t timer_id);
+  uint32_t               timer_get_unique_id();
+
 
 private:  
   void run_thread(); 
@@ -145,13 +147,32 @@ private:
   /* Buffers for PCH reception (not included in DL HARQ) */
   const static uint32_t  pch_payload_buffer_sz = 8*1024;
   srslte_softbuffer_rx_t pch_softbuffer;
-  uint8_t                pch_payload_buffer[pch_payload_buffer_sz]; 
-  
+  uint8_t                pch_payload_buffer[pch_payload_buffer_sz];
+
+
+  /* class that runs a thread to trigger timer callbacks in low priority */
+  class timer_thread : public thread {
+  public:
+    timer_thread(srslte::timers *timers_) : ttisync(10240),running(false),timers(timers_) {start();}
+    void tti_clock();
+    void stop();
+  private:
+    bool running;
+    void run_thread();
+    srslte::tti_sync_cv ttisync;
+    srslte::timers     *timers;
+  };
+
+
   /* Functions for MAC Timers */
-  srslte::timers  timers_db;
+  uint32_t        timer_alignment;
+  uint32_t        contention_resolution_timer;
   void            setup_timers();
-  void            timeAlignmentTimerExpire();
-  
+  void            timer_alignment_expire();
+  srslte::timers  timers;
+  timer_thread    timers_thread;
+
+
   // pointer to MAC PCAP object
   srslte::mac_pcap* pcap;
   bool is_first_ul_grant;
@@ -159,23 +180,6 @@ private:
 
   mac_metrics_t metrics; 
 
-
-  /* Class to run upper-layer timers with normal priority */
-  class upper_timers : public periodic_thread {
-  public:
-    upper_timers();
-    void reset();
-    void free(uint32_t timer_id);
-    srslte::timers::timer* get(uint32_t timer_id);
-    uint32_t get_unique_id();
-  private:
-    void run_period();
-    srslte::timers  timers_db;
-  };
-  upper_timers   upper_timers_thread;
-
-
-
   /* Class to process MAC PDUs from DEMUX unit */
   class pdu_process : public thread {
   public: 
diff --git a/srsue/hdr/mac/mac_common.h b/srsue/hdr/mac/mac_common.h
deleted file mode 100644
index d40d179eb..000000000
--- a/srsue/hdr/mac/mac_common.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/**
- *
- * \section COPYRIGHT
- *
- * Copyright 2013-2015 Software Radio Systems Limited
- *
- * \section LICENSE
- *
- * This file is part of the srsUE library.
- *
- * srsUE is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Affero General Public License as
- * published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- *
- * srsUE is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Affero General Public License for more details.
- *
- * A copy of the GNU Affero General Public License can be found in
- * the LICENSE file in the top-level directory of this distribution
- * and at http://www.gnu.org/licenses/.
- *
- */
-
-#ifndef MAC_COMMON_H
-#define MAC_COMMON_H
-
-namespace srsue {
-
-typedef enum {
-  HARQ_RTT, 
-  TIME_ALIGNMENT,
-  CONTENTION_TIMER,
-  BSR_TIMER_PERIODIC,
-  BSR_TIMER_RETX,
-  PHR_TIMER_PERIODIC,
-  PHR_TIMER_PROHIBIT,
-  NOF_MAC_TIMERS
-} mac_timers_t; 
-  
-} // namespace srsue
-
-#endif // MAC_COMMON_H
diff --git a/srsue/hdr/mac/proc_bsr.h b/srsue/hdr/mac/proc_bsr.h
index 21f278e29..656e9a7a3 100644
--- a/srsue/hdr/mac/proc_bsr.h
+++ b/srsue/hdr/mac/proc_bsr.h
@@ -81,6 +81,9 @@ private:
   bool generate_bsr(bsr_t *bsr, uint32_t nof_padding_bytes); 
   char* bsr_type_tostring(triggered_bsr_type_t type); 
   char* bsr_format_tostring(bsr_format_t format);
+
+  uint32_t timer_periodic_id;
+  uint32_t timer_retx_id;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/mac/proc_phr.h b/srsue/hdr/mac/proc_phr.h
index 942f30456..240a7bab7 100644
--- a/srsue/hdr/mac/proc_phr.h
+++ b/srsue/hdr/mac/proc_phr.h
@@ -49,7 +49,9 @@ public:
   
   bool generate_phr_on_ul_grant(float *phr);
   void timer_expired(uint32_t timer_id);
-  
+
+  void start_timer();
+
 private:
   
   bool pathloss_changed(); 
@@ -59,11 +61,15 @@ private:
   phy_interface_mac* phy_h; 
   srslte::timers* timers_db;
   bool initiated;
-  int timer_prohibit;
-  int timer_periodic;
+  int timer_prohibit_value;
+  int timer_periodic_value;
   int dl_pathloss_change; 
   int last_pathloss_db;
   bool phr_is_triggered;
+
+  uint32_t timer_periodic_id;
+  uint32_t timer_prohibit_id;
+
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/mac/proc_ra.h b/srsue/hdr/mac/proc_ra.h
index c1bdf3644..2bd2dd807 100644
--- a/srsue/hdr/mac/proc_ra.h
+++ b/srsue/hdr/mac/proc_ra.h
@@ -59,8 +59,7 @@ class ra_proc : public srslte::timer_callback
       phy_h                     = NULL; 
       log_h                     = NULL; 
       mac_cfg                   = NULL; 
-      timers_db                 = NULL; 
-      mux_unit                  = NULL; 
+      mux_unit                  = NULL;
       demux_unit                = NULL; 
       rrc                       = NULL; 
       transmitted_contention_id = 0; 
@@ -69,7 +68,10 @@ class ra_proc : public srslte::timer_callback
       started_by_pdcch          = false; 
       rar_grant_nbytes          = 0; 
       rar_grant_tti             = 0; 
-      msg3_flushed              = false; 
+      msg3_flushed              = false;
+
+      time_alignment_timer        = NULL;
+      contention_resolution_timer = NULL;
     };
 
     ~ra_proc();
@@ -78,8 +80,9 @@ class ra_proc : public srslte::timer_callback
               rrc_interface_mac *rrc_, 
               srslte::log *log_h, 
               mac_interface_rrc::ue_rnti_t *rntis, 
-              mac_interface_rrc::mac_cfg_t *mac_cfg, 
-              srslte::timers *timers_db, 
+              mac_interface_rrc::mac_cfg_t *mac_cfg,
+              srslte::timers::timer* time_alignment_timer_,
+              srslte::timers::timer* contention_resolution_timer_,
               mux *mux_unit, 
               demux *demux_unit);
     void reset();
@@ -101,9 +104,9 @@ class ra_proc : public srslte::timer_callback
     void start_pcap(srslte::mac_pcap* pcap);
 private: 
     static bool uecrid_callback(void *arg, uint64_t uecri);
-    
+
     bool contention_resolution_id_received(uint64_t uecri);
-    void process_timeadv_cmd(uint32_t ta_cmd); 
+    void process_timeadv_cmd(uint32_t ta_cmd);
     void step_initialization();
     void step_resource_selection();
     void step_preamble_transmission();
@@ -114,14 +117,14 @@ private:
     void step_contention_resolution();
     void step_completition();
 
-    //  Buffer to receive RAR PDU 
+    //  Buffer to receive RAR PDU
     static const uint32_t MAX_RAR_PDU_LEN = 2048;
     uint8_t     rar_pdu_buffer[MAX_RAR_PDU_LEN];
-    srslte::rar_pdu rar_pdu_msg; 
-    
+    srslte::rar_pdu rar_pdu_msg;
+
     // Random Access parameters provided by higher layers defined in 5.1.1
     uint32_t configIndex;
-    uint32_t nof_preambles; 
+    uint32_t nof_preambles;
     uint32_t nof_groupA_preambles;
     uint32_t nof_groupB_preambles;
     uint32_t messagePowerOffsetGroupB;
@@ -130,26 +133,25 @@ private:
     uint32_t powerRampingStep;
     uint32_t preambleTransMax;
     uint32_t iniReceivedTargetPower;
-    int      delta_preamble_db; 
-    uint32_t contentionResolutionTimer; 
-    uint32_t maskIndex; 
-    int      preambleIndex;    
-    uint32_t new_ra_msg_len; 
-    
+    int      delta_preamble_db;
+    uint32_t contentionResolutionTimer;
+    uint32_t maskIndex;
+    int      preambleIndex;
+    uint32_t new_ra_msg_len;
+
     // Internal variables
-    uint32_t preambleTransmissionCounter; 
-    uint32_t backoff_param_ms; 
-    uint32_t sel_maskIndex; 
-    uint32_t sel_preamble; 
+    uint32_t preambleTransmissionCounter;
+    uint32_t backoff_param_ms;
+    uint32_t sel_maskIndex;
+    uint32_t sel_preamble;
     uint32_t backoff_interval_start;
     uint32_t backoff_inteval;
-    int      received_target_power_dbm; 
-    uint32_t ra_rnti; 
-    uint32_t current_ta; 
-    
-    srslte_softbuffer_rx_t softbuffer_rar; 
-    
-    
+    int      received_target_power_dbm;
+    uint32_t ra_rnti;
+    uint32_t current_ta;
+
+    srslte_softbuffer_rx_t softbuffer_rar;
+
     enum {
       IDLE = 0,
       INITIALIZATION,           // Section 5.1.1
@@ -163,36 +165,38 @@ private:
       COMPLETION,               // Section 5.1.6
       COMPLETION_DONE,
       RA_PROBLEM                // Section 5.1.5 last part
-    } state; 
-    
+    } state;
+
     typedef enum {RA_GROUP_A, RA_GROUP_B} ra_group_t;
-    
-    ra_group_t  last_msg3_group; 
-    bool        msg3_transmitted; 
-    bool        first_rar_received; 
+
+    ra_group_t  last_msg3_group;
+    bool        msg3_transmitted;
+    bool        first_rar_received;
     void        read_params();
-    
+
     phy_interface_mac *phy_h;
     srslte::log       *log_h;
-    srslte::timers    *timers_db;
     mux               *mux_unit;
     demux             *demux_unit;
     srslte::mac_pcap  *pcap;
     rrc_interface_mac *rrc;
 
+    srslte::timers::timer  *time_alignment_timer;
+    srslte::timers::timer  *contention_resolution_timer;
+
     mac_interface_rrc::ue_rnti_t *rntis;
     mac_interface_rrc::mac_cfg_t *mac_cfg;
-    
+
     uint64_t    transmitted_contention_id;
-    uint16_t    transmitted_crnti; 
-    
+    uint16_t    transmitted_crnti;
+
     enum {
-       PDCCH_CRNTI_NOT_RECEIVED = 0, 
-       PDCCH_CRNTI_UL_GRANT, 
-       PDCCH_CRNTI_DL_GRANT      
+       PDCCH_CRNTI_NOT_RECEIVED = 0,
+       PDCCH_CRNTI_UL_GRANT,
+       PDCCH_CRNTI_DL_GRANT
     } pdcch_to_crnti_received;
 
-    bool started_by_pdcch; 
+    bool started_by_pdcch;
     uint32_t rar_grant_nbytes;
     uint32_t rar_grant_tti;
     bool msg3_flushed;
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index e57af77ba..52aef7bd7 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -35,7 +35,6 @@
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/common/log.h"
 #include "mac/mux.h"
-#include "mac/mac_common.h"
 #include "mac/ul_sps.h"
 #include "srslte/common/mac_pcap.h"
 #include "srslte/common/timers.h"
@@ -55,9 +54,10 @@ public:
   
   ul_harq_entity() : proc(N)
   {
-    pcap        = NULL; 
-    timers_db   = NULL; 
-    mux_unit    = NULL; 
+    contention_timer = NULL;
+
+    pcap        = NULL;
+    mux_unit    = NULL;
     log_h       = NULL; 
     params      = NULL;
     rntis       = NULL; 
@@ -68,14 +68,14 @@ public:
   bool init(srslte::log *log_h_, 
             mac_interface_rrc_common::ue_rnti_t *rntis_,
             mac_interface_rrc_common::ul_harq_params_t *params_,
-            srslte::timers* timers_db_, 
+            srslte::timers::timer* contention_timer_,
             mux *mux_unit_)
   {
     log_h     = log_h_;
     mux_unit  = mux_unit_;
     params    = params_;
     rntis     = rntis_;
-    timers_db = timers_db_;
+    contention_timer = contention_timer_;
     for (uint32_t i=0;i<N;i++) {
       if (!proc[i].init(i, this)) {
         return false;
@@ -350,7 +350,7 @@ private:
 
       // On every Msg3 retransmission, restart mac-ContentionResolutionTimer as defined in Section 5.1.5
       if (is_msg3) {
-        harq_entity->timers_db->get(CONTENTION_TIMER)->reset();
+        harq_entity->contention_timer->reset();
       }
 
       harq_entity->mux_unit->pusch_retx(tti_tx, pid);
@@ -415,7 +415,7 @@ private:
   
   ul_sps           ul_sps_assig;
 
-  srslte::timers  *timers_db;
+  srslte::timers::timer  *contention_timer;
   mux             *mux_unit;
   std::vector<ul_harq_process> proc;
   srslte::log     *log_h;
diff --git a/srsue/src/mac/demux.cc b/srsue/src/mac/demux.cc
index 4fcb402fc..75bafd1b4 100644
--- a/srsue/src/mac/demux.cc
+++ b/srsue/src/mac/demux.cc
@@ -40,12 +40,12 @@ demux::demux(uint8_t nof_harq_proc_) : mac_msg(20), pending_mac_msg(20), nof_har
 {
 }
 
-void demux::init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc_, srslte::log* log_h_, srslte::timers* timers_db_)
+void demux::init(phy_interface_mac_common* phy_h_, rlc_interface_mac *rlc_, srslte::log* log_h_, srslte::timers::timer* time_alignment_timer_)
 {
   phy_h     = phy_h_; 
   log_h     = log_h_; 
-  rlc       = rlc_;  
-  timers_db = timers_db_;
+  rlc       = rlc_;
+  time_alignment_timer = time_alignment_timer_;
   pdus.init(this, log_h);
 }
 
@@ -190,8 +190,8 @@ bool demux::process_ce(srslte::sch_subh *subh) {
       Info("Received TA=%d\n", subh->get_ta_cmd());
       
       // Start or restart timeAlignmentTimer
-      timers_db->get(TIME_ALIGNMENT)->reset();
-      timers_db->get(TIME_ALIGNMENT)->run();
+      time_alignment_timer->reset();
+      time_alignment_timer->run();
       break;
     case srslte::sch_subh::PADDING:
       break;
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index a74908db2..665fd40e9 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -42,56 +42,60 @@
 namespace srsue {
 
 mac::mac() : ttisync(10240), 
-             timers_db((uint32_t) NOF_MAC_TIMERS),
+             timers(64),
+             timers_thread(&timers),
              mux_unit(MAC_NOF_HARQ_PROC),
              demux_unit(SRSLTE_MAX_TB*MAC_NOF_HARQ_PROC),
              pdu_process_thread(&demux_unit)
 {
-  started = false;  
-  pcap    = NULL;   
+  started = false;
+  pcap    = NULL;
   bzero(&metrics, sizeof(mac_metrics_t));
 }
-  
+
 bool mac::init(phy_interface_mac *phy, rlc_interface_mac *rlc, rrc_interface_mac *rrc, srslte::log *log_h_)
 {
-  started = false; 
+  started = false;
   phy_h = phy;
-  rlc_h = rlc;   
-  rrc_h = rrc;   
-  log_h = log_h_; 
-  tti = 0; 
-  is_synchronized = false;   
-  last_temporal_crnti = 0; 
-  phy_rnti = 0; 
-  
+  rlc_h = rlc;
+  rrc_h = rrc;
+  log_h = log_h_;
+  tti = 0;
+  is_synchronized = false;
+  last_temporal_crnti = 0;
+  phy_rnti = 0;
+
   srslte_softbuffer_rx_init(&pch_softbuffer, 100);
-  
-  bsr_procedure.init(       rlc_h, log_h,          &config, &timers_db);
-  phr_procedure.init(phy_h,        log_h,          &config,                &timers_db);
+
+  timer_alignment             = timers.get_unique_id();
+  contention_resolution_timer = timers.get_unique_id();
+
+  bsr_procedure.init(       rlc_h, log_h,          &config,                &timers);
+  phr_procedure.init(phy_h,        log_h,          &config,                &timers);
   mux_unit.init     (       rlc_h, log_h,                                              &bsr_procedure, &phr_procedure);
-  demux_unit.init   (phy_h, rlc_h, log_h,                                  &timers_db);
-  ra_procedure.init (phy_h, rrc,   log_h, &uernti, &config,                &timers_db, &mux_unit, &demux_unit);
+  demux_unit.init   (phy_h, rlc_h, log_h,                                  timers.get(timer_alignment));
+  ra_procedure.init (phy_h, rrc,   log_h, &uernti, &config,                timers.get(timer_alignment), timers.get(contention_resolution_timer), &mux_unit, &demux_unit);
   sr_procedure.init (phy_h, rrc,   log_h,          &config);
-  ul_harq.init      (              log_h, &uernti, &config.ul_harq_params, &timers_db, &mux_unit);
-  dl_harq.init      (              log_h,                                  &timers_db, &demux_unit);
+  ul_harq.init      (              log_h, &uernti, &config.ul_harq_params, timers.get(contention_resolution_timer), &mux_unit);
+  dl_harq.init      (              log_h,                                  timers.get(timer_alignment),             &demux_unit);
 
   reset();
-  
-  started = true; 
+
+  started = true;
   start(MAC_MAIN_THREAD_PRIO);
-  
-  
-  return started; 
+
+
+  return started;
 }
 
 void mac::stop()
 {
   srslte_softbuffer_rx_free(&pch_softbuffer);
 
-  started = false;   
+  started = false;
   ttisync.increase();
-  upper_timers_thread.thread_cancel();
   pdu_process_thread.stop();
+  timers_thread.stop();
   wait_thread_finish();
 }
 
@@ -116,8 +120,7 @@ void mac::reset()
 
   Info("Resetting MAC\n");
 
-  timers_db.stop_all();
-  upper_timers_thread.reset();
+  timers.stop_all();
 
   ul_harq.reset_ndi();
 
@@ -157,7 +160,8 @@ void mac::run_thread() {
       tti = phy_h->get_current_tti();
 
       log_h->step(tti);
-      timers_db.step_all();
+
+      timers_thread.tti_clock();
 
       // Step all procedures
       bsr_procedure.step(tti);
@@ -297,7 +301,7 @@ void mac::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
   /* Start PHR Periodic timer on first UL grant */
   if (is_first_ul_grant) {
     is_first_ul_grant = false;
-    timers_db.get(PHR_TIMER_PERIODIC)->run();
+    phr_procedure.start_timer();
   }
   if (grant.rnti_type == SRSLTE_RNTI_USER && ra_procedure.is_contention_resolution()) {
     ra_procedure.pdcch_to_crnti(true);
@@ -343,25 +347,23 @@ void mac::setup_timers()
 {
   int value = liblte_rrc_time_alignment_timer_num[config.main.time_alignment_timer];
   if (value > 0) {
-    timers_db.get(TIME_ALIGNMENT)->set(this, value);
+    timers.get(timer_alignment)->set(this, value);
   }
 }
 
 void mac::timer_expired(uint32_t timer_id)
 {
-  switch(timer_id) {
-    case TIME_ALIGNMENT:
-      timeAlignmentTimerExpire();
-      break;
-    default:
-      break;
+  if(timer_id == timer_alignment) {
+    timer_alignment_expire();
+  } else {
+    Warning("Received callback from unknown timer_id=%d\n", timer_id);
   }
 }
 
 /* Function called on expiry of TimeAlignmentTimer */
-void mac::timeAlignmentTimerExpire()
+void mac::timer_alignment_expire()
 {
-  printf("timeAlignmentTimer has expired value=%d ms\n", timers_db.get(TIME_ALIGNMENT)->get_timeout());
+  printf("TimeAlignment timer has expired value=%d ms\n", timers.get(timer_alignment)->get_timeout());
   rrc_h->release_pucch_srs();
   dl_harq.reset();
   ul_harq.reset();
@@ -414,24 +416,7 @@ void mac::setup_lcid(uint32_t lcid, uint32_t lcg, uint32_t priority, int PBR_x_t
   bsr_procedure.set_priority(lcid, priority);
 }
 
-uint32_t mac::get_unique_id()
-{
-  return upper_timers_thread.get_unique_id();
-}
-
-/* Front-end to upper-layer timers */
-srslte::timers::timer* mac::get(uint32_t timer_id)
-{
-  return upper_timers_thread.get(timer_id);
-}
-
-void mac::free(uint32_t timer_id)
-{
-  upper_timers_thread.free(timer_id);
-}
-
-
-  void mac::get_metrics(mac_metrics_t &m)
+void mac::get_metrics(mac_metrics_t &m)
 {
   Info("DL retx: %.2f \%%, perpkt: %.2f, UL retx: %.2f \%% perpkt: %.2f\n", 
        metrics.rx_pkts?((float) 100*metrics.rx_errors/metrics.rx_pkts):0.0, 
@@ -445,47 +430,59 @@ void mac::free(uint32_t timer_id)
 }
 
 
+
+
 /********************************************************
  *
- * Class to run upper-layer timers with normal priority
+ * Interface for timers used by upper layers
  *
  *******************************************************/
-
-mac::upper_timers::upper_timers() : timers_db(MAC_NOF_UPPER_TIMERS)
+srslte::timers::timer* mac::timer_get(uint32_t timer_id)
 {
-  start_periodic(1000, MAC_MAIN_THREAD_PRIO+1);
+  return timers.get(timer_id);
 }
 
-void mac::upper_timers::run_period()
+void mac::timer_release_id(uint32_t timer_id)
 {
-  timers_db.step_all();
+  timers.release_id(timer_id);
 }
 
-srslte::timers::timer* mac::upper_timers::get(uint32_t timer_id)
+uint32_t mac::timer_get_unique_id()
 {
-  return timers_db.get(timer_id%MAC_NOF_UPPER_TIMERS);
+  return timers.get_unique_id();
 }
 
-uint32_t mac::upper_timers::get_unique_id()
+
+/********************************************************
+ *
+ * Class that runs timers in lower priority
+ *
+ *******************************************************/
+void mac::timer_thread::tti_clock()
 {
-  return timers_db.get_unique_id();
+  ttisync.increase();
 }
 
-void mac::upper_timers::reset()
+void mac::timer_thread::run_thread()
 {
-  timers_db.stop_all();
+  running=true;
+  ttisync.set_producer_cntr(0);
+  ttisync.resync();
+  while(running) {
+    ttisync.wait();
+    timers->step_all();
+  }
 }
 
-
-void mac::upper_timers::free(uint32_t timer_id)
+void mac::timer_thread::stop()
 {
-  timers_db.release_id(timer_id);
+  running=false;
+  ttisync.increase();
+  wait_thread_finish();
 }
 
 
-
-
-  /********************************************************
+/********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from
  * DEMUX unit
diff --git a/srsue/src/mac/proc_bsr.cc b/srsue/src/mac/proc_bsr.cc
index 3117eeffc..898943ab9 100644
--- a/srsue/src/mac/proc_bsr.cc
+++ b/srsue/src/mac/proc_bsr.cc
@@ -50,17 +50,21 @@ void bsr_proc::init(rlc_interface_mac *rlc_, srslte::log* log_h_, mac_interface_
   log_h     = log_h_; 
   rlc       = rlc_; 
   mac_cfg   = mac_cfg_;
-  timers_db = timers_db_; 
+  timers_db = timers_db_;
+
+  timer_periodic_id = timers_db->get_unique_id();
+  timer_retx_id     = timers_db->get_unique_id();
+
   reset();
   initiated = true;
 }
 
 void bsr_proc::reset()
 {
-  timers_db->get(BSR_TIMER_PERIODIC)->stop();
-  timers_db->get(BSR_TIMER_PERIODIC)->reset();
-  timers_db->get(BSR_TIMER_RETX)->stop();
-  timers_db->get(BSR_TIMER_RETX)->reset();
+  timers_db->get(timer_periodic_id)->stop();
+  timers_db->get(timer_periodic_id)->reset();
+  timers_db->get(timer_retx_id)->stop();
+  timers_db->get(timer_retx_id)->reset();
   
   reset_sr = false; 
   sr_is_sent = false; 
@@ -77,23 +81,20 @@ void bsr_proc::reset()
 
 /* Process Periodic BSR */
 void bsr_proc::timer_expired(uint32_t timer_id) {
-  switch(timer_id) {
-    case BSR_TIMER_PERIODIC:
-      if (triggered_bsr_type == NONE) {
-        // Check condition 4 in Sec 5.4.5 
-        triggered_bsr_type = PERIODIC; 
-        Debug("BSR:   Triggering Periodic BSR\n");
-      }
-      break;
-    case BSR_TIMER_RETX:
-      // Enable reTx of SR only if periodic timer is not infinity
-      int periodic = liblte_rrc_periodic_bsr_timer_num[mac_cfg->main.ulsch_cnfg.periodic_bsr_timer];
-      if (periodic >= 0) {
-        triggered_bsr_type = REGULAR; 
-        Debug("BSR:   Triggering BSR reTX\n");
-        sr_is_sent = false; 
-      }
-      break;      
+  if(timer_id == timer_periodic_id) {
+    if (triggered_bsr_type == NONE) {
+      // Check condition 4 in Sec 5.4.5
+      triggered_bsr_type = PERIODIC;
+      Debug("BSR:   Triggering Periodic BSR\n");
+    }
+  } else if (timer_id == timer_retx_id) {
+    // Enable reTx of SR only if periodic timer is not infinity
+    int periodic = liblte_rrc_periodic_bsr_timer_num[mac_cfg->main.ulsch_cnfg.periodic_bsr_timer];
+    if (periodic >= 0) {
+      triggered_bsr_type = REGULAR;
+      Debug("BSR:   Triggering BSR reTX\n");
+      sr_is_sent = false;
+    }
   }
 }
 
@@ -222,17 +223,17 @@ void bsr_proc::step(uint32_t tti)
   }  
   
   int periodic = liblte_rrc_periodic_bsr_timer_num[mac_cfg->main.ulsch_cnfg.periodic_bsr_timer];
-  if (periodic > 0 && (uint32_t)periodic != timers_db->get(BSR_TIMER_PERIODIC)->get_timeout())
+  if (periodic > 0 && (uint32_t)periodic != timers_db->get(timer_periodic_id)->get_timeout())
   {
-    timers_db->get(BSR_TIMER_PERIODIC)->set(this, periodic);
-    timers_db->get(BSR_TIMER_PERIODIC)->run();
+    timers_db->get(timer_periodic_id)->set(this, periodic);
+    timers_db->get(timer_periodic_id)->run();
     Info("BSR:   Configured timer periodic %d ms\n", periodic);    
   }      
   int retx = liblte_rrc_retransmission_bsr_timer_num[mac_cfg->main.ulsch_cnfg.retx_bsr_timer];
-  if (retx > 0 && (uint32_t)retx != timers_db->get(BSR_TIMER_RETX)->get_timeout())
+  if (retx > 0 && (uint32_t)retx != timers_db->get(timer_retx_id)->get_timeout())
   {
-    timers_db->get(BSR_TIMER_RETX)->set(this, retx);
-    timers_db->get(BSR_TIMER_RETX)->run();
+    timers_db->get(timer_retx_id)->set(this, retx);
+    timers_db->get(timer_retx_id)->run();
     Info("BSR:   Configured timer reTX %d ms\n", retx);
   }
 
@@ -309,18 +310,18 @@ bool bsr_proc::need_to_send_bsr_on_ul_grant(uint32_t grant_size, bsr_t *bsr)
           grant_size, total_data, bsr_sz);
       ret = true; 
     }    
-    if (timers_db->get(BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
-      timers_db->get(BSR_TIMER_PERIODIC)->reset();
-      timers_db->get(BSR_TIMER_PERIODIC)->run();
+    if (timers_db->get(timer_periodic_id)->get_timeout() && bsr->format != TRUNC_BSR) {
+      timers_db->get(timer_periodic_id)->reset();
+      timers_db->get(timer_periodic_id)->run();
     }
   }
   // Cancel all triggered BSR and SR     
   triggered_bsr_type = NONE; 
   reset_sr = true;     
   // Restart or Start ReTX timer
-  if (timers_db->get(BSR_TIMER_RETX)->get_timeout()) {
-    timers_db->get(BSR_TIMER_RETX)->reset();
-    timers_db->get(BSR_TIMER_RETX)->run();
+  if (timers_db->get(timer_retx_id)->get_timeout()) {
+    timers_db->get(timer_retx_id)->reset();
+    timers_db->get(timer_retx_id)->run();
   }
   return ret;   
 }
@@ -340,9 +341,9 @@ bool bsr_proc::generate_padding_bsr(uint32_t nof_padding_bytes, bsr_t *bsr)
          bsr_type_tostring(triggered_bsr_type), bsr_format_tostring(bsr->format), 
          bsr->buff_size[0], bsr->buff_size[1], bsr->buff_size[2], bsr->buff_size[3]);
     
-    if (timers_db->get(BSR_TIMER_PERIODIC)->get_timeout() && bsr->format != TRUNC_BSR) {
-      timers_db->get(BSR_TIMER_PERIODIC)->reset();
-      timers_db->get(BSR_TIMER_PERIODIC)->run();
+    if (timers_db->get(timer_periodic_id)->get_timeout() && bsr->format != TRUNC_BSR) {
+      timers_db->get(timer_periodic_id)->reset();
+      timers_db->get(timer_periodic_id)->run();
     }    
     
   }
diff --git a/srsue/src/mac/proc_phr.cc b/srsue/src/mac/proc_phr.cc
index 11828f306..ead0bf153 100644
--- a/srsue/src/mac/proc_phr.cc
+++ b/srsue/src/mac/proc_phr.cc
@@ -49,14 +49,18 @@ void phr_proc::init(phy_interface_mac* phy_h_, srslte::log* log_h_, mac_interfac
   mac_cfg   = mac_cfg_;
   timers_db = timers_db_; 
   initiated = true;
+
+  timer_periodic_id = timers_db->get_unique_id();
+  timer_prohibit_id = timers_db->get_unique_id();
+
   reset();
 }
 
 void phr_proc::reset()
 {
   phr_is_triggered = false; 
-  timer_periodic = -2; 
-  timer_prohibit = -2;
+  timer_periodic_value = -2;
+  timer_prohibit_value = -2;
   dl_pathloss_change = -2; 
 }
 
@@ -72,23 +76,26 @@ bool phr_proc::pathloss_changed() {
     return false;
   }
 }
+    
+void phr_proc::start_timer() {
+  timers_db->get(timer_periodic_id)->run();
+}    
 
 /* Trigger PHR when timers exire */
 void phr_proc::timer_expired(uint32_t timer_id) {
-  switch(timer_id) {
-    case PHR_TIMER_PERIODIC:
-      timers_db->get(PHR_TIMER_PERIODIC)->reset();
-      timers_db->get(PHR_TIMER_PERIODIC)->run();
-      Debug("PHR:   Triggered by timer periodic (timer expired).\n");
-      phr_is_triggered = true; 
-      break;
-    case PHR_TIMER_PROHIBIT:
-      int pathloss_db = liblte_rrc_dl_pathloss_change_num[mac_cfg->main.phr_cnfg.dl_pathloss_change];
-      if (pathloss_changed()) {
-        Info("PHR:   Triggered by pathloss difference. cur_pathloss_db=%f (timer expired)\n", last_pathloss_db);
-        phr_is_triggered = true; 
-      }
-      break;      
+  if(timer_id == timer_periodic_id) {
+    timers_db->get(timer_periodic_id)->reset();
+    timers_db->get(timer_periodic_id)->run();
+    Debug("PHR:   Triggered by timer periodic (timer expired).\n");
+    phr_is_triggered = true;
+  } else if (timer_id == timer_prohibit_id) {
+    int pathloss_db = liblte_rrc_dl_pathloss_change_num[mac_cfg->main.phr_cnfg.dl_pathloss_change];
+    if (pathloss_changed()) {
+      Info("PHR:   Triggered by pathloss difference. cur_pathloss_db=%f (timer expired)\n", last_pathloss_db);
+      phr_is_triggered = true;
+    }
+  } else {
+    log_h->warning("Received timer callback from unknown timer_id=%d\n", timer_id);
   }
 }
 
@@ -102,28 +109,28 @@ void phr_proc::step(uint32_t tti)
     int cfg_timer_periodic = liblte_rrc_periodic_phr_timer_num[mac_cfg->main.phr_cnfg.periodic_phr_timer];
     
     // Setup timers and trigger PHR when configuration changed by higher layers
-    if (timer_periodic != cfg_timer_periodic && cfg_timer_periodic > 0) 
+    if (timer_periodic_value != cfg_timer_periodic && cfg_timer_periodic > 0)
     {
-      timer_periodic = cfg_timer_periodic; 
-      timers_db->get(PHR_TIMER_PERIODIC)->set(this, timer_periodic);
-      timers_db->get(PHR_TIMER_PERIODIC)->run();
+      timer_periodic_value = cfg_timer_periodic;
+      timers_db->get(timer_periodic_id)->set(this, timer_periodic_value);
+      timers_db->get(timer_periodic_id)->run();
       phr_is_triggered = true; 
-      Info("PHR:   Configured timer periodic %d ms\n", timer_periodic);
+      Info("PHR:   Configured timer periodic %d ms\n", timer_periodic_value);
     }
 
   }
 
   int cfg_timer_prohibit = liblte_rrc_prohibit_phr_timer_num[mac_cfg->main.phr_cnfg.prohibit_phr_timer];
 
-  if (timer_prohibit != cfg_timer_prohibit && cfg_timer_prohibit > 0) 
+  if (timer_prohibit_value != cfg_timer_prohibit && cfg_timer_prohibit > 0)
   {
-    timer_prohibit = cfg_timer_prohibit; 
-    timers_db->get(PHR_TIMER_PROHIBIT)->set(this, timer_prohibit);
-    timers_db->get(PHR_TIMER_PROHIBIT)->run();
-    Info("PHR:   Configured timer prohibit %d ms\n", timer_prohibit);
+    timer_prohibit_value = cfg_timer_prohibit;
+    timers_db->get(timer_prohibit_id)->set(this, timer_prohibit_value);
+    timers_db->get(timer_prohibit_id)->run();
+    Info("PHR:   Configured timer prohibit %d ms\n", timer_prohibit_value);
     phr_is_triggered = true; 
   }  
-  if (pathloss_changed() && timers_db->get(PHR_TIMER_PROHIBIT)->is_expired())
+  if (pathloss_changed() && timers_db->get(timer_prohibit_id)->is_expired())
   {
     Info("PHR:   Triggered by pathloss difference. cur_pathloss_db=%f\n", last_pathloss_db);
     phr_is_triggered = true;        
@@ -140,10 +147,10 @@ bool phr_proc::generate_phr_on_ul_grant(float *phr)
     
     Debug("PHR:   Generating PHR=%f\n", phr?*phr:0.0);
     
-    timers_db->get(PHR_TIMER_PERIODIC)->reset();
-    timers_db->get(PHR_TIMER_PROHIBIT)->reset();
-    timers_db->get(PHR_TIMER_PERIODIC)->run();
-    timers_db->get(PHR_TIMER_PROHIBIT)->run();
+    timers_db->get(timer_periodic_id)->reset();
+    timers_db->get(timer_prohibit_id)->reset();
+    timers_db->get(timer_periodic_id)->run();
+    timers_db->get(timer_prohibit_id)->run();
     
     phr_is_triggered = false; 
     
diff --git a/srsue/src/mac/proc_ra.cc b/srsue/src/mac/proc_ra.cc
index 3c5adacd9..4bceef2a5 100644
--- a/srsue/src/mac/proc_ra.cc
+++ b/srsue/src/mac/proc_ra.cc
@@ -53,7 +53,8 @@ void ra_proc::init(phy_interface_mac* phy_h_,
                    srslte::log* log_h_, 
                    mac_interface_rrc::ue_rnti_t *rntis_, 
                    mac_interface_rrc::mac_cfg_t *mac_cfg_, 
-                   srslte::timers* timers_db_,
+                   srslte::timers::timer* time_alignment_timer_,
+                   srslte::timers::timer* contention_resolution_timer_,
                    mux* mux_unit_, 
                    demux* demux_unit_)
 {
@@ -61,10 +62,13 @@ void ra_proc::init(phy_interface_mac* phy_h_,
   log_h     = log_h_; 
   mac_cfg   = mac_cfg_;
   rntis     = rntis_;
-  timers_db = timers_db_;
   mux_unit  = mux_unit_; 
   demux_unit= demux_unit_; 
-  rrc       = rrc_; 
+  rrc       = rrc_;
+
+  time_alignment_timer        = time_alignment_timer_;
+  contention_resolution_timer = contention_resolution_timer_;
+  
   srslte_softbuffer_rx_init(&softbuffer_rar, 10);
   
   // Tell demux to call us when a UE CRID is received
@@ -119,7 +123,7 @@ void ra_proc::read_params() {
   delta_preamble_db         = delta_preamble_db_table[configIndex%5]; 
   
   if (contentionResolutionTimer > 0) {
-    timers_db->get(CONTENTION_TIMER)->set(this, contentionResolutionTimer);
+    contention_resolution_timer->set(this, contentionResolutionTimer);
   }
 
 }
@@ -169,14 +173,14 @@ void ra_proc::process_timeadv_cmd(uint32_t ta) {
   if (preambleIndex == 0) {
     // Preamble not selected by UE MAC 
     phy_h->set_timeadv_rar(ta);
-    timers_db->get(TIME_ALIGNMENT)->reset();
-    timers_db->get(TIME_ALIGNMENT)->run();
+    time_alignment_timer->reset();
+    time_alignment_timer->run();
     Debug("Applying RAR TA CMD %d\n", ta);
   } else {
     // Preamble selected by UE MAC 
-    if (!timers_db->get(TIME_ALIGNMENT)->is_running()) {
+    if (!time_alignment_timer->is_running()) {
       phy_h->set_timeadv_rar(ta);
-      timers_db->get(TIME_ALIGNMENT)->run();
+      time_alignment_timer->run();
       Debug("Applying RAR TA CMD %d\n", ta);
     } else {
       // Ignore TA CMD
@@ -361,8 +365,8 @@ void ra_proc::tb_decoded_ok() {
         state = CONTENTION_RESOLUTION;
         
         // Start contention resolution timer 
-        timers_db->get(CONTENTION_TIMER)->reset();
-        timers_db->get(CONTENTION_TIMER)->run();
+        contention_resolution_timer->reset();
+        contention_resolution_timer->run();
       }  
     } else {
       rDebug("Found RAR for preamble %d\n", rar_pdu_msg.get()->get_rapid());
@@ -423,7 +427,7 @@ bool ra_proc::contention_resolution_id_received(uint64_t rx_contention_id) {
   rDebug("MAC PDU Contains Contention Resolution ID CE\n");
   
   // MAC PDU successfully decoded and contains MAC CE contention Id
-  timers_db->get(CONTENTION_TIMER)->stop();
+  contention_resolution_timer->stop();
   
   if (transmitted_contention_id == rx_contention_id) 
   {    
@@ -459,7 +463,7 @@ void ra_proc::step_contention_resolution() {
             (started_by_pdcch && pdcch_to_crnti_received != PDCCH_CRNTI_NOT_RECEIVED))
       {
         rDebug("PDCCH for C-RNTI received\n");
-        timers_db->get(CONTENTION_TIMER)->stop();
+        contention_resolution_timer->stop();
         rntis->temp_rnti = 0; 
         state = COMPLETION;           
       }            
@@ -571,7 +575,7 @@ void ra_proc::pdcch_to_crnti(bool contains_uplink_grant) {
 
 void ra_proc::harq_retx()
 {
-  timers_db->get(CONTENTION_TIMER)->reset();
+  contention_resolution_timer->reset();
 }
 
 }
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 8f064dd3a..43ec73187 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -93,9 +93,9 @@ void rrc::init(phy_interface_rrc *phy_,
   pthread_mutex_init(&mutex, NULL);
 
   ue_category = SRSLTE_UE_CATEGORY;
-  t301 = mac_timers->get_unique_id();
-  t310 = mac_timers->get_unique_id();
-  t311 = mac_timers->get_unique_id();
+  t301 = mac_timers->timer_get_unique_id();
+  t310 = mac_timers->timer_get_unique_id();
+  t311 = mac_timers->timer_get_unique_id();
 
   transaction_id = 0;
 
@@ -233,9 +233,9 @@ void rrc::run_thread() {
         set_phy_default();
         set_mac_default();
         mac->pcch_start_rx();
-        mac_timers->get(t311)->run();
-        mac_timers->get(t310)->stop();
-        mac_timers->get(t311)->stop();
+        mac_timers->timer_get(t311)->run();
+        mac_timers->timer_get(t310)->stop();
+        mac_timers->timer_get(t311)->stop();
         state = RRC_STATE_IDLE;
         break;
       default:
@@ -475,11 +475,11 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 // Detection of physical layer problems (5.3.11.1)
 void rrc::out_of_sync() {
     current_cell->in_sync = false;
-  if (!mac_timers->get(t311)->is_running() && !mac_timers->get(t310)->is_running()) {
+  if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
     n310_cnt++;
     if (n310_cnt == N310) {
-      mac_timers->get(t310)->reset();
-      mac_timers->get(t310)->run();
+      mac_timers->timer_get(t310)->reset();
+      mac_timers->timer_get(t310)->run();
       n310_cnt = 0;
       rrc_log->info("Detected %d out-of-sync from PHY. Starting T310 timer\n", N310);
     }
@@ -489,10 +489,10 @@ void rrc::out_of_sync() {
 // Recovery of physical layer problems (5.3.11.2)
 void rrc::in_sync() {
   current_cell->in_sync = true;
-  if (mac_timers->get(t310)->is_running()) {
+  if (mac_timers->timer_get(t310)->is_running()) {
     n311_cnt++;
     if (n311_cnt == N311) {
-      mac_timers->get(t310)->stop();
+      mac_timers->timer_get(t310)->stop();
       n311_cnt = 0;
       rrc_log->info("Detected %d in-sync from PHY. Stopping T310 timer\n", N311);
     }
@@ -652,9 +652,9 @@ void rrc::send_con_restablish_request() {
 
   rrc_log->info("Initiating RRC Connection Reestablishment Procedure\n");
   rrc_log->console("RRC Connection Reestablishment\n");
-  mac_timers->get(t310)->stop();
-  mac_timers->get(t311)->reset();
-  mac_timers->get(t311)->run();
+  mac_timers->timer_get(t310)->stop();
+  mac_timers->timer_get(t311)->reset();
+  mac_timers->timer_get(t311)->run();
 
   set_phy_default();
   mac->reset();
@@ -667,9 +667,9 @@ void rrc::send_con_restablish_request() {
     usleep(10000);
     timeout_cnt++;
   }
-  mac_timers->get(t301)->reset();
-  mac_timers->get(t301)->run();
-  mac_timers->get(t311)->stop();
+  mac_timers->timer_get(t301)->reset();
+  mac_timers->timer_get(t301)->run();
+  mac_timers->timer_get(t311)->stop();
   rrc_log->info("Cell Selection finished. Initiating transmission of RRC Connection Reestablishment Request\n");
 
   // Byte align and pack the message bits for PDCP
@@ -1337,15 +1337,15 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
                 liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
                 sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
 
-  mac_timers->get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
-  mac_timers->get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
-  mac_timers->get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+  mac_timers->timer_get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
+  mac_timers->timer_get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
+  mac_timers->timer_get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
   N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
   N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
 
   rrc_log->info("Set Constants and Timers: N310=%d, N311=%d, t301=%d, t310=%d, t311=%d\n",
-                N310, N311, mac_timers->get(t301)->get_timeout(),
-                mac_timers->get(t310)->get_timeout(), mac_timers->get(t311)->get_timeout());
+                N310, N311, mac_timers->timer_get(t301)->get_timeout(),
+                mac_timers->timer_get(t310)->get_timeout(), mac_timers->timer_get(t311)->get_timeout());
 
 }
 
@@ -1583,7 +1583,7 @@ void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
 
 /* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
 void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
-  mac_timers->get(t301)->stop();
+  mac_timers->timer_get(t301)->stop();
 
   // TODO: Restablish DRB1. Not done because never was suspended
 
@@ -1740,8 +1740,8 @@ void rrc::set_mac_default() {
 void rrc::set_rrc_default() {
   N310 = 1;
   N311 = 1;
-  mac_timers->get(t310)->set(this, 1000);
-  mac_timers->get(t311)->set(this, 1000);
+  mac_timers->timer_get(t310)->set(this, 1000);
+  mac_timers->timer_get(t311)->set(this, 1000);
 }
 
 

From c0fac73a84869d19e69e9e897f75f9bb356af7ec Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Mon, 18 Sep 2017 16:45:12 +0200
Subject: [PATCH 198/269] Solved PHICH Segmentation fault for MIMO

---
 lib/include/srslte/phy/phch/phich.h     | 14 ++------------
 lib/src/phy/phch/phich.c                | 23 +++--------------------
 lib/src/phy/phch/test/phich_file_test.c |  2 +-
 lib/src/phy/phch/test/phich_test.c      | 24 ++++++++++++++----------
 lib/src/phy/ue/ue_dl.c                  |  8 +-------
 5 files changed, 21 insertions(+), 50 deletions(-)

diff --git a/lib/include/srslte/phy/phch/phich.h b/lib/include/srslte/phy/phch/phich.h
index f560fdc80..0645e22b1 100644
--- a/lib/include/srslte/phy/phch/phich.h
+++ b/lib/include/srslte/phy/phch/phich.h
@@ -101,8 +101,8 @@ SRSLTE_API void srslte_phich_calc(srslte_phich_t *q,
                                   uint32_t *nseq); 
 
 SRSLTE_API int srslte_phich_decode(srslte_phich_t *q, 
-                                   cf_t *slot_symbols, 
-                                   cf_t *ce[SRSLTE_MAX_PORTS],
+                                   cf_t *slot_symbols[SRSLTE_MAX_PORTS],
+                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
                                    float noise_estimate, 
                                    uint32_t ngroup, 
                                    uint32_t nseq, 
@@ -110,16 +110,6 @@ SRSLTE_API int srslte_phich_decode(srslte_phich_t *q,
                                    uint8_t *ack, 
                                    float *distance);
 
-SRSLTE_API int srslte_phich_decode_multi(srslte_phich_t *q, 
-                                         cf_t *slot_symbols[SRSLTE_MAX_PORTS], 
-                                         cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
-                                         float noise_estimate, 
-                                         uint32_t ngroup, 
-                                         uint32_t nseq, 
-                                         uint32_t nsubframe, 
-                                         uint8_t *ack, 
-                                         float *distance);
-
 SRSLTE_API int srslte_phich_encode(srslte_phich_t *q, 
                                    uint8_t ack, 
                                    uint32_t ngroup, 
diff --git a/lib/src/phy/phch/phich.c b/lib/src/phy/phch/phich.c
index a7cb282fd..14a0d5426 100644
--- a/lib/src/phy/phch/phich.c
+++ b/lib/src/phy/phch/phich.c
@@ -173,26 +173,9 @@ void srslte_phich_ack_encode(uint8_t ack, uint8_t bits[SRSLTE_PHICH_NBITS]) {
   memset(bits, ack, 3 * sizeof(uint8_t));
 }
 
-int srslte_phich_decode(srslte_phich_t *q, cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate,
-    uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) 
-{
-  cf_t *_sf_symbols[SRSLTE_MAX_PORTS]; 
-  cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
-  
-  _sf_symbols[0] = sf_symbols; 
-  for (int i=0;i<q->cell.nof_ports;i++) {
-    _ce[i][0] = ce[i]; 
-  }
-
-  return srslte_phich_decode_multi(q, _sf_symbols, _ce, noise_estimate, ngroup, nseq, subframe, ack, distance);
-}
-/* Decodes the phich channel and saves the CFI in the cfi pointer.
- *
- * Returns 1 if successfully decoded the CFI, 0 if not and -1 on error
- */
-int srslte_phich_decode_multi(srslte_phich_t *q, cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
-                              uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) 
-{
+int srslte_phich_decode(srslte_phich_t *q, cf_t *sf_symbols[SRSLTE_MAX_PORTS],
+                        cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
+                        uint32_t ngroup, uint32_t nseq, uint32_t subframe, uint8_t *ack, float *distance) {
 
   /* Set pointers for layermapping & precoding */
   int i, j;
diff --git a/lib/src/phy/phch/test/phich_file_test.c b/lib/src/phy/phch/test/phich_file_test.c
index 0250c6f0b..968aa5cbc 100644
--- a/lib/src/phy/phch/test/phich_file_test.c
+++ b/lib/src/phy/phch/test/phich_file_test.c
@@ -263,7 +263,7 @@ int main(int argc, char **argv) {
   for (ngroup=0;ngroup<srslte_phich_ngroups(&phich);ngroup++) {
     for (nseq=0;nseq<max_nseq;nseq++) {
 
-      if (srslte_phich_decode(&phich, fft_buffer, ce, srslte_chest_dl_get_noise_estimate(&chest), ngroup, nseq, numsubframe, &ack_rx, &distance)<0) {
+      if (srslte_phich_decode(&phich, &fft_buffer, &ce, srslte_chest_dl_get_noise_estimate(&chest), ngroup, nseq, numsubframe, &ack_rx, &distance)<0) {
         printf("Error decoding ACK\n");
         exit(-1);
       }
diff --git a/lib/src/phy/phch/test/phich_test.c b/lib/src/phy/phch/test/phich_test.c
index 5334b210f..cffb85663 100644
--- a/lib/src/phy/phch/test/phich_test.c
+++ b/lib/src/phy/phch/test/phich_test.c
@@ -103,7 +103,7 @@ int main(int argc, char **argv) {
   srslte_phich_t phich;
   srslte_regs_t regs;
   int i, j;
-  cf_t *ce[SRSLTE_MAX_PORTS];
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
   int nof_re;
   cf_t *slot_symbols[SRSLTE_MAX_PORTS];
   uint8_t ack[50][SRSLTE_PHICH_NORM_NSEQUENCES], ack_rx;
@@ -120,13 +120,15 @@ int main(int argc, char **argv) {
 
   /* init memory */
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
-    ce[i] = malloc(sizeof(cf_t) * nof_re);
-    if (!ce[i]) {
-      perror("malloc");
-      exit(-1);
-    }
-    for (j=0;j<nof_re;j++) {
-      ce[i][j] = 1;
+    for (int k=0;k<SRSLTE_MAX_PORTS;k++) {
+      ce[k][i] = malloc(sizeof(cf_t) * nof_re);
+      if (!ce[k][i]) {
+        perror("malloc");
+        exit(-1);
+      }
+      for (j = 0; j < nof_re; j++) {
+        ce[k][i][j] = 1;
+      }
     }
     slot_symbols[i] = malloc(sizeof(cf_t) * nof_re);
     if (!slot_symbols[i]) {
@@ -185,7 +187,7 @@ int main(int argc, char **argv) {
       for (ngroup=0;ngroup<srslte_phich_ngroups(&phich);ngroup++) {
         for (nseq=0;nseq<max_nseq;nseq++) {
 
-          if (srslte_phich_decode(&phich, slot_symbols[0], ce, 0, ngroup, nseq, nsf, &ack_rx, &distance)<0) {
+          if (srslte_phich_decode(&phich, slot_symbols, ce, 0, ngroup, nseq, nsf, &ack_rx, &distance)<0) {
             printf("Error decoding ACK\n");
             exit(-1);
           }
@@ -208,7 +210,9 @@ int main(int argc, char **argv) {
   srslte_phich_free(&phich);
 
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
-    free(ce[i]);
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      free(ce[i][j]);
+    }
     free(slot_symbols[i]);
   }
   printf("OK\n");
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 46dfea566..cbeb6ce87 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -776,13 +776,7 @@ bool srslte_ue_dl_decode_phich(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t n_pr
     sf_idx, n_prb_lowest, n_dmrs, ngroup, nseq, 
     srslte_phich_ngroups(&q->phich), srslte_phich_nsf(&q->phich));
   
-  cf_t *ce0[SRSLTE_MAX_PORTS];
-  for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
-    ce0[i] = q->ce_m[i][0];
-  }
-
-  
-  if (!srslte_phich_decode(&q->phich, q->sf_symbols_m[0], ce0, 0, ngroup, nseq, sf_idx, &ack_bit, &distance)) {
+  if (!srslte_phich_decode(&q->phich, q->sf_symbols_m, q->ce_m, 0, ngroup, nseq, sf_idx, &ack_bit, &distance)) {
     INFO("Decoded PHICH %d with distance %f\n", ack_bit, distance);    
   } else {
     fprintf(stderr, "Error decoding PHICH\n");

From 648f7c35912db2c69177e269c6d7db3a9674d5f8 Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Tue, 19 Sep 2017 10:32:29 +0200
Subject: [PATCH 199/269] Increased compatibility for TM3 and TM4 with one Rx
 antenna

---
 lib/src/phy/mimo/precoding.c |  4 +--
 lib/src/phy/phch/pdsch.c     | 19 +++++++++---
 lib/src/phy/ue/ue_dl.c       | 60 +++++++++++++++++-------------------
 srsue/src/phy/phch_worker.cc | 33 +++++++++++++++-----
 4 files changed, 71 insertions(+), 45 deletions(-)

diff --git a/lib/src/phy/mimo/precoding.c b/lib/src/phy/mimo/precoding.c
index bf9047977..6f3e43889 100644
--- a/lib/src/phy/mimo/precoding.c
+++ b/lib/src/phy/mimo/precoding.c
@@ -1367,7 +1367,7 @@ int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_P
                                  int nof_rxant, int nof_ports, int nof_layers, int codebook_idx, int nof_symbols,
                                  float noise_estimate)
 {
-  if (nof_ports == 2 && nof_rxant == 2) {
+  if (nof_ports == 2 && nof_rxant <= 2) {
     if (nof_layers == 2) {
       switch (mimo_decoder) {
         case SRSLTE_MIMO_DECODER_ZF:
@@ -1407,7 +1407,7 @@ int srslte_predecoding_multiplex(cf_t *y[SRSLTE_MAX_PORTS], cf_t *h[SRSLTE_MAX_P
   } else if (nof_ports == 4) {
     ERROR("Error predecoding multiplex: not implemented for %d Tx ports", nof_ports);
   } else {
-    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennax %d\n", nof_ports, nof_rxant);
+    ERROR("Error predecoding multiplex: Invalid combination of ports %d and rx antennas %d\n", nof_ports, nof_rxant);
   }
   return SRSLTE_ERROR;
 }
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 16f72a37e..de553630e 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -246,7 +246,7 @@ static int pdsch_init(srslte_pdsch_t *q, uint32_t max_prb, bool is_ue, uint32_t
         goto clean;
       }
       if (q->is_ue) {
-        for (int j=0;j<q->nof_rx_antennas;j++) {
+        for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
           q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
           if (!q->ce[i][j]) {
             goto clean;
@@ -309,7 +309,7 @@ void srslte_pdsch_free(srslte_pdsch_t *q) {
       free(q->symbols[i]);
     }
     if (q->is_ue) {
-      for (int j=0;j<q->nof_rx_antennas;j++) {
+      for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
         if (q->ce[i][j]) {
           free(q->ce[i][j]);
         }
@@ -713,8 +713,9 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
                                   cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate, uint32_t nof_ce,
                                   uint32_t pmi[SRSLTE_MAX_LAYERS], float sinr[SRSLTE_MAX_LAYERS][SRSLTE_MAX_CODEBOOKS]) {
 
-  if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
-    for (int nof_layers = 1; nof_layers <= 2; nof_layers++ ) {
+  if (q->cell.nof_ports == 2 && q->nof_rx_antennas <= 2) {
+    int nof_layers = 1;
+    for (; nof_layers <= q->nof_rx_antennas; nof_layers++ ) {
       if (sinr[nof_layers - 1] && pmi) {
         if (srslte_precoding_pmi_select(ce, nof_ce, noise_estimate, nof_layers, &pmi[nof_layers - 1],
                                         sinr[nof_layers - 1]) < 0) {
@@ -723,6 +724,16 @@ int srslte_pdsch_pmi_select(srslte_pdsch_t *q,
         }
       }
     }
+
+    /* FIXME: Set other layers to 0 */
+    for (; nof_layers <= SRSLTE_MAX_LAYERS; nof_layers++ ) {
+      if (sinr[nof_layers - 1] && pmi) {
+        for (int cb = 0; cb < SRSLTE_MAX_CODEBOOKS; cb++) {
+          sinr[nof_layers - 1][cb] = -INFINITY;
+        }
+        pmi[nof_layers - 1] = 0;
+      }
+    }
   } else {
     ERROR("Not implemented configuration");
     return SRSLTE_ERROR_INVALID_INPUTS;
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index cbeb6ce87..fd04a4c85 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -115,7 +115,7 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     }
     srslte_cfo_set_tol(&q->sfo_correct, 1e-5f/q->fft.symbol_sz);
     
-    for (int j=0;j<nof_rx_antennas;j++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
       q->sf_symbols_m[j] = srslte_vec_malloc(MAX_SFLEN_RE * sizeof(cf_t));
       if (!q->sf_symbols_m[j]) {
         perror("malloc");
@@ -127,6 +127,7 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
           perror("malloc");
           goto clean_exit; 
         }
+        bzero(q->ce_m[i][j], MAX_SFLEN_RE * sizeof(cf_t));
       }
     }
     
@@ -163,7 +164,7 @@ void srslte_ue_dl_free(srslte_ue_dl_t *q) {
         free(q->softbuffers[i]);
       }
     }
-    for (int j=0;j<q->nof_rx_antennas;j++) {
+    for (int j = 0; j < SRSLTE_MAX_PORTS; j++) {
       if (q->sf_symbols_m[j]) {
         free(q->sf_symbols_m[j]);
       }
@@ -516,10 +517,10 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
   float best_sinr = -INFINITY;
   uint8_t best_pmi = 0, best_ri = 0;
 
-  if (q->cell.nof_ports < 2 || q->nof_rx_antennas < 2) {
+  if (q->cell.nof_ports < 2) {
     /* Do nothing */
     return SRSLTE_SUCCESS;
-  } else if (q->cell.nof_ports == 2 && q->nof_rx_antennas == 2) {
+  } else {
     if (srslte_pdsch_pmi_select(&q->pdsch, &q->pdsch_cfg, q->ce_m, noise_estimate,
                                   SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp), q->pmi, q->sinr)) {
       ERROR("SINR calculation error");
@@ -527,7 +528,7 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
     }
 
     /* Select the best Rank indicator (RI) and Precoding Matrix Indicator (PMI) */
-    for (uint32_t nof_layers = 1; nof_layers <= 2; nof_layers++) {
+    for (uint32_t nof_layers = 1; nof_layers <= SRSLTE_MAX_LAYERS; nof_layers++) {
       float _sinr = q->sinr[nof_layers - 1][q->pmi[nof_layers - 1]] * nof_layers * nof_layers;
       if (_sinr > best_sinr + 0.1) {
         best_sinr = _sinr;
@@ -535,37 +536,34 @@ int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, flo
         best_ri = (uint8_t) (nof_layers - 1);
       }
     }
+  }
 
-    /* Set RI */
-    if (ri != NULL) {
-      *ri = best_ri;
-    }
+  /* Print Trace */
+  if (ri != NULL && pmi != NULL && current_sinr != NULL) {
+    INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
+         10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+  }
 
-    /* Set PMI */
-    if (pmi != NULL) {
-      *pmi = best_pmi;
-    }
+  /* Set RI */
+  if (ri != NULL) {
+    *ri = best_ri;
+  }
 
-    /* Set current SINR */
-    if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
-      if (q->pdsch_cfg.nof_layers == 1) {
-        *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
-      } else if (q->pdsch_cfg.nof_layers == 2) {
-        *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
-      } else {
-        ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
-        return SRSLTE_ERROR;
-      }
-    }
+  /* Set PMI */
+  if (pmi != NULL) {
+    *pmi = best_pmi;
+  }
 
-    /* Print Trace */
-    if (ri != NULL && pmi != NULL && current_sinr != NULL) {
-      INFO("PDSCH Select RI=%d; PMI=%d; Current SINR=%.1fdB (nof_layers=%d, codebook_idx=%d)\n", *ri, *pmi,
-           10*log10(*current_sinr), q->pdsch_cfg.nof_layers, q->pdsch_cfg.codebook_idx);
+  /* Set current SINR */
+  if (current_sinr != NULL && q->pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
+    if (q->pdsch_cfg.nof_layers == 1) {
+      *current_sinr = q->sinr[0][q->pdsch_cfg.codebook_idx];
+    } else if (q->pdsch_cfg.nof_layers == 2) {
+      *current_sinr = q->sinr[1][q->pdsch_cfg.codebook_idx - 1];
+    } else {
+      ERROR("Not implemented number of layers (%d)", q->pdsch_cfg.nof_layers);
+      return SRSLTE_ERROR;
     }
-  } else {
-    ERROR("Not implemented configuration");
-    return SRSLTE_ERROR_INVALID_INPUTS;
   }
 
   return SRSLTE_SUCCESS;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 2f99c2699..f1dff5f8d 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -255,9 +255,17 @@ void phch_worker::work_imp()
 
       /* Select Rank Indicator by computing Condition Number */
       if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_3) {
-        float cn = 0.0f;
-        srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
-        uci_data.uci_ri_len = 1;
+        if (ue_dl.nof_rx_antennas > 1) {
+          /* If 2 ort more receiving antennas, select RI */
+          float cn = 0.0f;
+          srslte_ue_dl_ri_select(&ue_dl, &uci_data.uci_ri, &cn);
+          uci_data.uci_ri_len = 1;
+        } else {
+          /* If only one receiving antenna, force RI for 1 layer */
+          uci_data.uci_ri = 0;
+          uci_data.uci_ri_len = 1;
+          Warning("Only one receiving antenna with TM3. Forcing RI=1 layer.\n");
+        }
       } else if (phy->config->dedicated.antenna_info_explicit_value.tx_mode == LIBLTE_RRC_TRANSMISSION_MODE_4){
         float sinr = 0.0f;
         uint8 packed_pmi = 0;
@@ -271,6 +279,11 @@ void phch_worker::work_imp()
           uci_data.uci_pmi_len = 1;
           uci_data.uci_dif_cqi_len = 3;
         }
+
+        /* If only one antenna in TM4 print limitation warning */
+        if (ue_dl.nof_rx_antennas < 2) {
+          Warning("Only one receiving antenna with TM4. Forcing RI=1 layer (PMI=%d).\n", packed_pmi);
+        }
       }
     }
   }
@@ -508,20 +521,22 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
     case LIBLTE_RRC_TRANSMISSION_MODE_3:
       if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = SRSLTE_MIMO_TYPE_TX_DIVERSITY;
-      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_CDD;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM3\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+        Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
+              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
       break;
     case LIBLTE_RRC_TRANSMISSION_MODE_4:
       if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 1) {
         mimo_type = (grant->pinfo == 0) ? SRSLTE_MIMO_TYPE_TX_DIVERSITY : SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
-      } else if (SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
+      } else if (ue_dl.nof_rx_antennas > 1 && SRSLTE_RA_DL_GRANT_NOF_TB(grant) == 2) {
         mimo_type = SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX;
       } else {
-        Error("Wrong number of transport blocks (%d) for TM4\n", SRSLTE_RA_DL_GRANT_NOF_TB(grant));
+        Error("Wrong combination of antennas (%d) or transport blocks (%d) for TM3\n", ue_dl.nof_rx_antennas,
+              SRSLTE_RA_DL_GRANT_NOF_TB(grant));
         valid_config = false;
       }
     break;
@@ -923,12 +938,14 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, sr=%s, cfo=%.1f Hz%s\n",
+  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f Hz%s\n",
          tx_power, (tti+4)%10240, 
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
        uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
        uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
        uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
+       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[1]?"1":"0"):"no",
+       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[0]?"1":"0"):"",
        uci_data.scheduling_request?"yes":"no",
          cfo*15000, timestr);        
   }   

From f8c5eb28d37644a11997ed4c099d12bbcacee2a5 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 19:22:20 +0200
Subject: [PATCH 200/269] Fix not showing L/U/O warnings

---
 srsue/hdr/ue.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index d5f5f9fc5..2bdb0f0ff 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -102,7 +102,7 @@ private:
 #else
     srslte::logger_file logger;
 #endif
-  srslte::log_filter  rf_log;
+  // rf_log is on ue_base
   srslte::log_filter  phy_log;
   srslte::log_filter  mac_log;
   srslte::log_filter  rlc_log;

From 8fd4ba7486ddfc57145114dda556740aa7ed9ea2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 19:57:43 +0200
Subject: [PATCH 201/269] added per-thread PHY logging

---
 srsue/hdr/phy/phch_common.h  |  4 ++--
 srsue/hdr/phy/phch_worker.h  |  3 ++-
 srsue/hdr/phy/phy.h          |  5 +++--
 srsue/hdr/ue.h               |  2 +-
 srsue/src/phy/phch_worker.cc | 24 +++++++++++++-----------
 srsue/src/phy/phy.cc         |  9 +++++----
 srsue/src/ue.cc              | 20 ++++++++++++++++----
 srsue/test/mac/mac_test.cc   | 36 ++++++++++++++++++++++--------------
 8 files changed, 64 insertions(+), 39 deletions(-)

diff --git a/srsue/hdr/phy/phch_common.h b/srsue/hdr/phy/phch_common.h
index 7f0075e33..04102fded 100644
--- a/srsue/hdr/phy/phch_common.h
+++ b/srsue/hdr/phy/phch_common.h
@@ -48,7 +48,6 @@ namespace srsue {
     /* Common variables used by all phy workers */
     phy_interface_rrc::phy_cfg_t *config; 
     phy_args_t                   *args; 
-    srslte::log       *log_h;
     mac_interface_phy *mac;
     srslte_ue_ul_t     ue_ul; 
     
@@ -116,7 +115,8 @@ namespace srsue {
     bool               is_first_of_burst;
     srslte::radio      *radio_h;
     float              cfo;
-    
+    srslte::log       *log_h;
+
     
     bool               ul_rnti_active(uint32_t tti);
     bool               dl_rnti_active(uint32_t tti);
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index e6fce4f02..307cea0f0 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -45,7 +45,7 @@ public:
   ~phch_worker();
   void  reset(); 
   void  set_common(phch_common *phy);
-  bool  init(uint32_t max_prb);
+  bool  init(uint32_t max_prb, srslte::log *log);
 
   bool  set_cell(srslte_cell_t cell);
 
@@ -113,6 +113,7 @@ private:
   
   /* Common objects */  
   phch_common    *phy;
+  srslte::log    *log_h;
   srslte_cell_t  cell;
   bool           mem_initiated;
   bool           cell_initiated;
diff --git a/srsue/hdr/phy/phy.h b/srsue/hdr/phy/phy.h
index 2df93b07b..0c77360b2 100644
--- a/srsue/hdr/phy/phy.h
+++ b/srsue/hdr/phy/phy.h
@@ -52,8 +52,8 @@ public:
   phy();
   bool init(srslte::radio_multi *radio_handler, 
             mac_interface_phy *mac, 
-            rrc_interface_phy *rrc, 
-            srslte::log *log_h, 
+            rrc_interface_phy *rrc,
+            std::vector<void*> log_vec,
             phy_args_t *args = NULL);
   
   void stop();
@@ -148,6 +148,7 @@ private:
   const static int WORKERS_THREAD_PRIO = 0; 
   
   srslte::radio_multi      *radio_handler;
+  std::vector<void*>        log_vec;
   srslte::log              *log_h;
   srsue::mac_interface_phy *mac;
   srsue::rrc_interface_phy *rrc;
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 2bdb0f0ff..473e01e0e 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -103,7 +103,7 @@ private:
     srslte::logger_file logger;
 #endif
   // rf_log is on ue_base
-  srslte::log_filter  phy_log;
+  std::vector<void*>  phy_log;
   srslte::log_filter  mac_log;
   srslte::log_filter  rlc_log;
   srslte::log_filter  pdcp_log;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index f1dff5f8d..b4b1169ed 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -30,10 +30,10 @@
 #include "srslte/interfaces/ue_interfaces.h"
 #include "srslte/asn1/liblte_rrc.h"
 
-#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) phy->log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) phy->log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) phy->log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
-#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) phy->log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Error(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Warning(fmt, ...) if (SRSLTE_DEBUG_ENABLED) log_h->warning_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
+#define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 
 
 /* This is to visualize the channel response */
@@ -106,8 +106,9 @@ void phch_worker::set_common(phch_common* phy_)
   phy = phy_;   
 }
 
-bool phch_worker::init(uint32_t max_prb)
+bool phch_worker::init(uint32_t max_prb, srslte::log *log_h)
 {
+  this->log_h = log_h;
   // ue_sync in phy.cc requires a buffer for 3 subframes
   for (uint32_t i=0;i<phy->args->nof_rx_ant;i++) {
     signal_buffer[i] = (cf_t*) srslte_vec_malloc(3 * sizeof(cf_t) * SRSLTE_SF_LEN_PRB(max_prb));
@@ -166,6 +167,7 @@ void phch_worker::set_tti(uint32_t tti_, uint32_t tx_tti_)
 {
   tti    = tti_; 
   tx_tti = tx_tti_;
+  log_h->step(tti);
 }
 
 void phch_worker::set_cfo(float cfo_)
@@ -477,7 +479,7 @@ bool phch_worker::decode_pdcch_dl(srsue::mac_interface_phy::mac_grant_t* grant)
 
     char hexstr[16];
     hexstr[0]='\0';
-    if (phy->log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
+    if (log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
       srslte_vec_sprint_hex(hexstr, dci_msg.data, dci_msg.nof_bits);
     }
     Info("PDCCH: DL DCI %s cce_index=%2d, L=%d, n_data_bits=%d, hex=%s\n", srslte_dci_format_string(dci_msg.format), 
@@ -678,7 +680,7 @@ bool phch_worker::decode_pdcch_ul(mac_interface_phy::mac_grant_t* grant)
       
       char hexstr[16];
       hexstr[0]='\0';
-      if (phy->log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
+      if (log_h->get_level() >= srslte::LOG_LEVEL_INFO) {
         srslte_vec_sprint_hex(hexstr, dci_msg.data, dci_msg.nof_bits);
       }
       // Change to last_location_ul
@@ -783,7 +785,7 @@ void phch_worker::set_uci_periodic_cqi()
         cqi_report.type = SRSLTE_CQI_TYPE_SUBBAND;
         cqi_report.subband.subband_cqi = srslte_cqi_from_snr(phy->avg_snr_db);
         cqi_report.subband.subband_label = 0;
-        phy->log_h->console("Warning: Subband CQI periodic reports not implemented\n");
+        log_h->console("Warning: Subband CQI periodic reports not implemented\n");
         Info("PUCCH: Periodic CQI=%d, SNR=%.1f dB\n", cqi_report.subband.subband_cqi, phy->avg_snr_db);
       } else {
         cqi_report.type = SRSLTE_CQI_TYPE_WIDEBAND;
@@ -1118,13 +1120,13 @@ void phch_worker::start_plot() {
 #ifdef ENABLE_GUI
   if (plot_worker_id == -1) {
     plot_worker_id = get_id();
-    phy->log_h->console("Starting plot for worker_id=%d\n", plot_worker_id);
+    log_h->console("Starting plot for worker_id=%d\n", plot_worker_id);
     init_plots(this);
   } else {
-    phy->log_h->console("Trying to start a plot but already started by worker_id=%d\n", plot_worker_id);
+    log_h->console("Trying to start a plot but already started by worker_id=%d\n", plot_worker_id);
   }
 #else 
-    phy->log_h->console("Trying to start a plot but plots are disabled (ENABLE_GUI constant in phch_worker.cc)\n");
+    log_h->console("Trying to start a plot but plots are disabled (ENABLE_GUI constant in phch_worker.cc)\n");
 #endif
 }
 
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index afce102b3..44b548c4e 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -96,12 +96,13 @@ bool phy::check_args(phy_args_t *args)
 }
 
 bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_interface_phy *rrc,
-               srslte::log *log_h, phy_args_t *phy_args) {
+               std::vector<void*> log_vec, phy_args_t *phy_args) {
 
   mlockall(MCL_CURRENT | MCL_FUTURE);
 
   n_ta = 0;
-  this->log_h         = log_h;
+  this->log_vec       = log_vec;
+  this->log_h         = (srslte::log*) log_vec[0];
   this->radio_handler = radio_handler;
   this->mac           = mac;
   this->rrc           = rrc;
@@ -128,12 +129,12 @@ bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_i
 void phy::run_thread() {
 
   prach_buffer.init(&config.common.prach_cnfg, SRSLTE_MAX_PRB, args, log_h);
-  workers_common.init(&config, args, log_h, radio_handler, mac);
+  workers_common.init(&config, args, (srslte::log*) log_vec[0], radio_handler, mac);
 
   // Add workers to workers pool and start threads
   for (uint32_t i=0;i<nof_workers;i++) {
     workers[i].set_common(&workers_common);
-    workers[i].init(SRSLTE_MAX_PRB);
+    workers[i].init(SRSLTE_MAX_PRB, (srslte::log*) log_vec[i]);
     workers_pool.init_worker(i, &workers[i], WORKERS_THREAD_PRIO, args->worker_cpu_mask);
   }
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 4619b43ce..aab6bbc08 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -56,7 +56,15 @@ bool ue::init(all_args_t *args_)
     logger.init(args->log.filename);
 #endif
   rf_log.init("RF  ", &logger);
-  phy_log.init("PHY ", &logger, true);
+  // Create array of pointers to phy_logs
+  for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
+    srslte::log_filter *mylog = new srslte::log_filter;
+    char tmp[16];
+    sprintf(tmp, "PHY%d",i);
+    mylog->init(tmp, &logger, true);
+    phy_log.push_back((void*) mylog);
+  }
+
   mac_log.init("MAC ", &logger, true);
   rlc_log.init("RLC ", &logger);
   pdcp_log.init("PDCP", &logger);
@@ -70,7 +78,9 @@ bool ue::init(all_args_t *args_)
   logger.log("\n\n");
 #endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
-  phy_log.set_level(level(args->log.phy_level));
+  for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
+    ((srslte::log_filter*) phy_log[i])->set_level(level(args->log.phy_level));
+  }
   mac_log.set_level(level(args->log.mac_level));
   rlc_log.set_level(level(args->log.rlc_level));
   pdcp_log.set_level(level(args->log.pdcp_level));
@@ -79,7 +89,9 @@ bool ue::init(all_args_t *args_)
   gw_log.set_level(level(args->log.gw_level));
   usim_log.set_level(level(args->log.usim_level));
 
-  phy_log.set_hex_limit(args->log.phy_hex_limit);
+  for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
+    ((srslte::log_filter*) phy_log[i])->set_hex_limit(args->log.phy_hex_limit);
+  }
   mac_log.set_hex_limit(args->log.mac_hex_limit);
   rlc_log.set_hex_limit(args->log.rlc_hex_limit);
   pdcp_log.set_hex_limit(args->log.pdcp_hex_limit);
@@ -104,7 +116,7 @@ bool ue::init(all_args_t *args_)
 
   // PHY initis in background, start before radio
   args->expert.phy.nof_rx_ant = args->rf.nof_rx_ant;
-  phy.init(&radio, &mac, &rrc, &phy_log, &args->expert.phy);
+  phy.init(&radio, &mac, &rrc, phy_log, &args->expert.phy);
 
   /* Start Radio */
   char *dev_name = NULL;
diff --git a/srsue/test/mac/mac_test.cc b/srsue/test/mac/mac_test.cc
index 368a7f35e..4cb47e9f8 100644
--- a/srsue/test/mac/mac_test.cc
+++ b/srsue/test/mac/mac_test.cc
@@ -427,22 +427,11 @@ private:
 
 int main(int argc, char *argv[])
 {
-  srslte::log_filter mac_log("MAC"), phy_log("PHY");
+  srslte::log_filter mac_log("MAC");
   rlctest my_rlc;  
   parse_args(&prog_args, argc, argv);
   
-  switch (prog_args.verbose) {
-    case 1:
-      mac_log.set_level(srslte::LOG_LEVEL_INFO);
-      phy_log.set_level(srslte::LOG_LEVEL_INFO);
-      break;
-    case 2: 
-      mac_log.set_level(srslte::LOG_LEVEL_DEBUG);
-      phy_log.set_level(srslte::LOG_LEVEL_DEBUG);
-      break;
-  }
- 
-  // Capture SIGINT to write traces 
+  // Capture SIGINT to write traces
   if (prog_args.do_trace) {
     signal(SIGINT, sig_int_handler);
     //radio.start_trace();
@@ -461,7 +450,26 @@ int main(int argc, char *argv[])
   if (!radio.init()) {
     exit(1);
   }
-  phy.init(&radio, &mac, NULL, &phy_log);
+
+  std::vector<void*> phy_log;
+
+  srslte::log_filter *mylog = new srslte::log_filter("PHY");
+  char tmp[16];
+  sprintf(tmp, "PHY%d",0);
+  phy_log.push_back((void*) mylog);
+
+  switch (prog_args.verbose) {
+    case 1:
+      mac_log.set_level(srslte::LOG_LEVEL_INFO);
+      mylog->set_level(srslte::LOG_LEVEL_INFO);
+      break;
+    case 2:
+      mac_log.set_level(srslte::LOG_LEVEL_DEBUG);
+      mylog->set_level(srslte::LOG_LEVEL_DEBUG);
+      break;
+  }
+
+  phy.init(&radio, &mac, NULL, phy_log);
   if (prog_args.rf_rx_gain > 0 && prog_args.rf_tx_gain > 0) {
     radio.set_rx_gain(prog_args.rf_rx_gain);
     radio.set_tx_gain(prog_args.rf_tx_gain);

From db4045f21431d1f4dea90c514e3eb169a62ce1f2 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 19:58:04 +0200
Subject: [PATCH 202/269] Do not exit on UHD recv error code

---
 lib/src/phy/rf/rf_uhd_imp.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 3966ef56b..78f1d033e 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -653,7 +653,6 @@ int rf_uhd_recv_with_time_multi(void *h,
         return -1;
       } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE ) {
         fprintf(stderr, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
-        return -1;
       }
       
     } while (n < nsamples && trials < 100);

From 931cfa2db91fa6f1599ef120ffe0b140e88a2919 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 22:47:14 +0200
Subject: [PATCH 203/269] Add source of Late message

---
 lib/src/phy/rf/rf_uhd_imp.c | 9 +++++----
 srsue/src/ue_base.cc        | 2 +-
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 78f1d033e..8f0e5a0c0 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -76,9 +76,10 @@ static void log_overflow(rf_uhd_handler_t *h) {
   }
 }
 
-static void log_late(rf_uhd_handler_t *h) {  
+static void log_late(rf_uhd_handler_t *h, bool is_rx) {
   if (h->uhd_error_handler) {
-    srslte_rf_error_t error; 
+    srslte_rf_error_t error;
+    error.opt = is_rx?1:0;
     bzero(&error, sizeof(srslte_rf_error_t));
     error.type = SRSLTE_RF_ERROR_LATE;
     h->uhd_error_handler(error);
@@ -109,7 +110,7 @@ static void* async_thread(void *h) {
             event_code == UHD_ASYNC_METADATA_EVENT_CODE_UNDERFLOW_IN_PACKET) {
           log_underflow(handler);
         } else if (event_code == UHD_ASYNC_METADATA_EVENT_CODE_TIME_ERROR) {
-          log_late(handler);
+          log_late(handler, false);
         }
       }
     } else {
@@ -647,7 +648,7 @@ int rf_uhd_recv_with_time_multi(void *h,
       if (error_code == UHD_RX_METADATA_ERROR_CODE_OVERFLOW) {
         log_overflow(handler);
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_LATE_COMMAND) {
-        log_late(handler);
+        log_late(handler, true);
       } else if (error_code == UHD_RX_METADATA_ERROR_CODE_TIMEOUT) {
         fprintf(stderr, "Error timed out while receiving samples from UHD.\n");
         return -1;
diff --git a/srsue/src/ue_base.cc b/srsue/src/ue_base.cc
index c84393ae4..94b9b5155 100644
--- a/srsue/src/ue_base.cc
+++ b/srsue/src/ue_base.cc
@@ -88,7 +88,7 @@ void ue_base::handle_rf_msg(srslte_rf_error_t error)
   } else if(error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_LATE) {
     rf_metrics.rf_l++;
     rf_metrics.rf_error = true;
-    rf_log.warning("Late\n");
+    rf_log.warning("Late (detected in %s)\n", error.opt?"rx call":"asynchronous thread");
   } else if (error.type == srslte_rf_error_t::SRSLTE_RF_ERROR_OTHER) {
     std::string str(error.msg);
     str.erase(std::remove(str.begin(), str.end(), '\n'), str.end());

From bb9ff5fcc546a2de57c2c759ee3c48d2cd80ebdf Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 22:47:55 +0200
Subject: [PATCH 204/269] UE to recover from an sporadic loss of
 synchronization due to USRP overflow

---
 srsue/hdr/phy/phch_common.h  |  6 ++++--
 srsue/src/phy/phch_common.cc |  5 +++--
 srsue/src/phy/phch_recv.cc   |  7 +------
 srsue/src/phy/phch_worker.cc | 22 ++++++++++++++++++----
 srsue/src/phy/phy.cc         |  2 +-
 srsue/src/upper/rrc.cc       |  5 +++++
 6 files changed, 32 insertions(+), 15 deletions(-)

diff --git a/srsue/hdr/phy/phch_common.h b/srsue/hdr/phy/phch_common.h
index 04102fded..851fc3581 100644
--- a/srsue/hdr/phy/phch_common.h
+++ b/srsue/hdr/phy/phch_common.h
@@ -47,7 +47,8 @@ namespace srsue {
     
     /* Common variables used by all phy workers */
     phy_interface_rrc::phy_cfg_t *config; 
-    phy_args_t                   *args; 
+    phy_args_t                   *args;
+    rrc_interface_phy *rrc;
     mac_interface_phy *mac;
     srslte_ue_ul_t     ue_ul; 
     
@@ -68,7 +69,8 @@ namespace srsue {
     void init(phy_interface_rrc::phy_cfg_t *config, 
               phy_args_t  *args, 
               srslte::log *_log, 
-              srslte::radio *_radio, 
+              srslte::radio *_radio,
+              rrc_interface_phy *rrc,
               mac_interface_phy *_mac);
     
     /* For RNTI searches, -1 means now or forever */    
diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index c65ec7c77..1eb0be420 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -74,10 +74,11 @@ phch_common::phch_common(uint32_t max_mutex_) : tx_mutex(max_mutex_)
   sync_metrics_count = 0;
 }
   
-void phch_common::init(phy_interface_rrc::phy_cfg_t *_config, phy_args_t *_args, srslte::log *_log, srslte::radio *_radio, mac_interface_phy *_mac)
+void phch_common::init(phy_interface_rrc::phy_cfg_t *_config, phy_args_t *_args, srslte::log *_log, srslte::radio *_radio, rrc_interface_phy *_rrc, mac_interface_phy *_mac)
 {
   log_h     = _log; 
-  radio_h   = _radio; 
+  radio_h   = _radio;
+  rrc       = _rrc;
   mac       = _mac; 
   config    = _config;     
   args      = _args; 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index eac26d9ec..630de4518 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -454,7 +454,7 @@ void phch_recv::reset_sync() {
 
   wait_radio_reset();
 
-  Info("SYNC:  Resetting sync\n");
+  Warning("SYNC:  Resetting sync, cell_search_in_progress=%s\n", cell_search_in_progress?"yes":"no");
   srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
   srslte_ue_sync_reset(&ue_sync);
   resync_sfn();
@@ -718,11 +718,6 @@ void phch_recv::run_thread() {
                 worker_com->cur_radio_power = SRSLTE_MIN(SRSLTE_PC_MAX, worker_com->pathloss+worker_com->p0_preamble);
               }
               workers_pool->start_worker(worker);
-              // Notify RRC in-sync every 1 frame
-              if ((tti % 10) == 0) {
-                rrc->in_sync();
-                log_h->debug("SYNC:  Sending in-sync to RRC\n");
-              }
               break;
             case 0:
               log_h->error("SYNC:  Sync error. Sending out-of-sync to RRC\n");
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index b4b1169ed..8e53d9ca0 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -219,11 +219,14 @@ void phch_worker::work_imp()
   bzero(&ul_action, sizeof(mac_interface_phy::tb_action_ul_t));
 
   /* Do FFT and extract PDCCH LLR, or quit if no actions are required in this subframe */
-  if (extract_fft_and_pdcch_llr()) {
-    
-    
+  bool chest_ok = extract_fft_and_pdcch_llr();
+
+  bool snr_th_ok = 10*log10(srslte_chest_dl_get_snr(&ue_dl.chest))>1.0;
+
+  if (chest_ok && snr_th_ok) {
+
     /***** Downlink Processing *******/
-    
+
     /* PDCCH DL + PDSCH */
     dl_grant_available = decode_pdcch_dl(&dl_mac_grant); 
     if(dl_grant_available) {
@@ -360,6 +363,17 @@ void phch_worker::work_imp()
   }
 
   update_measurements();
+
+  if (chest_ok) {
+    if (snr_th_ok) {
+      phy->rrc->in_sync();
+      log_h->debug("SYNC:  Sending in-sync to RRC\n");
+    } else {
+      phy->rrc->out_of_sync();
+      log_h->debug("SNR=%.1f dB under threshold. Sending out-of-sync to RRC\n",
+                   10*log10(srslte_chest_dl_get_snr(&ue_dl.chest)));
+    }
+  }
   
   /* Tell the plotting thread to draw the plots */
 #ifdef ENABLE_GUI
diff --git a/srsue/src/phy/phy.cc b/srsue/src/phy/phy.cc
index 44b548c4e..e74107661 100644
--- a/srsue/src/phy/phy.cc
+++ b/srsue/src/phy/phy.cc
@@ -129,7 +129,7 @@ bool phy::init(srslte::radio_multi* radio_handler, mac_interface_phy *mac, rrc_i
 void phy::run_thread() {
 
   prach_buffer.init(&config.common.prach_cnfg, SRSLTE_MAX_PRB, args, log_h);
-  workers_common.init(&config, args, (srslte::log*) log_vec[0], radio_handler, mac);
+  workers_common.init(&config, args, (srslte::log*) log_vec[0], radio_handler, rrc, mac);
 
   // Add workers to workers pool and start threads
   for (uint32_t i=0;i<nof_workers;i++) {
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 43ec73187..3bb3e0017 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -478,6 +478,9 @@ void rrc::out_of_sync() {
   if (!mac_timers->timer_get(t311)->is_running() && !mac_timers->timer_get(t310)->is_running()) {
     n310_cnt++;
     if (n310_cnt == N310) {
+      // attempt resync
+      phy->sync_reset();
+
       mac_timers->timer_get(t310)->reset();
       mac_timers->timer_get(t310)->run();
       n310_cnt = 0;
@@ -656,8 +659,10 @@ void rrc::send_con_restablish_request() {
   mac_timers->timer_get(t311)->reset();
   mac_timers->timer_get(t311)->run();
 
+  phy->reset();
   set_phy_default();
   mac->reset();
+  set_mac_default();
 
   // FIXME: Cell selection should be different??
 

From 18d2c44b718478d8616e9890ed72cb7a494c165d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 20 Sep 2017 23:06:08 +0200
Subject: [PATCH 205/269] Restored L/U/O metrics

---
 srsue/hdr/ue.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 473e01e0e..57dce1231 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -116,8 +116,7 @@ private:
 
   all_args_t       *args;
   bool              started;
-  rf_metrics_t     rf_metrics;
-  
+
   bool check_srslte_version();
 };
 

From 55446fc2a214a68ce4ed6e35ff0d6c474b4fd72b Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Thu, 21 Sep 2017 10:03:02 +0200
Subject: [PATCH 206/269] Over the wire format can be selected through RF args

---
 lib/src/phy/rf/rf_uhd_imp.c | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index ad69c0642..d392d828c 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -386,7 +386,17 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
       uhd_usrp_set_clock_source(handler->usrp, "gpsdo", 0);       
     }
 
-      
+    // Set over the wire format
+    char *otw_format = "sc16";
+    if (strstr(args, "otw_format=sc12")) {
+      otw_format = "sc12";
+    } else if (strstr(args, "otw_format=sc16")) {
+      /* Do nothing */
+    } else if (strstr(args, "otw_format=")) {
+      fprintf(stderr, "Wrong over the wire format. Valid formats: sc12, sc16\n", error);
+      return -1;
+    }
+
     handler->has_rssi = get_has_rssi(handler);  
     if (handler->has_rssi) {        
       uhd_sensor_value_make_from_realnum(&handler->rssi_value, "rssi", 0, "dBm", "%f");      
@@ -395,7 +405,7 @@ int rf_uhd_open_multi(char *args, void **h, uint32_t nof_channels)
     size_t channel[4] = {0, 1, 2, 3};
     uhd_stream_args_t stream_args = {
           .cpu_format = "fc32",
-          .otw_format = (nof_channels > 1) ? "sc12" : "sc16",
+          .otw_format = otw_format,
           .args = "",
           .channel_list = channel,
           .n_channels = nof_channels,
@@ -632,7 +642,8 @@ int rf_uhd_recv_with_time_multi(void *h,
         return -1;
       } else if (error_code != UHD_RX_METADATA_ERROR_CODE_NONE ) {
         fprintf(stderr, "Error code 0x%x was returned during streaming. Aborting.\n", error_code);
-        return -1;
+        // FIXME: Keep going if an error not mentioned above occurs
+        // return -1;
       }
       
     } while (n < nsamples && trials < 100);

From 4753366096db5c7cb0b04e496525df99a4198ad9 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 21 Sep 2017 21:21:35 +0200
Subject: [PATCH 207/269] Added option to ue.conf/enb.conf to forward logs to
 stdout

---
 srsenb/enb.conf.example |  5 +++--
 srsenb/hdr/enb.h        | 11 ++++-------
 srsenb/src/enb.cc       | 33 +++++++++++++++++++--------------
 srsue/hdr/ue.h          | 11 ++++-------
 srsue/src/main.cc       |  1 +
 srsue/src/ue.cc         | 33 ++++++++++++++++++---------------
 srsue/ue.conf.example   |  3 ++-
 7 files changed, 51 insertions(+), 46 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 6847f7392..5e386ddc1 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -98,8 +98,9 @@ filename = /tmp/enb.pcap
 # Logging layers: phy, mac, rlc, pdcp, rrc, nas, gtpu, usim, all
 # Logging levels: debug, info, warning, error, none
 #
-# filename: File path to use for log output
-#####################################################################
+# filename: File path to use for log output. Can be set to stdout
+#           to print logs to standard output
+git c#####################################################################
 [log]
 all_level = info
 all_hex_limit = 32
diff --git a/srsenb/hdr/enb.h b/srsenb/hdr/enb.h
index 8b847302e..7b1681cd6 100644
--- a/srsenb/hdr/enb.h
+++ b/srsenb/hdr/enb.h
@@ -138,8 +138,6 @@ typedef struct {
   Main UE class
 *******************************************************************************/
 
-//#define LOG_STDOUT
-
 class enb
     :public enb_metrics_interface {
 public:
@@ -180,11 +178,10 @@ private:
   srsenb::gtpu gtpu;
   srsenb::s1ap s1ap;
 
-#ifdef LOG_STDOUT
-    srslte::logger_stdout logger;
-#else
-    srslte::logger_file logger;
-#endif
+  srslte::logger_stdout logger_stdout;
+  srslte::logger_file   logger_file;
+  srslte::logger        *logger;
+
   srslte::log_filter  rf_log;
   std::vector<void*>  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index e2c1c9a29..d00fd4c0b 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -26,6 +26,7 @@
 
 #include <boost/algorithm/string.hpp>
 #include <boost/thread/mutex.hpp>
+#include <enb.h>
 #include "enb.h"
 
 namespace srsenb {
@@ -68,30 +69,34 @@ bool enb::init(all_args_t *args_)
 {
   args     = args_;
 
-#ifndef LOG_STDOUT
-    logger.init(args->log.filename);
-#endif
-  rf_log.init("RF  ", &logger);
+  if (!args->log.filename.compare("stdout")) {
+    logger = &logger_stdout;
+    printf("log name is output\n");
+  } else {
+    printf("lgo name is %s\n", args->log.filename.c_str());
+    logger_file.init(args->log.filename);
+    logger_file.log("\n\n");
+    logger = &logger_file;
+  }
+
+  rf_log.init("RF  ", logger);
   
   // Create array of pointers to phy_logs 
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     srslte::log_filter *mylog = new srslte::log_filter;
     char tmp[16];
     sprintf(tmp, "PHY%d",i);
-    mylog->init(tmp, &logger, true);
+    mylog->init(tmp, logger, true);
     phy_log.push_back((void*) mylog); 
   }
-  mac_log.init("MAC ", &logger, true);
-  rlc_log.init("RLC ", &logger);
-  pdcp_log.init("PDCP", &logger);
-  rrc_log.init("RRC ", &logger);
-  gtpu_log.init("GTPU", &logger);
-  s1ap_log.init("S1AP", &logger);
+  mac_log.init("MAC ", logger, true);
+  rlc_log.init("RLC ", logger);
+  pdcp_log.init("PDCP", logger);
+  rrc_log.init("RRC ", logger);
+  gtpu_log.init("GTPU", logger);
+  s1ap_log.init("S1AP", logger);
 
   // Init logs
-#ifndef LOG_STDOUT
-    logger.log("\n\n");
-#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     ((srslte::log_filter*) phy_log[i])->set_level(level(args->log.phy_level));
diff --git a/srsue/hdr/ue.h b/srsue/hdr/ue.h
index 57dce1231..4e48f6b6d 100644
--- a/srsue/hdr/ue.h
+++ b/srsue/hdr/ue.h
@@ -57,8 +57,6 @@
 
 namespace srsue {
 
-//#define LOG_STDOUT
-
 /*******************************************************************************
   Main UE class
 *******************************************************************************/
@@ -97,11 +95,10 @@ private:
   srsue::gw          gw;
   srsue::usim        usim;
 
-#ifdef LOG_STDOUT
-    srslte::logger_stdout logger;
-#else
-    srslte::logger_file logger;
-#endif
+  srslte::logger_stdout logger_stdout;
+  srslte::logger_file   logger_file;
+  srslte::logger        *logger;
+
   // rf_log is on ue_base
   std::vector<void*>  phy_log;
   srslte::log_filter  mac_log;
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index f095a60c4..02394f3e4 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -103,6 +103,7 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
     ("log.usim_level", bpo::value<string>(&args->log.usim_level), "USIM log level")
     ("log.usim_hex_limit", bpo::value<int>(&args->log.usim_hex_limit), "USIM log hex dump limit")
 
+
     ("log.all_level", bpo::value<string>(&args->log.all_level)->default_value("info"), "ALL log level")
     ("log.all_hex_limit", bpo::value<int>(&args->log.all_hex_limit)->default_value(32), "ALL log hex dump limit")
 
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index aab6bbc08..646b87612 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -32,6 +32,7 @@
 #include <string>
 #include <algorithm>
 #include <iterator>
+#include <ue_base.h>
 
 using namespace srslte;
 
@@ -52,31 +53,33 @@ bool ue::init(all_args_t *args_)
 {
   args     = args_;
 
-#ifndef LOG_STDOUT
-    logger.init(args->log.filename);
-#endif
-  rf_log.init("RF  ", &logger);
+  if (!args->log.filename.compare("stdout")) {
+    logger = &logger_stdout;
+  } else {
+    logger_file.init(args->log.filename);
+    logger_file.log("\n\n");
+    logger = &logger_file;
+  }
+
+  rf_log.init("RF  ", logger);
   // Create array of pointers to phy_logs
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     srslte::log_filter *mylog = new srslte::log_filter;
     char tmp[16];
     sprintf(tmp, "PHY%d",i);
-    mylog->init(tmp, &logger, true);
+    mylog->init(tmp, logger, true);
     phy_log.push_back((void*) mylog);
   }
 
-  mac_log.init("MAC ", &logger, true);
-  rlc_log.init("RLC ", &logger);
-  pdcp_log.init("PDCP", &logger);
-  rrc_log.init("RRC ", &logger);
-  nas_log.init("NAS ", &logger);
-  gw_log.init("GW  ", &logger);
-  usim_log.init("USIM", &logger);
+  mac_log.init("MAC ", logger, true);
+  rlc_log.init("RLC ", logger);
+  pdcp_log.init("PDCP", logger);
+  rrc_log.init("RRC ", logger);
+  nas_log.init("NAS ", logger);
+  gw_log.init("GW  ", logger);
+  usim_log.init("USIM", logger);
 
   // Init logs
-#ifndef LOG_STDOUT
-  logger.log("\n\n");
-#endif
   rf_log.set_level(srslte::LOG_LEVEL_INFO);
   for (int i=0;i<args->expert.phy.nof_phy_threads;i++) {
     ((srslte::log_filter*) phy_log[i])->set_level(level(args->log.phy_level));
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 76868008f..30b152280 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -64,7 +64,8 @@ filename = /tmp/ue.pcap
 # Logging layers: phy, mac, rlc, pdcp, rrc, nas, gw, usim, all
 # Logging levels: debug, info, warning, error, none
 #
-# filename: File path to use for log output
+# filename: File path to use for log output. Can be set to stdout
+#           to print logs to standard output
 #####################################################################
 [log]
 all_level = info

From f064b8318367d54e9c7ed4fb12528dd2b1a492ad Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 21 Sep 2017 21:36:15 +0200
Subject: [PATCH 208/269] Added buffer overflow check in RLC UM reassemble

---
 lib/src/upper/rlc_um.cc | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index c5399407f..d44df6348 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -513,11 +513,20 @@ void rlc_um::reassemble_rx_sdus()
     }
     
     // Handle last segment
-    memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_ur].buf->msg, rx_window[vr_ur].buf->N_bytes);
-    rx_sdu->N_bytes += rx_window[vr_ur].buf->N_bytes;
-    log->debug("Writting last segment in SDU buffer. Updating vr_ur=%d, Buffer size=%d, segment size=%d\n", 
-               vr_ur, rx_sdu->N_bytes, rx_window[vr_ur].buf->N_bytes);
-    vr_ur_in_rx_sdu = vr_ur; 
+    // Handle last segment
+    if (rx_sdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES                 ||
+        rx_window[vr_ur].buf->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES   ||
+        rx_window[vr_ur].buf->N_bytes + rx_sdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES) {
+
+      memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_ur].buf->msg, rx_window[vr_ur].buf->N_bytes);
+      rx_sdu->N_bytes += rx_window[vr_ur].buf->N_bytes;
+      log->debug("Writting last segment in SDU buffer. Updating vr_ur=%d, Buffer size=%d, segment size=%d\n",
+                 vr_ur, rx_sdu->N_bytes, rx_window[vr_ur].buf->N_bytes);
+    } else {
+      log->error("Out of bounds while reassembling SDU buffer in UM: sdu_len=%d, window_buffer_len=%d, vr_ur=%d\n",
+                 rx_sdu->N_bytes, rx_window[vr_ur].buf->N_bytes, vr_ur);
+    }
+    vr_ur_in_rx_sdu = vr_ur;
     if(rlc_um_end_aligned(rx_window[vr_ur].header.fi))
     {
       if(pdu_lost && !rlc_um_start_aligned(rx_window[vr_ur].header.fi)) {

From 6de06457fa3a7ccdc9faa143e7153b1eb6bc9a8b Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 21 Sep 2017 21:42:27 +0200
Subject: [PATCH 209/269] Removed MAC timers thread and call them from main MAC
 thread

---
 srsue/hdr/mac/mac.h  | 15 ---------------
 srsue/src/mac/mac.cc | 34 ++--------------------------------
 2 files changed, 2 insertions(+), 47 deletions(-)

diff --git a/srsue/hdr/mac/mac.h b/srsue/hdr/mac/mac.h
index ed5ad151a..a306af187 100644
--- a/srsue/hdr/mac/mac.h
+++ b/srsue/hdr/mac/mac.h
@@ -150,27 +150,12 @@ private:
   uint8_t                pch_payload_buffer[pch_payload_buffer_sz];
 
 
-  /* class that runs a thread to trigger timer callbacks in low priority */
-  class timer_thread : public thread {
-  public:
-    timer_thread(srslte::timers *timers_) : ttisync(10240),running(false),timers(timers_) {start();}
-    void tti_clock();
-    void stop();
-  private:
-    bool running;
-    void run_thread();
-    srslte::tti_sync_cv ttisync;
-    srslte::timers     *timers;
-  };
-
-
   /* Functions for MAC Timers */
   uint32_t        timer_alignment;
   uint32_t        contention_resolution_timer;
   void            setup_timers();
   void            timer_alignment_expire();
   srslte::timers  timers;
-  timer_thread    timers_thread;
 
 
   // pointer to MAC PCAP object
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 665fd40e9..8d5bf91b9 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -43,7 +43,6 @@ namespace srsue {
 
 mac::mac() : ttisync(10240), 
              timers(64),
-             timers_thread(&timers),
              mux_unit(MAC_NOF_HARQ_PROC),
              demux_unit(SRSLTE_MAX_TB*MAC_NOF_HARQ_PROC),
              pdu_process_thread(&demux_unit)
@@ -95,7 +94,6 @@ void mac::stop()
   started = false;
   ttisync.increase();
   pdu_process_thread.stop();
-  timers_thread.stop();
   wait_thread_finish();
 }
 
@@ -161,7 +159,7 @@ void mac::run_thread() {
 
       log_h->step(tti);
 
-      timers_thread.tti_clock();
+      timers.step_all();
 
       // Step all procedures
       bsr_procedure.step(tti);
@@ -312,6 +310,7 @@ void mac::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
 
 void mac::new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t* action)
 {
+  log_h->info("new_grant_ul_ack\n");
   int tbs = ul_harq.get_current_tbs(tti);
   ul_harq.new_grant_ul_ack(grant, ack, action);
   if (!ack) {
@@ -453,35 +452,6 @@ uint32_t mac::timer_get_unique_id()
 }
 
 
-/********************************************************
- *
- * Class that runs timers in lower priority
- *
- *******************************************************/
-void mac::timer_thread::tti_clock()
-{
-  ttisync.increase();
-}
-
-void mac::timer_thread::run_thread()
-{
-  running=true;
-  ttisync.set_producer_cntr(0);
-  ttisync.resync();
-  while(running) {
-    ttisync.wait();
-    timers->step_all();
-  }
-}
-
-void mac::timer_thread::stop()
-{
-  running=false;
-  ttisync.increase();
-  wait_thread_finish();
-}
-
-
 /********************************************************
  *
  * Class that runs a thread to process DL MAC PDUs from

From b12d69b439fced0426ffc292fc36b6da8dbd1568 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 21 Sep 2017 21:43:24 +0200
Subject: [PATCH 210/269] Set default TX mode to continuous and fixed uplink
 synchronization (radio set offset concurrent access)

---
 lib/include/srslte/radio/radio.h | 14 +++--------
 lib/src/phy/rf/rf_uhd_imp.c      | 20 +++++++++------
 lib/src/phy/rf/uhd_c_api.cpp     |  6 +++++
 lib/src/phy/rf/uhd_c_api.h       |  1 +
 lib/src/phy/ue/ue_sync.c         |  2 +-
 lib/src/radio/radio.cc           | 30 +++++++++--------------
 srsue/hdr/mac/ul_harq.h          |  1 +
 srsue/hdr/phy/phch_common.h      |  5 ++--
 srsue/hdr/phy/phch_recv.h        | 10 ++++++--
 srsue/hdr/phy/phch_worker.h      |  6 +++--
 srsue/src/phy/phch_common.cc     |  2 --
 srsue/src/phy/phch_recv.cc       | 42 +++++++++++++++++++-------------
 srsue/src/phy/phch_worker.cc     | 19 +++++++++------
 13 files changed, 89 insertions(+), 69 deletions(-)

diff --git a/lib/include/srslte/radio/radio.h b/lib/include/srslte/radio/radio.h
index 866bc4697..329dd825a 100644
--- a/lib/include/srslte/radio/radio.h
+++ b/lib/include/srslte/radio/radio.h
@@ -57,8 +57,7 @@ namespace srslte {
         bzero(&end_of_burst_time, sizeof(srslte_timestamp_t));
         bzero(zeros, burst_preamble_max_samples*sizeof(cf_t));
         
-        sf_len                  = 0;
-        burst_preamble_sec      = 0; 
+        burst_preamble_sec      = 0;
         is_start_of_burst       = false; 
         burst_preamble_samples  = 0; 
         burst_preamble_time_rounded = 0; 
@@ -72,9 +71,7 @@ namespace srslte {
         rx_freq                 = 0; 
         trace_enabled           = false; 
         tti                     = 0; 
-        agc_enabled             = false; 
-        offset                  = 0; 
-        
+        agc_enabled             = false;
       };
       
       bool init(char *args = NULL, char *devname = NULL);
@@ -124,9 +121,8 @@ namespace srslte {
       void stop_rx();
       
       void set_tti(uint32_t tti);
-      void tx_offset(int offset);
-      void set_tti_len(uint32_t sf_len);
-      uint32_t get_tti_len();
+
+      bool is_first_of_burst();
 
       void register_error_handler(srslte_rf_error_handler_t h);
       
@@ -169,8 +165,6 @@ namespace srslte {
       bool trace_enabled;
       uint32_t tti;
       bool agc_enabled;
-      int offset;
-      uint32_t sf_len;
 
       char saved_args[128];
       char saved_devname[128];
diff --git a/lib/src/phy/rf/rf_uhd_imp.c b/lib/src/phy/rf/rf_uhd_imp.c
index 0c7bd53ff..95287a628 100644
--- a/lib/src/phy/rf/rf_uhd_imp.c
+++ b/lib/src/phy/rf/rf_uhd_imp.c
@@ -710,11 +710,11 @@ int rf_uhd_send_timed_multi(void *h,
   }
 
   size_t txd_samples;
-  if (has_time_spec) {
-    uhd_tx_metadata_set_time_spec(&handler->tx_md, secs, frac_secs);
-  }
-  int trials = 0; 
+  int trials = 0;
   if (blocking) {
+    if (has_time_spec) {
+      uhd_tx_metadata_set_time_spec(&handler->tx_md, secs, frac_secs);
+    }
     int n = 0;
     cf_t *data_c[4];
     for (int i = 0; i < 4; i++) {
@@ -722,8 +722,8 @@ int rf_uhd_send_timed_multi(void *h,
     }
     do {
       size_t tx_samples = handler->tx_nof_samples;
-      
-      // First packet is start of burst if so defined, others are never 
+
+      // First packet is start of burst if so defined, others are never
       if (n == 0) {
         uhd_tx_metadata_set_start(&handler->tx_md, is_start_of_burst);
       } else {
@@ -760,9 +760,15 @@ int rf_uhd_send_timed_multi(void *h,
     for (int i = 0; i < 4; i++) {
      buffs_ptr[i] = data[i];
     }
+    uhd_tx_metadata_set_has_time_spec(&handler->tx_md, is_start_of_burst);
     uhd_tx_metadata_set_start(&handler->tx_md, is_start_of_burst);
     uhd_tx_metadata_set_end(&handler->tx_md, is_end_of_burst);
-    return uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, nsamples, &handler->tx_md, 0.0, &txd_samples);
+    uhd_error error = uhd_tx_streamer_send(handler->tx_stream, buffs_ptr, nsamples, &handler->tx_md, 3.0, &txd_samples);
+    if (error) {
+      fprintf(stderr, "Error sending to UHD: %d\n", error);
+      return -1;
+    }
+    return txd_samples;
   }
 }
 
diff --git a/lib/src/phy/rf/uhd_c_api.cpp b/lib/src/phy/rf/uhd_c_api.cpp
index da348c17b..d98c3c92a 100644
--- a/lib/src/phy/rf/uhd_c_api.cpp
+++ b/lib/src/phy/rf/uhd_c_api.cpp
@@ -38,6 +38,12 @@ void uhd_tx_metadata_set_start(uhd_tx_metadata_handle *md, bool is_start_of_burs
   (*md)->tx_metadata_cpp.start_of_burst = is_start_of_burst;
 }
 
+void uhd_tx_metadata_set_has_time_spec(uhd_tx_metadata_handle *md, bool has_time_spec)
+{
+  (*md)->tx_metadata_cpp.has_time_spec = has_time_spec;
+}
+
+
 void uhd_tx_metadata_set_end(uhd_tx_metadata_handle *md, bool is_end_of_burst)
 {
   (*md)->tx_metadata_cpp.end_of_burst = is_end_of_burst;
diff --git a/lib/src/phy/rf/uhd_c_api.h b/lib/src/phy/rf/uhd_c_api.h
index 15bb5f33c..8f6cb2743 100644
--- a/lib/src/phy/rf/uhd_c_api.h
+++ b/lib/src/phy/rf/uhd_c_api.h
@@ -32,5 +32,6 @@
 SRSLTE_API void rf_uhd_register_msg_handler_c(void (*new_handler)(const char*));
 SRSLTE_API void uhd_tx_metadata_set_time_spec(uhd_tx_metadata_handle *md, time_t secs, double frac_secs);
 SRSLTE_API void uhd_tx_metadata_set_start(uhd_tx_metadata_handle *md, bool is_start_of_burst);
+SRSLTE_API void uhd_tx_metadata_set_has_time_spec(uhd_tx_metadata_handle *md, bool has_time_spec);
 SRSLTE_API void uhd_tx_metadata_set_end(uhd_tx_metadata_handle *md, bool is_end_of_burst);
 SRSLTE_API void uhd_tx_metadata_add_time_spec(uhd_tx_metadata_handle *md, double frac_secs);
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index 2126950a6..3f5b131eb 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -513,7 +513,7 @@ static int track_peak_ok(srslte_ue_sync_t *q, uint32_t track_idx) {
     discard the offseted samples to align next frame */
   if (q->next_rf_sample_offset > 0 && q->next_rf_sample_offset < MAX_TIME_OFFSET) {
     DEBUG("Positive time offset %d samples.\n", q->next_rf_sample_offset);
-    if (q->recv_callback(q->stream, dummy_offset_buffer, (uint32_t) q->next_rf_sample_offset, &q->last_timestamp) < 0) {
+    if (q->recv_callback(q->stream, dummy_offset_buffer, (uint32_t) q->next_rf_sample_offset, NULL) < 0) {
       fprintf(stderr, "Error receiving from USRP\n");
       return SRSLTE_ERROR; 
     }
diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index 6cd2d9dd3..c0e828f71 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -120,11 +120,6 @@ void radio::set_tx_adv_neg(bool tx_adv_is_neg) {
   tx_adv_negative = tx_adv_is_neg; 
 }
 
-void radio::tx_offset(int offset_)
-{
-  offset = offset_; 
-}
-
 bool radio::start_agc(bool tx_gain_same_rx)
 {
   if (srslte_rf_start_gain_thread(&rf_device, tx_gain_same_rx)) {
@@ -185,6 +180,12 @@ bool radio::has_rssi()
   return srslte_rf_has_rssi(&rf_device);
 }
 
+bool radio::is_first_of_burst() {
+  return is_start_of_burst;
+}
+
+#define BLOCKING_TX true
+
 bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
 {
   void *iq_samples[4] = {(void *) zeros, (void *) zeros, (void *) zeros, (void *) zeros};
@@ -203,7 +204,7 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
       save_trace(1, &tx_time_pad);
       srslte_rf_send_timed_multi(&rf_device, iq_samples, burst_preamble_samples, tx_time_pad.full_secs, tx_time_pad.frac_secs, true, true, false);
       is_start_of_burst = false; 
-    }        
+    }
   }
   
   // Save possible end of burst time 
@@ -212,9 +213,10 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   
   save_trace(0, &tx_time);
   iq_samples[0] = buffer;
-  int ret = srslte_rf_send_timed_multi(&rf_device, (void**) iq_samples, nof_samples+offset, tx_time.full_secs, tx_time.frac_secs, true, is_start_of_burst, false);
-  offset = 0; 
-  is_start_of_burst = false; 
+  int ret = srslte_rf_send_timed_multi(&rf_device, (void**) iq_samples, nof_samples,
+                                       tx_time.full_secs, tx_time.frac_secs,
+                                       BLOCKING_TX, is_start_of_burst, false);
+  is_start_of_burst = false;
   if (ret > 0) {
     return true; 
   } else {
@@ -222,16 +224,6 @@ bool radio::tx(void* buffer, uint32_t nof_samples, srslte_timestamp_t tx_time)
   }
 }
 
-uint32_t radio::get_tti_len()
-{
-  return sf_len; 
-}
-
-void radio::set_tti_len(uint32_t sf_len_)
-{
-  sf_len = sf_len_; 
-}
-
 void radio::tx_end()
 {
   if (!is_start_of_burst) {
diff --git a/srsue/hdr/mac/ul_harq.h b/srsue/hdr/mac/ul_harq.h
index 52aef7bd7..4749971b6 100644
--- a/srsue/hdr/mac/ul_harq.h
+++ b/srsue/hdr/mac/ul_harq.h
@@ -368,6 +368,7 @@ private:
         current_tx_nb = 0;
         current_irv = 0;
         is_msg3 = is_msg3_;
+
         Info("UL %d:  New TX%s, RV=%d, TBS=%d, RNTI=%d\n",
              pid, is_msg3?" for Msg3":"", get_rv(), cur_grant.n_bytes[0],
              is_msg3?harq_entity->rntis->temp_rnti:cur_grant.rnti);
diff --git a/srsue/hdr/phy/phch_common.h b/srsue/hdr/phy/phch_common.h
index 851fc3581..aa64fe9ea 100644
--- a/srsue/hdr/phy/phch_common.h
+++ b/srsue/hdr/phy/phch_common.h
@@ -27,6 +27,9 @@
 #ifndef UEPHYWORKERCOMMON_H
 #define UEPHYWORKERCOMMON_H
 
+#define TX_MODE_CONTINUOUS 1
+
+
 #include <pthread.h>
 #include <string.h>
 #include <vector>
@@ -36,8 +39,6 @@
 #include "srslte/common/log.h"
 #include "phy/phy_metrics.h"
 
-//#define CONTINUOUS_TX
-
 
 namespace srsue {
 
diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index ac033f607..8c768f46b 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -67,7 +67,13 @@ public:
   void    set_time_adv_sec(float time_adv_sec);
   void    get_current_cell(srslte_cell_t *cell);
   
-  const static int MUTEX_X_WORKER = 4; 
+  const static int MUTEX_X_WORKER = 4;
+
+  // public variables needed by callback function
+  uint32_t              current_sflen;
+  srslte::radio_multi  *radio_h;
+  int                   next_offset;
+
 
 private:
 
@@ -97,7 +103,6 @@ private:
 
   bool   running; 
   
-  srslte::radio_multi  *radio_h;
   mac_interface_phy    *mac;
   rrc_interface_phy    *rrc;
   srslte::log          *log_h;
@@ -133,6 +138,7 @@ private:
   enum {
     SRATE_NONE=0, SRATE_FIND, SRATE_CAMP
   } srate_mode;
+  float         current_srate;
 
   srslte_cell_t cell;
   bool          cell_is_set;
diff --git a/srsue/hdr/phy/phch_worker.h b/srsue/hdr/phy/phch_worker.h
index 307cea0f0..0811723e0 100644
--- a/srsue/hdr/phy/phch_worker.h
+++ b/srsue/hdr/phy/phch_worker.h
@@ -52,7 +52,7 @@ public:
   /* Functions used by main PHY thread */
   cf_t* get_buffer(uint32_t antenna_idx);
   void  set_tti(uint32_t tti, uint32_t tx_tti); 
-  void  set_tx_time(srslte_timestamp_t tx_time);
+  void  set_tx_time(srslte_timestamp_t tx_time, uint32_t next_offset);
   void  set_cfo(float cfo);
   void  set_sample_offset(float sample_offset); 
   
@@ -123,7 +123,9 @@ private:
   bool           pregen_enabled;
   uint32_t       last_dl_pdcch_ncce;
   bool           rnti_is_set;
-  
+
+  uint32_t next_offset;
+
   /* Objects for DL */
   srslte_ue_dl_t ue_dl; 
   uint32_t       cfi; 
diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index 1eb0be420..6baf0b70e 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -34,8 +34,6 @@
 #define Info(fmt, ...)    if (SRSLTE_DEBUG_ENABLED) log_h->info_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 #define Debug(fmt, ...)   if (SRSLTE_DEBUG_ENABLED) log_h->debug_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
 
-#define TX_MODE_CONTINUOUS 0 
-
 namespace srsue {
 
 cf_t zeros[50000];
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index 630de4518..ba09f2d53 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -39,15 +39,16 @@
 
 namespace srsue {
 
-int radio_recv_wrapper_cs(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
-  srslte::radio_multi *radio_h = (srslte::radio_multi *) h;
+int radio_recv_wrapper_cs(void *obj, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *rx_time) {
+  phch_recv *h = (phch_recv*) obj;
+  srslte::radio_multi *radio_h = h->radio_h;
 
   if (radio_h->rx_now(data, nsamples, rx_time)) {
-    int offset = nsamples - radio_h->get_tti_len();
+    int offset = nsamples - h->current_sflen;
     if (abs(offset) < 10 && offset != 0) {
-      radio_h->tx_offset(offset);
+      h->next_offset = offset;
     } else if (nsamples < 10) {
-      radio_h->tx_offset(nsamples);
+      h->next_offset = nsamples;
     }
     return nsamples;
   } else {
@@ -87,7 +88,7 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
 
 
   if (srslte_ue_cellsearch_init_multi(&cs, 5, radio_recv_wrapper_cs, nof_rx_antennas,
-                                      radio_h)) {
+                                      this)) {
     Error("SYNC:  Initiating UE cell search\n");
     return;
   }
@@ -111,12 +112,12 @@ void phch_recv::  init(srslte::radio_multi *_radio_handler, mac_interface_phy *_
     return;
   }
 
-  if (srslte_ue_sync_init_multi(&ue_sync, SRSLTE_MAX_PRB, false, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+  if (srslte_ue_sync_init_multi(&ue_sync, SRSLTE_MAX_PRB, false, radio_recv_wrapper_cs, nof_rx_antennas, this)) {
     Error("SYNC:  Initiating ue_sync\n");
     return;
   }
 
-  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, radio_recv_wrapper_cs, nof_rx_antennas, radio_h)) {
+  if (srslte_ue_mib_sync_init_multi(&ue_mib_sync, radio_recv_wrapper_cs, nof_rx_antennas, this)) {
     Error("SYNC:  Initiating UE MIB synchronization\n");
     return;
   }
@@ -154,6 +155,7 @@ void phch_recv::reset() {
   running = true;
   phy_state = IDLE;
   time_adv_sec = 0;
+  next_offset  = 0;
   cell_is_set = false;
   sync_sfn_cnt = 0;
   srate_mode = SRATE_NONE;
@@ -185,7 +187,12 @@ void phch_recv::set_agc_enable(bool enable) {
 }
 
 void phch_recv::set_time_adv_sec(float _time_adv_sec) {
-  time_adv_sec = _time_adv_sec;
+  if (TX_MODE_CONTINUOUS && !radio_h->is_first_of_burst()) {
+    int nsamples = ceil(current_srate*_time_adv_sec);
+    next_offset = -nsamples;
+  } else {
+    time_adv_sec = _time_adv_sec;
+  }
 }
 
 void phch_recv::set_ue_sync_opts(srslte_ue_sync_t *q) {
@@ -241,7 +248,6 @@ bool phch_recv::set_cell() {
       return false;
     }
   }
-  radio_h->set_tti_len(SRSLTE_SF_LEN_PRB(cell.nof_prb));
   if (do_agc) {
     srslte_ue_sync_start_agc(&ue_sync, callback_set_rx_gain, last_gain);
   }
@@ -578,18 +584,19 @@ bool phch_recv::set_frequency()
 
 void phch_recv::set_sampling_rate()
 {
-  float srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
-  if (srate != -1) {
-    Info("SYNC:  Setting sampling rate %.2f MHz\n", srate/1000000);
+  current_srate = (float) srslte_sampling_freq_hz(cell.nof_prb);
+  current_sflen = SRSLTE_SF_LEN_PRB(cell.nof_prb);
+  if (current_srate != -1) {
+    Info("SYNC:  Setting sampling rate %.2f MHz\n", current_srate/1000000);
 
-    if (30720 % ((int) srate / 1000) == 0) {
+    if (30720 % ((int) current_srate / 1000) == 0) {
       radio_h->set_master_clock_rate(30.72e6);
     } else {
       radio_h->set_master_clock_rate(23.04e6);
     }
     srate_mode = SRATE_CAMP;
-    radio_h->set_rx_srate(srate);
-    radio_h->set_tx_srate(srate);
+    radio_h->set_rx_srate(current_srate);
+    radio_h->set_tx_srate(current_srate);
   } else {
     Error("Error setting sampling rate for cell with %d PRBs\n", cell.nof_prb);
   }
@@ -702,7 +709,8 @@ void phch_recv::run_thread() {
               srslte_ue_sync_get_last_timestamp(&ue_sync, &rx_time);
               srslte_timestamp_copy(&tx_time, &rx_time);
               srslte_timestamp_add(&tx_time, 0, 4e-3 - time_adv_sec);
-              worker->set_tx_time(tx_time);
+              worker->set_tx_time(tx_time, next_offset);
+              next_offset = 0;
 
               Debug("SYNC:  Setting TTI=%d, tx_mutex=%d to worker %d\n", tti, tx_mutex_cnt, worker->get_id());
               worker->set_tti(tti, tx_mutex_cnt);
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 8e53d9ca0..3672c155d 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -294,7 +294,7 @@ void phch_worker::work_imp()
   }
   
   // Decode PHICH 
-  bool ul_ack; 
+  bool ul_ack = false;
   bool ul_ack_available = decode_phich(&ul_ack); 
 
   /***** Uplink Processing + Transmission *******/
@@ -347,9 +347,13 @@ void phch_worker::work_imp()
   } 
 
   tr_log_end();
-  
-  phy->worker_end(tx_tti, signal_ready, signal_buffer[0], SRSLTE_SF_LEN_PRB(cell.nof_prb), tx_time);
-  
+
+  if (next_offset > 0) {
+    phy->worker_end(tx_tti, signal_ready, signal_buffer[0], SRSLTE_SF_LEN_PRB(cell.nof_prb)+next_offset, tx_time);
+  } else {
+    phy->worker_end(tx_tti, signal_ready, &signal_buffer[0][-next_offset], SRSLTE_SF_LEN_PRB(cell.nof_prb)+next_offset, tx_time);
+  }
+
   if (!dl_action.generate_ack_callback) {
     if (dl_mac_grant.rnti_type == SRSLTE_RNTI_PCH && dl_action.decode_enabled[0]) {
       phy->mac->pch_decoded_ok(dl_mac_grant.n_bytes[0]);
@@ -867,8 +871,9 @@ bool phch_worker::srs_is_ready_to_send() {
   return false; 
 }
 
-void phch_worker::set_tx_time(srslte_timestamp_t _tx_time)
+void phch_worker::set_tx_time(srslte_timestamp_t _tx_time, uint32_t next_offset)
 {
+  this->next_offset = next_offset;
   memcpy(&tx_time, &_tx_time, sizeof(srslte_timestamp_t));
 }
 
@@ -954,8 +959,8 @@ void phch_worker::encode_pucch()
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: power=%.2f dBm, tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f Hz%s\n",
-         tx_power, (tti+4)%10240, 
+  Info("PUCCH: tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f Hz%s\n",
+         (tti+4)%10240,
          last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
        uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
        uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",

From c7c2ab6ed5fa978d05231b1767ba3a179050f3b4 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 22 Sep 2017 00:58:21 +0200
Subject: [PATCH 211/269] Fix to prevent USRP from disabling TX when
 transmitting zeros

---
 srsue/src/phy/phch_common.cc | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/srsue/src/phy/phch_common.cc b/srsue/src/phy/phch_common.cc
index 6baf0b70e..d49b1ced2 100644
--- a/srsue/src/phy/phch_common.cc
+++ b/srsue/src/phy/phch_common.cc
@@ -61,6 +61,10 @@ phch_common::phch_common(uint32_t max_mutex_) : tx_mutex(max_mutex_)
   cur_pusch_power = 0;
   bzero(zeros, 50000*sizeof(cf_t));
 
+  // FIXME: This is an ungly fix to avoid the TX filters to empty
+  for (int i=0;i<50000;i++) {
+    zeros[i] = 0.01*cexpf(((float) i/50000)*0.1*_Complex_I);
+  }
   bzero(&dl_metrics, sizeof(dl_metrics_t));
   dl_metrics_read = true;
   dl_metrics_count = 0;

From 0636aa289c74ecf2d45d19dc2a436e8c793a9edd Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 22 Sep 2017 12:31:46 +0200
Subject: [PATCH 212/269] use metrics_hub in UE

---
 lib/include/srslte/common/metrics_hub.h |  2 +-
 srsue/hdr/metrics_stdout.h              | 16 ++------
 srsue/hdr/ue_metrics_interface.h        |  3 +-
 srsue/src/main.cc                       | 15 +++++---
 srsue/src/metrics_stdout.cc             | 50 +------------------------
 5 files changed, 17 insertions(+), 69 deletions(-)

diff --git a/lib/include/srslte/common/metrics_hub.h b/lib/include/srslte/common/metrics_hub.h
index 9575347a2..fb6cf2ad4 100644
--- a/lib/include/srslte/common/metrics_hub.h
+++ b/lib/include/srslte/common/metrics_hub.h
@@ -24,7 +24,7 @@ template<typename metrics_t>
 class metrics_listener
 {
 public: 
-  virtual void set_metrics(metrics_t &m) = 0; 
+  virtual void set_metrics(metrics_t &m, float report_period_secs=1.0) = 0;
 }; 
 
 template<typename metrics_t>
diff --git a/srsue/hdr/metrics_stdout.h b/srsue/hdr/metrics_stdout.h
index cd3efc1a3..b806c294d 100644
--- a/srsue/hdr/metrics_stdout.h
+++ b/srsue/hdr/metrics_stdout.h
@@ -36,35 +36,25 @@
 #include <stdint.h>
 #include <string>
 
+#include "srslte/common/metrics_hub.h"
 #include "ue_metrics_interface.h"
 
 namespace srsue {
 
-class metrics_stdout
+class metrics_stdout : public srslte::metrics_listener<ue_metrics_t>
 {
 public:
   metrics_stdout();
 
-  bool init(ue_metrics_interface *u, float report_period_secs=1.0);
-  void stop();
   void toggle_print(bool b);
-  static void* metrics_thread_start(void *m);
-  void metrics_thread_run();
+  void set_metrics(ue_metrics_t &m, float report_period_secs);
 
 private:
-  void        print_metrics();
-  void        print_disconnect();
   std::string float_to_string(float f, int digits);
   std::string float_to_eng_string(float f, int digits);
   std::string int_to_eng_string(int f, int digits);
-  
-  ue_metrics_interface *ue_;
 
-  bool          started;
   bool          do_print;
-  pthread_t     metrics_thread;
-  ue_metrics_t  metrics;
-  float         metrics_report_period; // seconds
   uint8_t       n_reports;
 };
 
diff --git a/srsue/hdr/ue_metrics_interface.h b/srsue/hdr/ue_metrics_interface.h
index 9555574f3..bc0614a03 100644
--- a/srsue/hdr/ue_metrics_interface.h
+++ b/srsue/hdr/ue_metrics_interface.h
@@ -29,6 +29,7 @@
 
 #include <stdint.h>
 
+#include "srslte/common/metrics_hub.h"
 #include "upper/gw_metrics.h"
 #include "srslte/upper/rlc_metrics.h"
 #include "mac/mac_metrics.h"
@@ -52,7 +53,7 @@ typedef struct {
 }ue_metrics_t;
 
 // UE interface
-class ue_metrics_interface
+class ue_metrics_interface : public srslte::metrics_interface<ue_metrics_t>
 {
 public:
   virtual bool get_metrics(ue_metrics_t &m) = 0;
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 02394f3e4..f140a593d 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -38,6 +38,7 @@
 
 #include "ue.h"
 #include "metrics_stdout.h"
+#include "srslte/common/metrics_hub.h"
 #include "srslte/version.h"
 
 using namespace std;
@@ -320,13 +321,13 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
 
 static bool running = true;
 static bool do_metrics = false;
+metrics_stdout metrics_screen;
 
 void sig_int_handler(int signo) {
   running = false;
 }
 
 void *input_loop(void *m) {
-  metrics_stdout *metrics = (metrics_stdout *) m;
   char key;
   while (running) {
     cin >> key;
@@ -337,7 +338,7 @@ void *input_loop(void *m) {
       } else {
         cout << "Enter t to restart trace." << endl;
       }
-      metrics->toggle_print(do_metrics);
+      metrics_screen.toggle_print(do_metrics);
     }
   }
   return NULL;
@@ -345,6 +346,7 @@ void *input_loop(void *m) {
 
 int main(int argc, char *argv[])
 {
+  srslte::metrics_hub<ue_metrics_t> metricshub;
   signal(SIGINT, sig_int_handler);
   all_args_t args;
   parse_args(&args, argc, argv);
@@ -361,11 +363,12 @@ int main(int argc, char *argv[])
     exit(1);
   }
 
-  metrics_stdout metrics;
-  metrics.init(ue, args.expert.metrics_period_secs);
+
+  metricshub.init(ue, args.expert.metrics_period_secs);
+  metricshub.add_listener(&metrics_screen);
 
   pthread_t input;
-  pthread_create(&input, NULL, &input_loop, &metrics);
+  pthread_create(&input, NULL, &input_loop, &args);
 
   bool plot_started = false;
   bool signals_pregenerated = false;
@@ -383,7 +386,7 @@ int main(int argc, char *argv[])
     sleep(1);
   }
   pthread_cancel(input);
-  metrics.stop();
+  metricshub.stop();
   ue->stop();
   ue->cleanup();
   cout << "---  exiting  ---" << endl;
diff --git a/srsue/src/metrics_stdout.cc b/srsue/src/metrics_stdout.cc
index 0a86683d2..3ca4756a3 100644
--- a/srsue/src/metrics_stdout.cc
+++ b/srsue/src/metrics_stdout.cc
@@ -48,57 +48,18 @@ char const * const prefixes[2][9] =
 };
 
 metrics_stdout::metrics_stdout()
-    :started(false)
-    ,do_print(false)
+    :do_print(false)
     ,n_reports(10)
 {
 }
 
-bool metrics_stdout::init(ue_metrics_interface *u, float report_period_secs)
-{
-  ue_ = u;
-  metrics_report_period = report_period_secs;
-
-  started = true;
-  pthread_create(&metrics_thread, NULL, &metrics_thread_start, this);
-  return true;
-}
-
-void metrics_stdout::stop()
-{
-  if(started)
-  {
-    started = false;
-    pthread_join(metrics_thread, NULL);
-  }
-}
-
 void metrics_stdout::toggle_print(bool b)
 {
   do_print = b;
 }
 
-void* metrics_stdout::metrics_thread_start(void *m_)
-{
-  metrics_stdout *m = (metrics_stdout*)m_;
-  m->metrics_thread_run();
-  return NULL;
-}
-
-void metrics_stdout::metrics_thread_run()
-{
-  while(started)
-  {
-    usleep(metrics_report_period*1e6);
-    if(ue_->get_metrics(metrics)) {
-      print_metrics();
-    } else {
-      print_disconnect();
-    }
-  }
-}
 
-void metrics_stdout::print_metrics()
+void metrics_stdout::set_metrics(ue_metrics_t &metrics, float metrics_report_period)
 {
   if(!do_print)
     return;
@@ -138,13 +99,6 @@ void metrics_stdout::print_metrics()
   
 }
 
-void metrics_stdout::print_disconnect()
-{
-  if(do_print) {
-    cout << "--- disconnected ---" << endl;
-  }
-}
-
 std::string metrics_stdout::float_to_string(float f, int digits)
 {
   std::ostringstream os;

From f92728ad6efe69edc6b69eea03eb91db4318c306 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 22 Sep 2017 15:00:07 +0200
Subject: [PATCH 213/269] add console message when prenting metrics in RRC_IDLE

---
 srsue/hdr/metrics_stdout.h       |  6 ++++--
 srsue/hdr/ue_metrics_interface.h |  1 +
 srsue/src/main.cc                |  1 +
 srsue/src/metrics_stdout.cc      | 13 ++++++++++++-
 srsue/src/ue.cc                  |  2 +-
 5 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/srsue/hdr/metrics_stdout.h b/srsue/hdr/metrics_stdout.h
index b806c294d..8ae33b24e 100644
--- a/srsue/hdr/metrics_stdout.h
+++ b/srsue/hdr/metrics_stdout.h
@@ -48,14 +48,16 @@ public:
 
   void toggle_print(bool b);
   void set_metrics(ue_metrics_t &m, float report_period_secs);
+  void set_ue_handle(ue_metrics_interface *ue_);
 
 private:
   std::string float_to_string(float f, int digits);
   std::string float_to_eng_string(float f, int digits);
   std::string int_to_eng_string(int f, int digits);
 
-  bool          do_print;
-  uint8_t       n_reports;
+  bool                  do_print;
+  uint8_t               n_reports;
+  ue_metrics_interface* ue;
 };
 
 } // namespace srsue
diff --git a/srsue/hdr/ue_metrics_interface.h b/srsue/hdr/ue_metrics_interface.h
index bc0614a03..20ee6f031 100644
--- a/srsue/hdr/ue_metrics_interface.h
+++ b/srsue/hdr/ue_metrics_interface.h
@@ -57,6 +57,7 @@ class ue_metrics_interface : public srslte::metrics_interface<ue_metrics_t>
 {
 public:
   virtual bool get_metrics(ue_metrics_t &m) = 0;
+  virtual bool is_attached() = 0;
 };
 
 } // namespace srsue
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index f140a593d..8ad288b4b 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -366,6 +366,7 @@ int main(int argc, char *argv[])
 
   metricshub.init(ue, args.expert.metrics_period_secs);
   metricshub.add_listener(&metrics_screen);
+  metrics_screen.set_ue_handle(ue);
 
   pthread_t input;
   pthread_create(&input, NULL, &input_loop, &args);
diff --git a/srsue/src/metrics_stdout.cc b/srsue/src/metrics_stdout.cc
index 3ca4756a3..6828c912f 100644
--- a/srsue/src/metrics_stdout.cc
+++ b/srsue/src/metrics_stdout.cc
@@ -50,9 +50,15 @@ char const * const prefixes[2][9] =
 metrics_stdout::metrics_stdout()
     :do_print(false)
     ,n_reports(10)
+    ,ue(NULL)
 {
 }
 
+void metrics_stdout::set_ue_handle(ue_metrics_interface *ue_)
+{
+  ue = ue_;
+}
+
 void metrics_stdout::toggle_print(bool b)
 {
   do_print = b;
@@ -61,9 +67,14 @@ void metrics_stdout::toggle_print(bool b)
 
 void metrics_stdout::set_metrics(ue_metrics_t &metrics, float metrics_report_period)
 {
-  if(!do_print)
+  if(!do_print || ue == NULL)
     return;
 
+  if (!ue->is_attached()) {
+    cout << "--- disconnected ---" << endl;
+    return;
+  }
+
   if(++n_reports > 10)
   {
     n_reports = 0;
diff --git a/srsue/src/ue.cc b/srsue/src/ue.cc
index 646b87612..03e4b6546 100644
--- a/srsue/src/ue.cc
+++ b/srsue/src/ue.cc
@@ -247,7 +247,7 @@ void ue::stop()
 
 bool ue::is_attached()
 {
-  return (EMM_STATE_REGISTERED == nas.get_state());
+  return (RRC_STATE_CONNECTED == rrc.get_state());
 }
 
 void ue::start_plot() {

From 4eb52f58fc606609fbae366942c780248f256738 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Sat, 23 Sep 2017 19:40:47 +0200
Subject: [PATCH 214/269] wait until periodic thread finishes during stop

---
 lib/include/srslte/common/metrics_hub.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/lib/include/srslte/common/metrics_hub.h b/lib/include/srslte/common/metrics_hub.h
index fb6cf2ad4..6020271e2 100644
--- a/lib/include/srslte/common/metrics_hub.h
+++ b/lib/include/srslte/common/metrics_hub.h
@@ -38,6 +38,7 @@ public:
   }
   void stop() {
     thread_cancel();
+    wait_thread_finish();
   }
   
   void add_listener(metrics_listener<metrics_t> *listener) {

From 124bd0a2bafd9648c6a7fa5c98b2c4af76e1cf69 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Sat, 23 Sep 2017 19:43:01 +0200
Subject: [PATCH 215/269] add basic metrics test

---
 srsue/test/CMakeLists.txt  |  4 ++
 srsue/test/metrics_test.cc | 81 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 85 insertions(+)
 create mode 100644 srsue/test/metrics_test.cc

diff --git a/srsue/test/CMakeLists.txt b/srsue/test/CMakeLists.txt
index c9949a7e2..c83b99413 100644
--- a/srsue/test/CMakeLists.txt
+++ b/srsue/test/CMakeLists.txt
@@ -21,3 +21,7 @@
 add_subdirectory(phy)
 add_subdirectory(mac)
 add_subdirectory(upper)
+
+add_executable(metrics_test metrics_test.cc ../src/metrics_stdout.cc)
+target_link_libraries(metrics_test srslte_phy srslte_common)
+add_test(metrics_test metrics_test)
diff --git a/srsue/test/metrics_test.cc b/srsue/test/metrics_test.cc
new file mode 100644
index 000000000..fbca7e1a4
--- /dev/null
+++ b/srsue/test/metrics_test.cc
@@ -0,0 +1,81 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <iostream>
+#include <strings.h>
+#include <unistd.h>
+#include "ue_metrics_interface.h"
+#include "srslte/common/metrics_hub.h"
+#include "metrics_stdout.h"
+
+using namespace srsue;
+
+namespace srsue {
+
+// fake classes
+class ue_dummy : public ue_metrics_interface
+{
+public:
+  bool get_metrics(ue_metrics_t &m)
+  {
+    // fill dummy values
+    bzero(&m, sizeof(ue_metrics_t));
+    m.rf.rf_o = 10;
+    m.phy.dl.rsrp = -10.0;
+    m.phy.dl.pathloss = 74;
+    return true;
+  }
+
+  bool is_attached()
+  {
+    return true;
+  }
+};
+}
+
+int main(int argc, char **argv)
+{
+  float period = 1.0;
+  ue_dummy ue;
+
+  // the default metrics type for stdout output
+  metrics_stdout metrics_screen;
+  metrics_screen.set_ue_handle(&ue);
+
+  // create metrics hub and register metrics for stdout
+  srslte::metrics_hub<ue_metrics_t> metricshub;
+  metricshub.init(&ue, period);
+  metricshub.add_listener(&metrics_screen);
+
+  // enable printing
+  metrics_screen.toggle_print(true);
+
+  std::cout << "Running for 2 seconds .." << std::endl;
+  usleep(2e6);
+
+  metricshub.stop();
+  return 0;
+}

From 6eb6468ba99e040cf33ffdd2096f0bf84035ca00 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Sat, 23 Sep 2017 22:24:37 +0200
Subject: [PATCH 216/269] add CSV metrics object

---
 srsue/hdr/metrics_csv.h    |  64 ++++++++++++++++++++++
 srsue/src/CMakeLists.txt   |   2 +-
 srsue/src/metrics_csv.cc   | 109 +++++++++++++++++++++++++++++++++++++
 srsue/test/CMakeLists.txt  |   4 +-
 srsue/test/metrics_test.cc |  41 ++++++++++++++
 5 files changed, 217 insertions(+), 3 deletions(-)
 create mode 100644 srsue/hdr/metrics_csv.h
 create mode 100644 srsue/src/metrics_csv.cc

diff --git a/srsue/hdr/metrics_csv.h b/srsue/hdr/metrics_csv.h
new file mode 100644
index 000000000..a897265d2
--- /dev/null
+++ b/srsue/hdr/metrics_csv.h
@@ -0,0 +1,64 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ * File:        metrics_csv.h
+ * Description: Metrics class writing to CSV file.
+ *****************************************************************************/
+
+#ifndef METRICS_CSV_H
+#define METRICS_CSV_H
+
+#include <pthread.h>
+#include <stdint.h>
+#include <string>
+#include <iostream>
+#include <fstream>
+
+#include "srslte/common/metrics_hub.h"
+#include "ue_metrics_interface.h"
+
+namespace srsue {
+
+class metrics_csv : public srslte::metrics_listener<ue_metrics_t>
+{
+public:
+  metrics_csv(std::string filename);
+
+  void set_metrics(ue_metrics_t &m, float report_period_secs);
+  void set_ue_handle(ue_metrics_interface *ue_);
+
+private:
+  std::string float_to_string(float f, int digits, bool add_semicolon = true);
+
+  std::ofstream         file;
+  ue_metrics_interface* ue;
+  uint32_t              n_reports;
+};
+
+} // namespace srsue
+
+#endif // METRICS_CSV_H
diff --git a/srsue/src/CMakeLists.txt b/srsue/src/CMakeLists.txt
index 5c6000f63..0755a6490 100644
--- a/srsue/src/CMakeLists.txt
+++ b/srsue/src/CMakeLists.txt
@@ -31,7 +31,7 @@ if (RPATH)
   set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
 endif (RPATH)
 
-add_executable(srsue main.cc ue_base.cc ue.cc metrics_stdout.cc)
+add_executable(srsue main.cc ue_base.cc ue.cc metrics_stdout.cc metrics_csv.cc)
 target_link_libraries(srsue   srsue_mac
                               srsue_phy
                               srsue_upper
diff --git a/srsue/src/metrics_csv.cc b/srsue/src/metrics_csv.cc
new file mode 100644
index 000000000..18c761263
--- /dev/null
+++ b/srsue/src/metrics_csv.cc
@@ -0,0 +1,109 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2015 The srsUE Developers. See the
+ * COPYRIGHT file at the top-level directory of this distribution.
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsUE library.
+ *
+ * srsUE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsUE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include "metrics_csv.h"
+
+#include <unistd.h>
+#include <sstream>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+#include <iomanip>
+#include <iostream>
+
+#include <stdio.h>
+
+using namespace std;
+
+namespace srsue{
+
+metrics_csv::metrics_csv(std::string filename)
+  :n_reports(0)
+  ,ue(NULL)
+{
+  file.open(filename.c_str());
+}
+
+void metrics_csv::set_ue_handle(ue_metrics_interface *ue_)
+{
+  ue = ue_;
+}
+
+void metrics_csv::set_metrics(ue_metrics_t &metrics, float metrics_report_period)
+{
+  if (file.is_open()) {
+    if (!ue->is_attached()) {
+      file << "# disconnected" << endl;
+      return;
+    }
+
+    if(n_reports == 0) {
+      file << "time;rsrp;pl;cfo;dl_mcs;dl_snr;dl_turbo;dl_brate;dl_bler;ul_mcs;ul_buff;ul_brate;ul_bler;rf_o;rf_u;rf_l" << endl;
+    }
+    file << (metrics_report_period*n_reports) << ";";
+    file << float_to_string(metrics.phy.dl.rsrp, 2);
+    file << float_to_string(metrics.phy.dl.pathloss, 2);
+    file << float_to_string(metrics.phy.sync.cfo, 2);
+    file << float_to_string(metrics.phy.dl.mcs, 2);
+    file << float_to_string(metrics.phy.dl.sinr, 2);
+    file << float_to_string(metrics.phy.dl.turbo_iters, 2);
+    file << float_to_string((float) metrics.mac.rx_brate/metrics_report_period, 2);
+    if (metrics.mac.rx_pkts > 0) {
+      file << float_to_string((float) 100*metrics.mac.rx_errors/metrics.mac.rx_pkts, 1);
+    } else {
+      file << float_to_string(0, 2);
+    }
+    file << float_to_string(metrics.phy.ul.mcs, 2);
+    file << float_to_string((float) metrics.mac.ul_buffer, 2);
+    file << float_to_string((float) metrics.mac.tx_brate/metrics_report_period, 2);
+    if (metrics.mac.tx_pkts > 0) {
+      file << float_to_string((float) 100*metrics.mac.tx_errors/metrics.mac.tx_pkts, 1);
+    } else {
+      file << float_to_string(0, 2);
+    }
+    file << float_to_string(metrics.rf.rf_o, 2);
+    file << float_to_string(metrics.rf.rf_u, 2);
+    file << float_to_string(metrics.rf.rf_l, 2, false);
+    file << endl;
+
+    n_reports++;
+  } else {
+    std::cout << "Error, couldn't write CSV file." << std::endl;
+  }
+}
+
+std::string metrics_csv::float_to_string(float f, int digits, bool add_semicolon)
+{
+  std::ostringstream os;
+  const int precision = (f == 0.0) ? digits-1 : digits - log10(fabs(f))-2*DBL_EPSILON;
+  os << std::fixed << std::setprecision(precision) << f;
+  if (add_semicolon)
+    os << ';';
+  return os.str();
+}
+
+} // namespace srsue
diff --git a/srsue/test/CMakeLists.txt b/srsue/test/CMakeLists.txt
index c83b99413..217c369ee 100644
--- a/srsue/test/CMakeLists.txt
+++ b/srsue/test/CMakeLists.txt
@@ -22,6 +22,6 @@ add_subdirectory(phy)
 add_subdirectory(mac)
 add_subdirectory(upper)
 
-add_executable(metrics_test metrics_test.cc ../src/metrics_stdout.cc)
+add_executable(metrics_test metrics_test.cc ../src/metrics_stdout.cc ../src/metrics_csv.cc)
 target_link_libraries(metrics_test srslte_phy srslte_common)
-add_test(metrics_test metrics_test)
+add_test(metrics_test metrics_test -o ${CMAKE_CURRENT_BINARY_DIR}/ue_metrics.csv)
diff --git a/srsue/test/metrics_test.cc b/srsue/test/metrics_test.cc
index fbca7e1a4..9cd17f824 100644
--- a/srsue/test/metrics_test.cc
+++ b/srsue/test/metrics_test.cc
@@ -25,16 +25,21 @@
  */
 
 #include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
 #include <strings.h>
 #include <unistd.h>
 #include "ue_metrics_interface.h"
 #include "srslte/common/metrics_hub.h"
 #include "metrics_stdout.h"
+#include "metrics_csv.h"
 
 using namespace srsue;
 
 namespace srsue {
 
+char *csv_file_name = NULL;
+
 // fake classes
 class ue_dummy : public ue_metrics_interface
 {
@@ -56,19 +61,55 @@ public:
 };
 }
 
+void usage(char *prog) {
+  printf("Usage: %s -o csv_output_file\n", prog);
+}
+
+void parse_args(int argc, char **argv) {
+  int opt;
+
+  while ((opt = getopt(argc, argv, "o")) != -1) {
+    switch(opt) {
+      case 'o':
+        csv_file_name = argv[optind];
+        break;
+      default:
+        usage(argv[0]);
+        exit(-1);
+    }
+  }
+  if (!csv_file_name) {
+    usage(argv[0]);
+    exit(-1);
+  }
+}
+
+
 int main(int argc, char **argv)
 {
   float period = 1.0;
   ue_dummy ue;
 
+  if (argc < 3) {
+    usage(argv[0]);
+    exit(-1);
+  }
+
+  parse_args(argc,argv);
+
   // the default metrics type for stdout output
   metrics_stdout metrics_screen;
   metrics_screen.set_ue_handle(&ue);
 
+  // the CSV file writer
+  metrics_csv metrics_file(csv_file_name);
+  metrics_file.set_ue_handle(&ue);
+
   // create metrics hub and register metrics for stdout
   srslte::metrics_hub<ue_metrics_t> metricshub;
   metricshub.init(&ue, period);
   metricshub.add_listener(&metrics_screen);
+  metricshub.add_listener(&metrics_file);
 
   // enable printing
   metrics_screen.toggle_print(true);

From 15d89c3e958a140e5db877781a1c9e25500c7901 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Sun, 24 Sep 2017 13:33:22 +0200
Subject: [PATCH 217/269] add CSV metrics to UE

---
 srsue/hdr/ue_base.h   |  2 ++
 srsue/src/main.cc     | 16 +++++++++++++++-
 srsue/ue.conf.example |  6 ++++++
 3 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/srsue/hdr/ue_base.h b/srsue/hdr/ue_base.h
index b105b2088..411896f70 100644
--- a/srsue/hdr/ue_base.h
+++ b/srsue/hdr/ue_base.h
@@ -107,6 +107,8 @@ typedef struct {
   float      metrics_period_secs;
   bool pregenerate_signals;
   std::string ue_cateogry;
+  bool metrics_csv_enable;
+  std::string metrics_csv_filename;
 }expert_args_t;
 
 typedef struct {
diff --git a/srsue/src/main.cc b/srsue/src/main.cc
index 8ad288b4b..569083998 100644
--- a/srsue/src/main.cc
+++ b/srsue/src/main.cc
@@ -38,6 +38,7 @@
 
 #include "ue.h"
 #include "metrics_stdout.h"
+#include "metrics_csv.h"
 #include "srslte/common/metrics_hub.h"
 #include "srslte/version.h"
 
@@ -135,6 +136,14 @@ void parse_args(all_args_t *args, int argc, char *argv[]) {
      bpo::value<float>(&args->expert.metrics_period_secs)->default_value(1.0),
      "Periodicity for metrics in seconds")
 
+    ("expert.metrics_csv_enable",
+     bpo::value<bool>(&args->expert.metrics_csv_enable)->default_value(false),
+     "Write UE metrics to CSV file")
+
+    ("expert.metrics_csv_filename",
+     bpo::value<string>(&args->expert.metrics_csv_filename)->default_value("/tmp/ue_metrics.csv"),
+     "Metrics CSV filename")
+
     ("expert.pregenerate_signals",
      bpo::value<bool>(&args->expert.pregenerate_signals)->default_value(false),
      "Pregenerate uplink signals after attach. Improves CPU performance.")
@@ -363,11 +372,16 @@ int main(int argc, char *argv[])
     exit(1);
   }
 
-
   metricshub.init(ue, args.expert.metrics_period_secs);
   metricshub.add_listener(&metrics_screen);
   metrics_screen.set_ue_handle(ue);
 
+  metrics_csv metrics_file(args.expert.metrics_csv_filename);
+  if (args.expert.metrics_csv_enable) {
+    metricshub.add_listener(&metrics_file);
+    metrics_file.set_ue_handle(ue);
+  }
+
   pthread_t input;
   pthread_create(&input, NULL, &input_loop, &args);
 
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 30b152280..332d00e59 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -130,6 +130,10 @@ enable = false
 #
 # pregenerate_signals:  Pregenerate uplink signals after attach. Improves CPU performance.
 #
+# metrics_csv_enable:   Write UE metrics to CSV file.
+#
+# metrics_csv_filename: File path to use for CSV metrics.
+#
 #####################################################################
 [expert]
 #ue_category         = 4
@@ -149,6 +153,8 @@ enable = false
 #sss_algorithm       = full
 #estimator_fil_w     = 0.1
 #pregenerate_signals = false
+#metrics_csv_enable  = false
+#metrics_csv_filename = /tmp/ue_metrics.csv
 
 #####################################################################
 # Manual RF calibration

From 3aabd832892668891b96328caa80eebc4039d8d9 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Sun, 24 Sep 2017 20:49:48 +0200
Subject: [PATCH 218/269] fixed blocked timers due to syncrhonization lost

---
 srsue/hdr/phy/phch_recv.h  |  3 +-
 srsue/src/mac/mac.cc       | 61 ++++++++++++++++++--------------------
 srsue/src/phy/phch_recv.cc | 17 ++++++++---
 3 files changed, 44 insertions(+), 37 deletions(-)

diff --git a/srsue/hdr/phy/phch_recv.h b/srsue/hdr/phy/phch_recv.h
index 8c768f46b..044960760 100644
--- a/srsue/hdr/phy/phch_recv.h
+++ b/srsue/hdr/phy/phch_recv.h
@@ -87,7 +87,7 @@ private:
 
   void   set_sampling_rate();
   bool   set_frequency();
-  void   resync_sfn();
+  void   resync_sfn(bool is_connected = false);
   bool   stop_sync();
 
   void   cell_search_inc();
@@ -129,6 +129,7 @@ private:
     IDLE = 0,
     CELL_SEARCH,
     CELL_SELECT,
+    CELL_RESELECT,
     CELL_MEASURE,
     CELL_CAMP
   } phy_state;
diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 8d5bf91b9..60327fcd4 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -142,47 +142,44 @@ void mac::reset()
 void mac::run_thread() {
   int cnt=0;
 
-  while(started) {
-
-    while (!phy_h->sync_status() && started) {
-      usleep(5000);
-      if (phy_h->sync_status()) {
-        Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
-        ttisync.set_producer_cntr(phy_h->get_current_tti());
-      }
+  while (!phy_h->sync_status() && started) {
+    usleep(5000);
+    if (phy_h->sync_status()) {
+      Debug("Setting ttysync to %d\n", phy_h->get_current_tti());
+      ttisync.set_producer_cntr(phy_h->get_current_tti());
     }
+  }
 
-    if (started && phy_h->sync_status()) {
-      /* Warning: Here order of invocation of procedures is important!! */
-      ttisync.wait();
-      tti = phy_h->get_current_tti();
+  while(started) {
 
-      log_h->step(tti);
+    /* Warning: Here order of invocation of procedures is important!! */
+    ttisync.wait();
+    tti = phy_h->get_current_tti();
 
-      timers.step_all();
+    log_h->step(tti);
+    timers.step_all();
 
-      // Step all procedures
-      bsr_procedure.step(tti);
-      phr_procedure.step(tti);
+    // Step all procedures
+    bsr_procedure.step(tti);
+    phr_procedure.step(tti);
 
-      // Check if BSR procedure need to start SR
+    // Check if BSR procedure need to start SR
 
-      if (bsr_procedure.need_to_send_sr(tti)) {
-        Debug("Starting SR procedure by BSR request, PHY TTI=%d\n", tti);
-        sr_procedure.start();
-      }
-      if (bsr_procedure.need_to_reset_sr()) {
-        Debug("Resetting SR procedure by BSR request\n");
-        sr_procedure.reset();
-      }
-      sr_procedure.step(tti);
+    if (bsr_procedure.need_to_send_sr(tti)) {
+      Debug("Starting SR procedure by BSR request, PHY TTI=%d\n", tti);
+      sr_procedure.start();
+    }
+    if (bsr_procedure.need_to_reset_sr()) {
+      Debug("Resetting SR procedure by BSR request\n");
+      sr_procedure.reset();
+    }
+    sr_procedure.step(tti);
 
-      // Check SR if we need to start RA
-      if (sr_procedure.need_random_access()) {
-        ra_procedure.start_mac_order();
-      }
-      ra_procedure.step(tti);
+    // Check SR if we need to start RA
+    if (sr_procedure.need_random_access()) {
+      ra_procedure.start_mac_order();
     }
+    ra_procedure.step(tti);
   }
 }
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index ba09f2d53..cad5219f9 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -422,7 +422,7 @@ int phch_recv::cell_meas_rsrp() {
   return 0;
 }
 
-void phch_recv::resync_sfn() {
+void phch_recv::resync_sfn(bool is_connected) {
 
   wait_radio_reset();
 
@@ -431,7 +431,7 @@ void phch_recv::resync_sfn() {
   srslte_ue_mib_reset(&ue_mib);
   Info("SYNC:  Starting SFN synchronization\n");
   sync_sfn_cnt = 0;
-  phy_state = CELL_SELECT;
+  phy_state = is_connected?CELL_RESELECT:CELL_SELECT;
 }
 
 void phch_recv::set_earfcn(std::vector<uint32_t> earfcn) {
@@ -463,7 +463,7 @@ void phch_recv::reset_sync() {
   Warning("SYNC:  Resetting sync, cell_search_in_progress=%s\n", cell_search_in_progress?"yes":"no");
   srslte_ue_sync_reset(&ue_mib_sync.ue_sync);
   srslte_ue_sync_reset(&ue_sync);
-  resync_sfn();
+  resync_sfn(true);
 }
 
 void phch_recv::cell_search_inc()
@@ -639,6 +639,7 @@ void phch_recv::run_thread() {
           }
         }
         break;
+      case CELL_RESELECT:
       case CELL_SELECT:
 
         srslte_ue_sync_decode_sss_on_track(&ue_sync, true);
@@ -664,8 +665,12 @@ void phch_recv::run_thread() {
         sync_sfn_cnt++;
         if (sync_sfn_cnt >= SYNC_SFN_TIMEOUT) {
           sync_sfn_cnt = 0;
-          phy_state = CELL_SEARCH;
           log_h->warning("SYNC:  Timeout while synchronizing SFN\n");
+          if (phy_state == CELL_SELECT) {
+            phy_state = CELL_SEARCH;
+          } else {
+            phy_state = IDLE;
+          }
         }
         break;
       case CELL_MEASURE:
@@ -733,6 +738,7 @@ void phch_recv::run_thread() {
               rrc->out_of_sync();
               worker->release();
               worker_com->reset_ul();
+              mac->tti_clock(tti);
               break;
             default:
               radio_error();
@@ -749,6 +755,9 @@ void phch_recv::run_thread() {
         }
         is_in_idle = true;
         usleep(1000);
+        // Keep running MAC timer from system clock
+        tti = (tti+1) % 10240;
+        mac->tti_clock(tti);
         break;
     }
   }

From 489ac434084b26c4820cbdd18d619cf2dd005462 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 25 Sep 2017 11:21:19 +0200
Subject: [PATCH 219/269] Minor logging edits

---
 srsenb/src/enb.cc            |  2 --
 srsue/src/phy/phch_worker.cc | 21 +++++++++++++--------
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index d00fd4c0b..2bf9c6b3f 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -71,9 +71,7 @@ bool enb::init(all_args_t *args_)
 
   if (!args->log.filename.compare("stdout")) {
     logger = &logger_stdout;
-    printf("log name is output\n");
   } else {
-    printf("lgo name is %s\n", args->log.filename.c_str());
     logger_file.init(args->log.filename);
     logger_file.log("\n\n");
     logger = &logger_file;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 3672c155d..40d4775d3 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -514,6 +514,8 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
                                      int rv[SRSLTE_MAX_CODEWORDS],
                                      uint16_t rnti, uint32_t harq_pid, bool acks[SRSLTE_MAX_CODEWORDS]) {
   char timestr[64];
+  char commonstr[128];
+  char tbstr[2][128];
   bool valid_config = true;
   timestr[0]='\0';
   srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
@@ -610,14 +612,17 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
         snprintf(timestr, 64, ", dec_time=%4d us", (int) t[0].tv_usec);
   #endif
 
-        Info(
-            "PDSCH: l_crb=%2d, harq=%d, scheme=%s, tb_en={%s, %s}, tbs={%d, %d}, mcs={%d, %d}, rv={%d, %d}, crc={%s, %s}, snr=%.1f dB, n_iter=%d, %s\n",
-            grant->nof_prb, harq_pid, srslte_mimotype2str(mimo_type), grant->tb_en[0] ? "on" : "off",
-            grant->tb_en[1] ? "on" : "off", grant->mcs[0].tbs / 8, grant->mcs[1].tbs / 8, grant->mcs[0].idx,
-            grant->mcs[1].idx, rv[0], rv[1], acks[0] ? "OK" : "KO", acks[1] ? "OK" : "KO",
-            10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)),
-            srslte_pdsch_last_noi(&ue_dl.pdsch),
-            timestr);
+        snprintf(commonstr, 128, "PDSCH: l_crb=%2d, harq=%d, snr=%.1f dB", grant->nof_prb, harq_pid,
+                 10 * log10(srslte_chest_dl_get_snr(&ue_dl.chest)));
+
+        for (int i=0;i<SRSLTE_MAX_CODEWORDS;i++) {
+          if (grant->tb_en[i]) {
+            snprintf(tbstr[i], 128, ", TB%d: tbs=%d, mcs=%d, rv=%d, crc=%s",
+                     i, grant->mcs[i].tbs/8, grant->mcs[i].idx, rv[i], acks[i] ? "OK" : "KO");
+          }
+        }
+
+        Info("%s%s%s%s\n", commonstr, grant->tb_en[0]?tbstr[0]:"", grant->tb_en[1]?tbstr[1]:"", timestr);
 
         // Store metrics
         dl_metrics.mcs    = grant->mcs[0].idx;

From 774a456e3116cdea05d4d6ee34bea357f4d84a95 Mon Sep 17 00:00:00 2001
From: Justin Tallon <justin@softwareradiosystems.com>
Date: Thu, 21 Sep 2017 11:48:09 +0100
Subject: [PATCH 220/269] adding phy layer support for mbms

---
 lib/examples/pdsch_enodeb.c                   | 295 +++++++----
 lib/examples/pdsch_ue.c                       | 143 ++++--
 .../srslte/phy/ch_estimation/chest_dl.h       |  21 +-
 .../srslte/phy/ch_estimation/refsignal_dl.h   |  44 +-
 lib/include/srslte/phy/common/phy_common.h    |  12 +
 lib/include/srslte/phy/common/sequence.h      |   6 +
 lib/include/srslte/phy/dft/ofdm.h             |  39 +-
 lib/include/srslte/phy/enb/enb_dl.h           |   2 +-
 lib/include/srslte/phy/phch/pmch.h            | 152 ++++++
 lib/include/srslte/phy/phch/ra.h              |   9 +
 lib/include/srslte/phy/phch/sch.h             |   1 +
 lib/include/srslte/phy/ue/ue_dl.h             |  52 +-
 lib/src/phy/ch_estimation/chest_dl.c          | 226 +++++---
 lib/src/phy/ch_estimation/refsignal_dl.c      | 211 +++++++-
 .../phy/ch_estimation/test/chest_test_dl.c    |   2 +-
 lib/src/phy/dft/ofdm.c                        | 118 ++++-
 lib/src/phy/enb/enb_dl.c                      |   2 +-
 lib/src/phy/phch/pmch.c                       | 486 ++++++++++++++++++
 lib/src/phy/phch/ra.c                         | 134 +++--
 lib/src/phy/phch/sch.c                        |   2 +-
 lib/src/phy/phch/sequences.c                  |   6 +
 lib/src/phy/ue/ue_dl.c                        | 163 +++++-
 lib/src/phy/ue/ue_sync.c                      |   1 +
 23 files changed, 1839 insertions(+), 288 deletions(-)
 create mode 100644 lib/include/srslte/phy/phch/pmch.h
 create mode 100644 lib/src/phy/phch/pmch.c

diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index d481ff20f..844e769a7 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -27,7 +27,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <strings.h>
 #include <unistd.h>
+#include <sys/select.h>
 #include <pthread.h>
 #include <semaphore.h>
 #include <signal.h>
@@ -38,10 +40,11 @@
 
 
 #define UE_CRNTI 0x1234
-
+#define M_CRNTI 0xFFFD
 
 #ifndef DISABLE_RF
 #include "srslte/phy/rf/rf.h"
+#include "srslte/phy/common/phy_common.h"
 srslte_rf_t rf;
 #else
 #warning Compiling pdsch_ue with no RF support
@@ -55,52 +58,67 @@ char *output_file_name = NULL;
 #define DOWN_KEY  66
 
 srslte_cell_t cell = {
-    25,                 // nof_prb
-    1,                  // nof_ports
-    0,                  // cell_id
-    SRSLTE_CP_NORM,     // cyclic prefix
-    SRSLTE_PHICH_NORM,  // PHICH length
-    SRSLTE_PHICH_R_1    // PHICH resources
+  25,               // nof_prb
+  1,                // nof_ports
+  0,                // cell_id
+  SRSLTE_CP_NORM,   // cyclic prefix
+  SRSLTE_PHICH_NORM, // PHICH length
+  SRSLTE_PHICH_R_1 // PHICH resources
 };
 
+uint16_t c = -1;
+  
 int net_port = -1; // -1 generates random dataThat means there is some problem sending samples to the device
 
-uint32_t cfi = 1;
+uint32_t cfi = 2;
 uint32_t mcs_idx = 1, last_mcs_idx = 1;
 int nof_frames = -1;
+
+
 char mimo_type_str[32] = "single";
 uint32_t nof_tb = 1;
 uint32_t multiplex_pmi = 0;
 uint32_t multiplex_nof_layers = 1;
 
+int mbsfn_area_id = -1;
 char *rf_args = "";
 float rf_amp = 0.8, rf_gain = 70.0, rf_freq = 2400000000;
 
 bool null_file_sink=false; 
 srslte_filesink_t fsink;
 srslte_ofdm_t ifft;
+srslte_ofdm_t ifft_mbsfn;
 srslte_pbch_t pbch;
 srslte_pcfich_t pcfich;
 srslte_pdcch_t pdcch;
 srslte_pdsch_t pdsch;
-srslte_pdsch_cfg_t pdsch_cfg; 
+srslte_pdsch_cfg_t pdsch_cfg;
+srslte_pmch_t pmch;
+srslte_pdsch_cfg_t  pmch_cfg;
 srslte_softbuffer_tx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
 srslte_regs_t regs;
-srslte_ra_dl_dci_t ra_dl;
+srslte_ra_dl_dci_t ra_dl;  
 int rvidx[SRSLTE_MAX_CODEWORDS] = {0, 0};
 
 cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL}, *output_buffer [SRSLTE_MAX_PORTS] = {NULL};
+
+
 int sf_n_re, sf_n_samples;
 
-pthread_t net_thread;
+pthread_t net_thread; 
 void *net_thread_fnc(void *arg);
 sem_t net_sem;
 bool net_packet_ready = false; 
 srslte_netsource_t net_source; 
 srslte_netsink_t net_sink; 
 
+
 int prbset_num = 1, last_prbset_num = 1; 
 int prbset_orig = 0; 
+//#define DATA_BUFF_SZ    1024*128
+//uint8_t data[8*DATA_BUFF_SZ], data2[DATA_BUFF_SZ];
+//uint8_t data_tmp[DATA_BUFF_SZ];
+
 
 #define DATA_BUFF_SZ    1024*1024
 uint8_t *data[2], data2[DATA_BUFF_SZ];
@@ -121,6 +139,7 @@ void usage(char *prog) {
   printf("\t-n number of frames [Default %d]\n", nof_frames);
   printf("\t-c cell id [Default %d]\n", cell.id);
   printf("\t-p nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-M MBSFN area id [Default %d]\n", mbsfn_area_id);
   printf("\t-x Transmission mode[single|diversity|cdd|multiplex] [Default %s]\n", mimo_type_str);
   printf("\t-b Precoding Matrix Index (multiplex mode only)* [Default %d]\n", multiplex_pmi);
   printf("\t-w Number of codewords/layers (multiplex mode only)* [Default %d]\n", multiplex_nof_layers);
@@ -132,7 +151,8 @@ void usage(char *prog) {
 
 void parse_args(int argc, char **argv) {
   int opt;
-  while ((opt = getopt(argc, argv, "aglfmoncpvutxbw")) != -1) {
+  while ((opt = getopt(argc, argv, "aglfmoncpvutxbwM")) != -1) {
+
     switch (opt) {
     case 'a':
       rf_args = argv[optind];
@@ -173,6 +193,9 @@ void parse_args(int argc, char **argv) {
     case 'w':
       multiplex_nof_layers = (uint32_t) atoi(argv[optind]);
       break;
+    case 'M':
+      mbsfn_area_id = atoi(argv[optind]);
+      break;
     case 'v':
       srslte_verbose++;
       break;
@@ -188,7 +211,7 @@ void parse_args(int argc, char **argv) {
   }
 #endif
 }
-
+  
 void base_init() {
   int i;
 
@@ -245,6 +268,7 @@ void base_init() {
     bzero(output_buffer[i], sizeof(cf_t) * sf_n_samples);
   }
 
+
   /* open file or USRP */
   if (output_file_name) {
     if (strcmp(output_file_name, "NULL")) {
@@ -291,7 +315,15 @@ void base_init() {
     fprintf(stderr, "Error creating iFFT object\n");
     exit(-1);
   }
+  if (srslte_ofdm_tx_init_mbsfn(&ifft_mbsfn, SRSLTE_CP_EXT, cell.nof_prb)) {
+    fprintf(stderr, "Error creating iFFT object\n");
+    exit(-1);
+  }
+  srslte_ofdm_set_non_mbsfn_region(&ifft_mbsfn, 2);
+  
   srslte_ofdm_set_normalize(&ifft, true);
+  srslte_ofdm_set_normalize(&ifft_mbsfn, true);
+  
   if (srslte_pbch_init(&pbch)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
@@ -300,12 +332,14 @@ void base_init() {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
   }
+  
+  
+  
 
   if (srslte_regs_init(&regs, cell)) {
     fprintf(stderr, "Error initiating regs\n");
     exit(-1);
   }
-
   if (srslte_pcfich_init(&pcfich, 1)) {
     fprintf(stderr, "Error creating PBCH object\n");
     exit(-1);
@@ -340,6 +374,14 @@ void base_init() {
 
   srslte_pdsch_set_rnti(&pdsch, UE_CRNTI);
 
+
+  if(mbsfn_area_id > -1){
+    if (srslte_pmch_init(&pmch, cell.nof_prb)) {
+      fprintf(stderr, "Error creating PMCH object\n");
+    }
+    srslte_pmch_set_area_id(&pmch, mbsfn_area_id);
+  }
+  
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     softbuffers[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
     if (!softbuffers[i]) {
@@ -354,6 +396,7 @@ void base_init() {
   }
 }
 
+
 void base_free() {
   int i;
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
@@ -366,8 +409,12 @@ void base_free() {
   srslte_pdcch_free(&pdcch);
   srslte_regs_free(&regs);
   srslte_pbch_free(&pbch);
-
+  if(mbsfn_area_id > -1){
+    srslte_pmch_free(&pmch); 
+  }
+  srslte_ofdm_tx_free(&ifft_mbsfn);
   srslte_ofdm_tx_free(&ifft);
+  
 
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     if (data[i]) {
@@ -481,7 +528,7 @@ int update_radl() {
   srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &dummy_grant);
   srslte_ra_dl_grant_to_nbits(&dummy_grant, cfi, cell, 0, &dummy_nbits);
   srslte_ra_dl_grant_fprint(stdout, &dummy_grant);
-
+  dummy_grant.sf_type = SRSLTE_SF_NORM;
   if (pdsch_cfg.mimo_type != SRSLTE_MIMO_TYPE_SINGLE_ANTENNA) {
     printf("\nTransmission mode key table:\n");
     printf("   Mode   |   1TB   | 2TB |\n");
@@ -596,6 +643,7 @@ int update_control() {
   }
 }
 
+
 /** Function run in a separate thread to receive UDP data */
 void *net_thread_fnc(void *arg) {
   int n; 
@@ -633,6 +681,7 @@ void *net_thread_fnc(void *arg) {
   return NULL;
 }
 
+
 int main(int argc, char **argv) {
   int nf=0, sf_idx=0, N_id_2=0;
   cf_t pss_signal[SRSLTE_PSS_LEN];
@@ -645,7 +694,8 @@ int main(int argc, char **argv) {
   srslte_dci_msg_t dci_msg;
   srslte_dci_location_t locations[SRSLTE_NSUBFRAMES_X_FRAME][30];
   uint32_t sfn; 
-  srslte_chest_dl_t est; 
+  srslte_refsignal_t csr_refs;
+  srslte_refsignal_t mbsfn_refs;
   
 #ifdef DISABLE_RF
   if (argc < 3) {
@@ -674,21 +724,31 @@ int main(int argc, char **argv) {
   srslte_pss_generate(pss_signal, N_id_2);
   srslte_sss_generate(sss_signal0, sss_signal5, cell.id);
   
-  /* Generate CRS signals */
-  if (srslte_chest_dl_init(&est, cell.nof_prb)) {
+
+  /* Generate reference signals */
+  if(srslte_refsignal_cs_init(&csr_refs, cell.nof_prb)) {
     fprintf(stderr, "Error initializing equalizer\n");
     exit(-1);
   }
-  if (srslte_chest_dl_set_cell(&est, cell)) {
-    fprintf(stderr, "Error initializing equalizer\n");
+  if(mbsfn_area_id > -1) {
+    if(srslte_refsignal_mbsfn_init(&mbsfn_refs, cell, mbsfn_area_id)) {
+      fprintf(stderr, "Error initializing equalizer\n");
+      exit(-1);
+    }
+  }
+  
+  if(srslte_refsignal_cs_set_cell(&csr_refs, cell)){
+    fprintf(stderr, "Error setting cell\n");
     exit(-1);
   }
-
+  
+  
   for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
     sf_symbols[i] = sf_buffer[i%cell.nof_ports];
     slot1_symbols[i] = &sf_buffer[i%cell.nof_ports][SRSLTE_SLOT_LEN_RE(cell.nof_prb, cell.cp)];
   }
 
+
 #ifndef DISABLE_RF
 
 
@@ -737,48 +797,51 @@ int main(int argc, char **argv) {
   /* Initiate valid DCI locations */
   for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
     srslte_pdcch_ue_locations(&pdcch, locations[i], 30, i, cfi, UE_CRNTI);
-    
   }
     
   nf = 0;
   
   bool send_data = false; 
-  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     srslte_softbuffer_tx_reset(softbuffers[i]);
   }
 
+
 #ifndef DISABLE_RF
   bool start_of_burst = true; 
 #endif
-  
+
   while ((nf < nof_frames || nof_frames == -1) && !go_exit) {
     for (sf_idx = 0; sf_idx < SRSLTE_NSUBFRAMES_X_FRAME && (nf < nof_frames || nof_frames == -1); sf_idx++) {
       /* Set Antenna port resource elements to zero */
       bzero(sf_symbols[0], sizeof(cf_t) * sf_n_re);
 
-      /* Populate Synchronization signals if required */
+
       if (sf_idx == 0 || sf_idx == 5) {
         srslte_pss_put_slot(pss_signal, sf_symbols[0], cell.nof_prb, SRSLTE_CP_NORM);
         srslte_sss_put_slot(sf_idx ? sss_signal5 : sss_signal0, sf_symbols[0], cell.nof_prb,
             SRSLTE_CP_NORM);
       }
-
+      
       /* Copy zeros, SSS, PSS into the rest of antenna ports */
       for (i = 1; i < cell.nof_ports; i++) {
         memcpy(sf_symbols[i], sf_symbols[0], sizeof(cf_t) * sf_n_re);
       }
-
-      /* Put reference signals */
-      for (i = 0; i < cell.nof_ports; i++) {
-        srslte_refsignal_cs_put_sf(cell, (uint32_t) i, est.csr_signal.pilots[i / 2][sf_idx], sf_symbols[i]);
+      
+      if(sf_idx == 1 && mbsfn_area_id > -1){
+        srslte_refsignal_mbsfn_put_sf(cell, 0,csr_refs.pilots[0][sf_idx], mbsfn_refs.pilots[0][sf_idx],  sf_symbols[0]);
+      } else { 
+        for (i = 0; i < cell.nof_ports; i++) {
+          srslte_refsignal_cs_put_sf(cell, (uint32_t) i, csr_refs.pilots[i / 2][sf_idx], sf_symbols[i]);
+        }
       }
-
+      
       srslte_pbch_mib_pack(&cell, sfn, bch_payload);
       if (sf_idx == 0) {
         srslte_pbch_encode(&pbch, bch_payload, slot1_symbols, nf%4);
       }
 
-      srslte_pcfich_encode(&pcfich, cfi, sf_symbols, sf_idx);       
+      srslte_pcfich_encode(&pcfich, cfi, sf_symbols, sf_idx);
 
       /* Update DL resource allocation from control port */
       if (update_control(sf_idx)) {
@@ -806,86 +869,134 @@ int main(int argc, char **argv) {
         } else {
           send_data = false;           
         }
-      }        
+      }      
       
       if (send_data) {
-        srslte_dci_format_t dci_format;
-        switch(pdsch_cfg.mimo_type) {
-          case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
-            dci_format = SRSLTE_DCI_FORMAT1;
-            break;
-          case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
-          case SRSLTE_MIMO_TYPE_CDD:
-            dci_format = SRSLTE_DCI_FORMAT2A;
-            break;
-          case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
-            dci_format = SRSLTE_DCI_FORMAT2;
-            if (multiplex_nof_layers == 1) {
-              ra_dl.pinfo = (uint8_t) (multiplex_pmi + 1);
-            } else {
-              ra_dl.pinfo = (uint8_t) multiplex_pmi;
-            }
-            break;
-          default:
-            fprintf(stderr, "Wrong MIMO configuration\n");
-            exit(SRSLTE_ERROR);
-        }
-        /* Encode PDCCH */
-        INFO("Putting DCI to location: n=%d, L=%d\n", locations[sf_idx][0].ncce, locations[sf_idx][0].L);
-        srslte_dci_msg_pack_pdsch(&ra_dl, dci_format, &dci_msg, cell.nof_prb, cell.nof_ports, false);
-        if (srslte_pdcch_encode(&pdcch, &dci_msg, locations[sf_idx][0], UE_CRNTI, sf_symbols, sf_idx, cfi)) {
-          fprintf(stderr, "Error encoding DCI message\n");
-          exit(-1);
-        }
+        if(sf_idx != 1 || mbsfn_area_id < 0) { // PDCCH + PDSCH
+          srslte_dci_format_t dci_format;
+          switch(pdsch_cfg.mimo_type) {
+            case SRSLTE_MIMO_TYPE_SINGLE_ANTENNA:
+              dci_format = SRSLTE_DCI_FORMAT1;
+              break;
+            case SRSLTE_MIMO_TYPE_TX_DIVERSITY:
+            case SRSLTE_MIMO_TYPE_CDD:
+              dci_format = SRSLTE_DCI_FORMAT2A;
+              break;
+            case SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX:
+              dci_format = SRSLTE_DCI_FORMAT2;
+              if (multiplex_nof_layers == 1) {
+                ra_dl.pinfo = (uint8_t) (multiplex_pmi + 1);
+              } else {
+                ra_dl.pinfo = (uint8_t) multiplex_pmi;
+              }
+              break;
+            default:
+              fprintf(stderr, "Wrong MIMO configuration\n");
+              exit(SRSLTE_ERROR);
+          }
+          /* Encode PDCCH */
+          INFO("Putting DCI to location: n=%d, L=%d\n", locations[sf_idx][0].ncce, locations[sf_idx][0].L);
+          srslte_dci_msg_pack_pdsch(&ra_dl, dci_format, &dci_msg, cell.nof_prb, cell.nof_ports, false);
+          if (srslte_pdcch_encode(&pdcch, &dci_msg, locations[sf_idx][0], UE_CRNTI, sf_symbols, sf_idx, cfi)) {
+            fprintf(stderr, "Error encoding DCI message\n");
+            exit(-1);
+          }
 
-        /* Configure pdsch_cfg parameters */
-        srslte_ra_dl_grant_t grant; 
-        srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
-        if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
-          fprintf(stderr, "Error configuring PDSCH\n");
-          exit(-1);
-        }
-       
-        /* Encode PDSCH */
-        if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI, sf_symbols)) {
-          fprintf(stderr, "Error encoding PDSCH\n");
-          exit(-1);
-        }        
-        if (net_port > 0 && net_packet_ready) {
-          if (null_file_sink) {
-            for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
-              if (pdsch_cfg.grant.tb_en[tb]) {
+          /* Configure pdsch_cfg parameters */
+          srslte_ra_dl_grant_t grant; 
+          srslte_ra_dl_dci_to_grant(&ra_dl, cell.nof_prb, UE_CRNTI, &grant);        
+          if (srslte_pdsch_cfg_mimo(&pdsch_cfg, cell, &grant, cfi, sf_idx, rvidx, pdsch_cfg.mimo_type, multiplex_pmi)) {
+            fprintf(stderr, "Error configuring PDSCH\n");
+            exit(-1);
+          }
+
+          /* Encode PDSCH */
+          if (srslte_pdsch_encode(&pdsch, &pdsch_cfg, softbuffers, data, UE_CRNTI, sf_symbols)) {
+            fprintf(stderr, "Error encoding PDSCH\n");
+            exit(-1);
+          }        
+          if (net_port > 0 && net_packet_ready) {
+            if (null_file_sink) {
+              for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
                 srslte_bit_pack_vector(data[tb], data_tmp, pdsch_cfg.grant.mcs[tb].tbs);
                 if (srslte_netsink_write(&net_sink, data_tmp, 1 + (pdsch_cfg.grant.mcs[tb].tbs - 1) / 8) < 0) {
                   fprintf(stderr, "Error sending data through UDP socket\n");
                 }
               }
             }
+            net_packet_ready = false; 
+            sem_post(&net_sem);
+          }
+        }else{ // We're sending MCH on subframe 1 - PDCCH + PMCH
+
+          /* Encode PDCCH */
+          INFO("Putting DCI to location: n=%d, L=%d\n", locations[sf_idx][0].ncce, locations[sf_idx][0].L);
+          srslte_dci_msg_pack_pdsch(&ra_dl, SRSLTE_DCI_FORMAT1, &dci_msg, cell.nof_prb, cell.nof_ports, false);
+          if (srslte_pdcch_encode(&pdcch, &dci_msg, locations[sf_idx][0], M_CRNTI, sf_symbols, sf_idx, cfi)) {
+              fprintf(stderr, "Error encoding DCI message\n");
+              exit(-1);
+          }
+          /* Configure pmch_cfg parameters */
+          srslte_ra_dl_grant_t grant;
+          grant.nof_tb = 1;
+          grant.mcs[0].idx = 2;
+          grant.mcs[0].mod = SRSLTE_MOD_QPSK;
+          grant.nof_prb = cell.nof_prb;
+          grant.sf_type = SRSLTE_SF_MBSFN;
+          grant.Qm[0] = srslte_mod_bits_x_symbol(grant.mcs[0].mod);
+          srslte_dl_fill_ra_mcs(&grant.mcs[0], cell.nof_prb);
+          for(int i = 0; i < 2; i++){
+            for(int j = 0; j < grant.nof_prb; j++){
+              grant.prb_idx[i][j] = true;
+            }
+          }
+          
+
+          if (srslte_pmch_cfg(&pmch_cfg, cell, &grant, cfi, sf_idx)) {
+            fprintf(stderr, "Error configuring PMCH\n");
+            exit(-1);
+          }
+          /* Encode PMCH */
+          if (srslte_pmch_encode(&pmch, &pmch_cfg, softbuffers[0], data[0], mbsfn_area_id, sf_symbols)) {
+            fprintf(stderr, "Error encoding PDSCH\n");
+            exit(-1);
+          }
+          if (net_port > 0 && net_packet_ready) {
+            if (null_file_sink) {
+              srslte_bit_pack_vector(data[0], data_tmp, pmch_cfg.grant.mcs[0].tbs);
+              if (srslte_netsink_write(&net_sink, data_tmp, 1+(pmch_cfg.grant.mcs[0].tbs-1)/8) < 0) {
+                fprintf(stderr, "Error sending data through UDP socket\n");
+              }
+            }
+            net_packet_ready = false;
+            sem_post(&net_sem);
           }
-          net_packet_ready = false; 
-          sem_post(&net_sem);
         }
       }
-      
+
       /* Transform to OFDM symbols */
-      for (i = 0; i < cell.nof_ports; i++) {
-        srslte_ofdm_tx_sf(&ifft, sf_buffer[i], output_buffer[i]);
+      if(sf_idx != 1 || mbsfn_area_id < 0){
+        for (i = 0; i < cell.nof_ports; i++) {
+          srslte_ofdm_tx_sf(&ifft, sf_buffer[i], output_buffer[i]);
+        }
+      }else{
+        srslte_ofdm_tx_sf(&ifft_mbsfn, sf_buffer[0], output_buffer[0]);
       }
-
+      
       /* send to file or usrp */
       if (output_file_name) {
         if (!null_file_sink) {
-          srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);
+           srslte_filesink_write_multi(&fsink, (void**) output_buffer, sf_n_samples, cell.nof_ports);       
         }
         usleep(1000);
       } else {
 #ifndef DISABLE_RF
-        float norm_factor = (float) cell.nof_prb/15/sqrtf(pdsch_cfg.grant.nof_prb);
-        for (i = 0; i < cell.nof_ports; i++) {
-          srslte_vec_sc_prod_cfc(output_buffer[i], rf_amp * norm_factor, output_buffer[i], SRSLTE_SF_LEN_PRB(cell.nof_prb));
-        }
-        srslte_rf_send_multi(&rf, (void**) output_buffer, sf_n_samples, true, start_of_burst, false);
-        start_of_burst=false;
+      float norm_factor = (float) cell.nof_prb/15/sqrtf(pdsch_cfg.grant.nof_prb);
+      for (i = 0; i < cell.nof_ports; i++) {
+        srslte_vec_sc_prod_cfc(output_buffer[i], rf_amp * norm_factor, output_buffer[i], SRSLTE_SF_LEN_PRB(cell.nof_prb));
+      }
+      srslte_rf_send_multi(&rf, (void**) output_buffer, sf_n_samples, true, start_of_burst, false);
+      start_of_burst=false;
 #endif
       }
     }
diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index a4279ba1a..38113dbcf 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -36,10 +36,8 @@
 #include <signal.h>
 #include <pthread.h>
 #include <semaphore.h>
-#include <srslte/srslte.h>
-#include <srslte/phy/phch/pdsch_cfg.h>
-#include <srslte/phy/phch/ra.h>
-
+#include <srslte/phy/common/phy_common.h>
+#include "srslte/phy/io/filesink.h"
 #include "srslte/srslte.h"
 
 #define ENABLE_AGC_DEFAULT
@@ -69,7 +67,7 @@ sem_t plot_sem;
 uint32_t plot_sf_idx=0;
 bool plot_track = true; 
 #endif
-
+char *output_file_name;
 #define PRINT_CHANGE_SCHEDULIGN
 
 //#define CORRECT_SAMPLE_OFFSET
@@ -101,6 +99,8 @@ typedef struct {
   int net_port_signal; 
   char *net_address_signal;
   int decimate;
+  int mbsfn_area_id;
+  uint8_t  non_mbsfn_region;
   int verbose;
 }prog_args_t;
 
@@ -132,10 +132,12 @@ void args_default(prog_args_t *args) {
   args->net_address_signal = "127.0.0.1";
   args->decimate = 0;
   args->cpu_affinity = -1;
+  args->mbsfn_area_id = -1;
+  args->non_mbsfn_region = 2;
 }
 
 void usage(prog_args_t *args, char *prog) {
-  printf("Usage: %s [agpPoOcildDnruv] -f rx_frequency (in Hz) | -i input_file\n", prog);
+  printf("Usage: %s [agpPoOcildDnruMNv] -f rx_frequency (in Hz) | -i input_file\n", prog);
 #ifndef DISABLE_RF
   printf("\t-a RF args [Default %s]\n", args->rf_args);
   printf("\t-A Number of RX antennas [Default %d]\n", args->rf_nof_rx_ant);
@@ -169,13 +171,15 @@ void usage(prog_args_t *args, char *prog) {
   printf("\t-S remote UDP address to send input signal [Default %s]\n", args->net_address_signal);
   printf("\t-u remote TCP port to send data (-1 does nothing with it) [Default %d]\n", args->net_port);
   printf("\t-U remote TCP address to send data [Default %s]\n", args->net_address);
+  printf("\t-M MBSFN area id [Default %s]\n", args->mbsfn_area_id);
+  printf("\t-N Non-MBSFN region [Default %s]\n", args->non_mbsfn_region);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 
 void parse_args(prog_args_t *args, int argc, char **argv) {
   int opt;
   args_default(args);
-  while ((opt = getopt(argc, argv, "aAoglipPcOCtdDnvrfuUsSZy")) != -1) {
+  while ((opt = getopt(argc, argv, "aAoglipPcOCtdDnvrfuUsSZyWMN")) != -1) {
     switch (opt) {
     case 'i':
       args->input_file_name = argv[optind];
@@ -250,6 +254,15 @@ void parse_args(prog_args_t *args, int argc, char **argv) {
     case 'y':
       args->cpu_affinity = atoi(argv[optind]);
       break;
+    case 'W':
+      output_file_name = argv[optind];
+      break;
+    case 'M':
+      args->mbsfn_area_id = atoi(argv[optind]);
+      break;
+    case 'N':
+      args->non_mbsfn_region = atoi(argv[optind]);
+      break;
     default:
       usage(args, argv[0]);
       exit(-1);
@@ -278,6 +291,7 @@ void sig_int_handler(int signo)
 
 cf_t *sf_buffer[SRSLTE_MAX_PORTS] = {NULL};
 
+
 #ifndef DISABLE_RF
 int srslte_rf_recv_wrapper(void *h, cf_t *data[SRSLTE_MAX_PORTS], uint32_t nsamples, srslte_timestamp_t *t) {
   DEBUG(" ----  Receive %d samples  ---- \n", nsamples);
@@ -303,9 +317,9 @@ srslte_ue_sync_t ue_sync;
 prog_args_t prog_args; 
 
 uint32_t sfn = 0; // system frame number
-srslte_netsink_t net_sink, net_sink_signal; 
-
+srslte_netsink_t net_sink, net_sink_signal;
 /* Useful macros for printing lines which will disappear */
+
 #define PRINT_LINE_INIT() int this_nof_lines = 0; static int prev_nof_lines = 0
 #define PRINT_LINE(_fmt, ...) printf("\033[K" _fmt "\n", ##__VA_ARGS__); this_nof_lines++
 #define PRINT_LINE_RESET_CURSOR() printf("\033[%dA", this_nof_lines); prev_nof_lines = this_nof_lines
@@ -317,6 +331,7 @@ int main(int argc, char **argv) {
   srslte_cell_t cell;  
   int64_t sf_cnt;
   srslte_ue_mib_t ue_mib; 
+
 #ifndef DISABLE_RF
   srslte_rf_t rf; 
 #endif
@@ -324,7 +339,7 @@ int main(int argc, char **argv) {
   uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN];
   int sfn_offset;
   float cfo = 0; 
-
+  
   parse_args(&prog_args, argc, argv);
   
   for (int i = 0; i< SRSLTE_MAX_CODEWORDS; i++) {
@@ -335,6 +350,7 @@ int main(int argc, char **argv) {
     }
   }
 
+  
   if(prog_args.cpu_affinity > -1) {
     
     cpu_set_t cpuset;
@@ -403,6 +419,7 @@ int main(int argc, char **argv) {
     srslte_rf_set_rx_freq(&rf, prog_args.rf_freq + prog_args.file_offset_freq);
     srslte_rf_rx_wait_lo_locked(&rf);
 
+
     uint32_t ntrial=0; 
     do {
       ret = rf_search_and_decode_mib(&rf, prog_args.rf_nof_rx_ant, &cell_detect_config, prog_args.force_N_id_2, &cell, &cfo);
@@ -455,7 +472,7 @@ 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_multi(&ue_sync, prog_args.file_nof_prb,
+    if (srslte_ue_sync_init_file_multi(&ue_sync, prog_args.file_nof_prb, 
       prog_args.input_file_name, prog_args.file_offset_time, prog_args.file_offset_freq, prog_args.rf_nof_rx_ant)) {
       fprintf(stderr, "Error initiating ue_sync\n");
       exit(-1); 
@@ -518,6 +535,11 @@ int main(int argc, char **argv) {
   /* Configure downlink receiver for the SI-RNTI since will be the only one we'll use */
   srslte_ue_dl_set_rnti(&ue_dl, prog_args.rnti); 
   
+  /* Configure MBSFN area id and non-MBSFN Region */
+  if (prog_args.mbsfn_area_id > -1) {
+    srslte_ue_dl_set_mbsfn_area_id(&ue_dl, prog_args.mbsfn_area_id);
+    srslte_ue_dl_set_non_mbsfn_region(&ue_dl, prog_args.non_mbsfn_region);
+  }
   /* Initialize subframe counter */
   sf_cnt = 0;
 
@@ -592,7 +614,6 @@ int main(int argc, char **argv) {
       }
     }
 
-
     ret = srslte_ue_sync_zerocopy_multi(&ue_sync, sf_buffer);
     if (ret < 0) {
       fprintf(stderr, "Error calling srslte_ue_sync_work()\n");
@@ -605,9 +626,12 @@ int main(int argc, char **argv) {
     
     /* srslte_ue_sync_get_buffer returns 1 if successfully read 1 aligned subframe */
     if (ret == 1) {
+
+      uint32_t sfidx = srslte_ue_sync_get_sfidx(&ue_sync);
+
       switch (state) {
         case DECODE_MIB:
-          if (srslte_ue_sync_get_sfidx(&ue_sync) == 0) {
+          if (sfidx == 0) {
             n = srslte_ue_mib_decode(&ue_mib, sf_buffer[0], bch_payload, NULL, &sfn_offset);
             if (n < 0) {
               fprintf(stderr, "Error decoding UE MIB\n");
@@ -626,51 +650,64 @@ int main(int argc, char **argv) {
             decode_pdsch = true;             
           } else {
             /* We are looking for SIB1 Blocks, search only in appropiate places */
-            if ((srslte_ue_sync_get_sfidx(&ue_sync) == 5 && (sfn%2)==0)) {
+            if ((sfidx == 5 && (sfn%2)==0) || sfidx == 1) {
               decode_pdsch = true; 
             } else {
               decode_pdsch = false; 
             }
           }
-
-          INFO("Attempting DL decode SFN=%d\n", sfn);
           if (decode_pdsch) {
-            if (cell.nof_ports == 1) {
-              /* Transmission mode 1 */
-              n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
-            } else {
-              if (prog_args.rf_nof_rx_ant == 1) {
-                /* Transmission mode 2 */
-                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 1, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
-                                        acks);
+            if(sfidx != 1 || prog_args.mbsfn_area_id < 0){ // Not an MBSFN subframe
+              if (cell.nof_ports == 1) {
+                /* Transmission mode 1 */
+                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 0, sfn*10+srslte_ue_sync_get_sfidx(&ue_sync), acks);
               } else {
-                /* Transmission mode 3 */
-                n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 2, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
-                                        acks);
-                if (n < 1) {
-                  /* Transmission mode 4 */
-                  n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 3, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                if (prog_args.rf_nof_rx_ant == 1) {
+                  /* Transmission mode 2 */
+                  n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 1, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                          acks);
+                } else {
+                  /* Transmission mode 3 */
+                  n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 2, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
                                           acks);
+                  if (n < 1) {
+                    /* Transmission mode 4 */
+                    n = srslte_ue_dl_decode(&ue_dl, sf_buffer, data, 3, sfn * 10 + srslte_ue_sync_get_sfidx(&ue_sync),
+                                            acks);
+                  }
+                }
+              }
+            }else{ // MBSFN subframe
+              n = srslte_ue_dl_decode_mbsfn(&ue_dl, 
+                                          sf_buffer,
+                                          data[0],
+                                          sfn*10+srslte_ue_sync_get_sfidx(&ue_sync));
+              if(n>0){
+                if(output_file_name){
+                  //srslte_filesink_init(&sink, output_file_name, SRSLTE_BYTE_BIN);
+                 // srslte_filesink_write(&sink, data, n);
+                  //srslte_filesink_free(&sink);
                 }
+                INFO("mbsfn PDU size is %d\n", n);
               }
             }
-
-
-          
             if (n < 0) {
              // fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
             } else if (n > 0) {
               
               /* Send data if socket active */
               if (prog_args.net_port > 0) {
+                if(sfidx == 1) {
+                  srslte_netsink_write(&net_sink, data[0], 1+(n-1)/8);
+                } else {
                 // FIXME: UDP Data transmission does not work
-                for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
-                  if (ue_dl.pdsch_cfg.grant.tb_en[tb]) {
-                    srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+                  for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+                    if (ue_dl.pdsch_cfg.grant.tb_en[tb]) {
+                      srslte_netsink_write(&net_sink, data[tb], 1 + (ue_dl.pdsch_cfg.grant.mcs[tb].tbs - 1) / 8);
+                    }
                   }
                 }
               }
-              
               #ifdef PRINT_CHANGE_SCHEDULIGN
               if (ue_dl.dl_dci.mcs_idx         != old_dl_dci.mcs_idx           || 
                   memcmp(&ue_dl.dl_dci.type0_alloc, &old_dl_dci.type0_alloc, sizeof(srslte_ra_type0_t)) ||
@@ -689,6 +726,7 @@ int main(int argc, char **argv) {
                                     
             nof_trials++; 
             
+ 
             rsrq = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrq(&ue_dl.chest), rsrq, 0.1f);
             rsrp0 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 0), rsrp0, 0.05f);
             rsrp1 = SRSLTE_VEC_EMA(srslte_chest_dl_get_rsrp_port(&ue_dl.chest, 1), rsrp1, 0.05f);
@@ -696,6 +734,7 @@ int main(int argc, char **argv) {
             enodebrate = SRSLTE_VEC_EMA((ue_dl.pdsch_cfg.grant.mcs[0].tbs + ue_dl.pdsch_cfg.grant.mcs[1].tbs)/1000.0f, enodebrate, 0.05f);
             uerate = SRSLTE_VEC_EMA(((acks[0]?ue_dl.pdsch_cfg.grant.mcs[0].tbs:0) + (acks[1]?ue_dl.pdsch_cfg.grant.mcs[1].tbs:0))/1000.0f, uerate, 0.01f);
 
+   
             nframes++;
             if (isnan(rsrq)) {
               rsrq = 0; 
@@ -704,20 +743,20 @@ int main(int argc, char **argv) {
               noise = 0; 
             }
             if (isnan(rsrp0)) {
-              rsrp1 = 0;
+              rsrp0 = 0; 
             }
-            if (isnan(rsrp0)) {
-              rsrp1 = 0;
+            if (isnan(rsrp1)) {
+              rsrp1 = 0; 
             }
           }
 
           // Plot and Printf
-          if (srslte_ue_sync_get_sfidx(&ue_sync) == 5 && sfn % 20 == 0) {
+          if (sfidx == 5) {
             float gain = prog_args.rf_gain; 
             if (gain < 0) {
               gain = 10*log10(srslte_agc_get_gain(&ue_sync.agc)); 
             }
-
+           
             /* Print transmission scheme */
             if (ue_dl.pdsch_cfg.mimo_type == SRSLTE_MIMO_TYPE_SPATIAL_MULTIPLEX) {
               PRINT_LINE("    Tx scheme: %s (codebook_idx=%d)", srslte_mimotype2str(ue_dl.pdsch_cfg.mimo_type),
@@ -733,7 +772,10 @@ int main(int argc, char **argv) {
             PRINT_LINE("          SNR: %+5.1f dB | %+5.1f dB", 10 * log10(rsrp0 / noise), 10 * log10(rsrp1 / noise));
             PRINT_LINE("           Rb: %6.2f / %6.2f Mbps (net/maximum)", uerate, enodebrate);
             PRINT_LINE("   PDCCH-Miss: %5.2f%%", 100 * (1 - (float) ue_dl.nof_detected / nof_trials));
-            PRINT_LINE("   PDSCH-BLER: %5.2f%%", (float) 100 * ue_dl.pkt_errors / ue_dl.pkts_total);
+            PRINT_LINE("   PDSCH-BLER: %5.2f%%", (float) 100 * ue_dl.pdsch_pkt_errors / ue_dl.pdsch_pkts_total);
+            if(prog_args.mbsfn_area_id > -1){
+              PRINT_LINE("   PMCH-BLER: %5.2f%%", (float) 100 * ue_dl.pmch_pkt_errors/ue_dl.pmch_pkts_total);
+            }
             PRINT_LINE("         TB 0: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[0].idx,
                        ue_dl.pdsch_cfg.grant.mcs[0].tbs);
             PRINT_LINE("         TB 1: mcs=%d; tbs=%d", ue_dl.pdsch_cfg.grant.mcs[1].idx,
@@ -775,17 +817,20 @@ int main(int argc, char **argv) {
             PRINT_LINE("Press enter maximum printing debug log of 1 subframe.");
             PRINT_LINE("");
             PRINT_LINE_RESET_CURSOR();
+
           }
           break;
       }
-      if (srslte_ue_sync_get_sfidx(&ue_sync) == 9) {
+      if (sfidx == 9) {
         sfn++; 
         if (sfn == 1024) {
           sfn = 0; 
           PRINT_LINE_ADVANCE_CURSOR();
+          ue_dl.pdsch_pkt_errors = 0;
+          ue_dl.pdsch_pkts_total = 0;
           /*
           ue_dl.pkt_errors = 0; 
-          ue_dl.pkts_total = 0; 
+          ue_dl.pkts_total = 0;
           ue_dl.nof_detected = 0;           
           nof_trials = 0;
           */
@@ -794,7 +839,7 @@ int main(int argc, char **argv) {
       
       #ifndef DISABLE_GRAPHICS
       if (!prog_args.disable_plots) {
-        if ((sfn%4) == 0 && decode_pdsch) {
+        if ((sfn%3) == 0 && decode_pdsch) {
           plot_sf_idx = srslte_ue_sync_get_sfidx(&ue_sync);
           plot_track = true;
           sem_post(&plot_sem);
@@ -816,8 +861,7 @@ int main(int argc, char **argv) {
         
     sf_cnt++;                  
   } // Main loop
-  printf("\033[30B\n");
-
+  
 #ifndef DISABLE_GRAPHICS
   if (!prog_args.disable_plots) {
     if (!pthread_kill(plot_thread, 0)) {
@@ -838,13 +882,14 @@ int main(int argc, char **argv) {
       free(sf_buffer[i]);
     }
   }
-  
+
 #ifndef DISABLE_RF
   if (!prog_args.input_file_name) {
     srslte_ue_mib_free(&ue_mib);
     srslte_rf_close(&rf);    
   }
 #endif
+  
   printf("\nBye\n");
   exit(0);
 }
diff --git a/lib/include/srslte/phy/ch_estimation/chest_dl.h b/lib/include/srslte/phy/ch_estimation/chest_dl.h
index c3fd795cf..0e43d113c 100644
--- a/lib/include/srslte/phy/ch_estimation/chest_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/chest_dl.h
@@ -60,7 +60,10 @@ typedef enum {
 
 typedef struct {
   srslte_cell_t cell; 
-  srslte_refsignal_cs_t csr_signal;
+  srslte_refsignal_t   csr_refs;
+  srslte_refsignal_t **mbsfn_refs;
+ 
+
   cf_t *pilot_estimates;
   cf_t *pilot_estimates_average; 
   cf_t *pilot_recv_signal; 
@@ -75,7 +78,7 @@ typedef struct {
 
   srslte_interp_linsrslte_vec_t srslte_interp_linvec; 
   srslte_interp_lin_t srslte_interp_lin; 
-  
+  srslte_interp_lin_t srslte_interp_lin_mbsfn;
   float rssi[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float rsrp[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS]; 
   float noise_estimate[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
@@ -96,9 +99,13 @@ SRSLTE_API int srslte_chest_dl_init(srslte_chest_dl_t *q,
 
 SRSLTE_API void srslte_chest_dl_free(srslte_chest_dl_t *q);
 
+
+SRSLTE_API int srslte_chest_dl_set_mbsfn_area_id(srslte_chest_dl_t *q,
+                                                 uint16_t mbsfn_area_id);
 SRSLTE_API int srslte_chest_dl_set_cell(srslte_chest_dl_t *q,
                                         srslte_cell_t cell);
 
+
 SRSLTE_API void srslte_chest_dl_set_smooth_filter(srslte_chest_dl_t *q, 
                                                   float *filter, 
                                                   uint32_t filter_len); 
@@ -109,6 +116,8 @@ SRSLTE_API void srslte_chest_dl_set_smooth_filter3_coeff(srslte_chest_dl_t* q,
 SRSLTE_API void srslte_chest_dl_set_noise_alg(srslte_chest_dl_t *q, 
                                               srslte_chest_dl_noise_alg_t noise_estimation_alg); 
 
+
+
 SRSLTE_API int srslte_chest_dl_estimate_multi(srslte_chest_dl_t *q, 
                                               cf_t *input[SRSLTE_MAX_PORTS],
                                               cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
@@ -120,6 +129,14 @@ SRSLTE_API int srslte_chest_dl_estimate(srslte_chest_dl_t *q,
                                         cf_t *ce[SRSLTE_MAX_PORTS],
                                         uint32_t sf_idx);
 
+SRSLTE_API int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q,
+                                                    cf_t *input[SRSLTE_MAX_PORTS],
+                                                    cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                                    uint32_t sf_idx,
+                                                    uint32_t nof_rx_antennas,
+                                                    uint16_t mbsfn_area_id);
+
+
 SRSLTE_API int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, 
                                              cf_t *input,
                                              cf_t *ce,
diff --git a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
index 6b21c4fa2..9263073a9 100644
--- a/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
+++ b/lib/include/srslte/phy/ch_estimation/refsignal_dl.h
@@ -40,25 +40,34 @@
 
 // Number of references in a subframe: there are 2 symbols for port_id=0,1 x 2 slots x 2 refs per prb
 #define SRSLTE_REFSIGNAL_NUM_SF(nof_prb, port_id)     (((port_id)<2?8:4)*(nof_prb))
+#define SRSLTE_REFSIGNAL_NUM_SF_MBSFN(nof_prb, port_id)     ((2 + 18)*(nof_prb))
+
 #define SRSLTE_REFSIGNAL_MAX_NUM_SF(nof_prb)          SRSLTE_REFSIGNAL_NUM_SF(nof_prb, 0)
+#define SRSLTE_REFSIGNAL_MAX_NUM_SF_MBSFN(nof_prb)    SRSLTE_REFSIGNAL_NUM_SF_MBSFN(nof_prb,0)
 
 #define SRSLTE_REFSIGNAL_PILOT_IDX(i,l,cell) (2*cell.nof_prb*(l)+(i))
 
+#define SRSLTE_REFSIGNAL_PILOT_IDX_MBSFN(i,l,cell) ((6*cell.nof_prb*(l)+(i)))
+
+
 
 /** Cell-Specific Reference Signal */
 typedef struct SRSLTE_API {
   srslte_cell_t cell; 
-  cf_t *pilots[2][SRSLTE_NSUBFRAMES_X_FRAME]; // Saves the reference signal per subframe for ports 0,1 and ports 2,3
-} srslte_refsignal_cs_t;
+  cf_t         *pilots[2][SRSLTE_NSUBFRAMES_X_FRAME]; // Saves the reference signal per subframe for ports 0,1 and ports 2,3
+  srslte_sf_t   type;
+  uint16_t      mbsfn_area_id;
+} srslte_refsignal_t;
 
 
-SRSLTE_API int srslte_refsignal_cs_init(srslte_refsignal_cs_t *q, 
+
+SRSLTE_API int srslte_refsignal_cs_init(srslte_refsignal_t *q, 
                                         uint32_t max_prb);
 
-SRSLTE_API int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q,
+SRSLTE_API int srslte_refsignal_cs_set_cell(srslte_refsignal_t * q,
                                            srslte_cell_t cell);
 
-SRSLTE_API void srslte_refsignal_cs_free(srslte_refsignal_cs_t *q);
+SRSLTE_API void srslte_refsignal_free(srslte_refsignal_t *q);
 
 SRSLTE_API int srslte_refsignal_cs_put_sf(srslte_cell_t cell, 
                                           uint32_t port_id, 
@@ -84,4 +93,29 @@ SRSLTE_API uint32_t srslte_refsignal_cs_v(uint32_t port_id,
 
 SRSLTE_API uint32_t srslte_refsignal_cs_nof_symbols(uint32_t port_id);
 
+SRSLTE_API int srslte_refsignal_mbsfn_init(srslte_refsignal_t *q, srslte_cell_t cell,
+                                           uint16_t mbsfn_area_id);
+
+SRSLTE_API int srslte_refsignal_mbsfn_get_sf(srslte_cell_t cell,
+                                             uint32_t port_id,
+                                             cf_t *sf_symbols,
+                                             cf_t *pilots);
+
+SRSLTE_API uint32_t srslte_refsignal_mbsfn_nsymbol(uint32_t l);
+
+SRSLTE_API uint32_t srslte_refsignal_mbsfn_fidx(uint32_t l);
+
+SRSLTE_API uint32_t srslte_refsignal_mbsfn_nof_symbols();
+
+SRSLTE_API int srslte_refsignal_mbsfn_put_sf(srslte_cell_t cell,
+                                             uint32_t port_id,
+                                             cf_t *cs_pilots,
+                                             cf_t *mbsfn_pilots,
+                                             cf_t *sf_symbols);
+
+SRSLTE_API  int srslte_refsignal_mbsfn_gen_seq(srslte_refsignal_t * q,
+                                               srslte_cell_t cell,
+                                               uint32_t N_mbsfn_id);
+
+
 #endif
diff --git a/lib/include/srslte/phy/common/phy_common.h b/lib/include/srslte/phy/common/phy_common.h
index 29bf8d3a4..148a12974 100644
--- a/lib/include/srslte/phy/common/phy_common.h
+++ b/lib/include/srslte/phy/common/phy_common.h
@@ -63,7 +63,11 @@
 #define SRSLTE_LTE_CRC16   0x11021
 #define SRSLTE_LTE_CRC8    0x19B
 
+#define SRSLTE_MAX_MBSFN_AREA_IDS 256
+#define SRSLTE_PMCH_RV            0
+
 typedef enum {SRSLTE_CP_NORM, SRSLTE_CP_EXT} srslte_cp_t;
+typedef enum {SRSLTE_SF_NORM, SRSLTE_SF_MBSFN} srslte_sf_t;
 
 
 #define SRSLTE_CRNTI_START  0x000B
@@ -130,6 +134,13 @@ typedef enum {SRSLTE_CP_NORM, SRSLTE_CP_EXT} srslte_cp_t;
         || l == SRSLTE_CP_NSYMB(cp) - 3)
 
 
+
+#define SRSLTE_SYMBOL_HAS_REF_MBSFN(l, s) ((l == 2 && s == 0) || (l == 0 && s == 1) || (l == 4 && s == 1))
+
+#define SRSLTE_NON_MBSFN_REGION_GUARD_LENGTH(non_mbsfn_region,symbol_sz) ((non_mbsfn_region == 1)?(SRSLTE_CP_LEN_EXT(symbol_sz) - SRSLTE_CP_LEN_NORM(0, symbol_sz)):(2*SRSLTE_CP_LEN_EXT(symbol_sz) - SRSLTE_CP_LEN_NORM(0, symbol_sz)- SRSLTE_CP_LEN_NORM(1, symbol_sz)))
+
+
+
 #define SRSLTE_NOF_LTE_BANDS 38
 
 #define SRSLTE_DEFAULT_MAX_FRAMES_PBCH      500
@@ -157,6 +168,7 @@ typedef enum {
   SRSLTE_RNTI_TEMP,     /* Temporary C-RNTI */
   SRSLTE_RNTI_SPS,      /* Semi-Persistent Scheduling C-RNTI */
   SRSLTE_RNTI_PCH,      /* Paging RNTI */
+  SRSLTE_RNTI_MBSFN,
   SRSLTE_RNTI_NOF_TYPES
 } srslte_rnti_type_t;
 
diff --git a/lib/include/srslte/phy/common/sequence.h b/lib/include/srslte/phy/common/sequence.h
index 4c7c2c945..62934c7e0 100644
--- a/lib/include/srslte/phy/common/sequence.h
+++ b/lib/include/srslte/phy/common/sequence.h
@@ -94,4 +94,10 @@ SRSLTE_API int srslte_sequence_pucch(srslte_sequence_t *seq,
                                      uint16_t rnti, 
                                      uint32_t nslot, 
                                      uint32_t cell_id); 
+
+SRSLTE_API int srslte_sequence_pmch(srslte_sequence_t *seq,
+                                    uint32_t nslot,
+                                    uint32_t mbsfn_id,
+                                    uint32_t len);
+
 #endif
diff --git a/lib/include/srslte/phy/dft/ofdm.h b/lib/include/srslte/phy/dft/ofdm.h
index 175fe1ea9..1363f5638 100644
--- a/lib/include/srslte/phy/dft/ofdm.h
+++ b/lib/include/srslte/phy/dft/ofdm.h
@@ -56,6 +56,12 @@ typedef struct SRSLTE_API{
   srslte_cp_t cp;
   cf_t *tmp; // for removing zero padding
   
+  bool     mbsfn_subframe;
+  uint32_t mbsfn_guard_len;
+  uint32_t nof_symbols_mbsfn;
+  uint8_t  non_mbsfn_region;
+
+  
   bool freq_shift;
   float freq_shift_f;
   cf_t *shift_buffer; 
@@ -64,8 +70,21 @@ typedef struct SRSLTE_API{
 SRSLTE_API int srslte_ofdm_init_(srslte_ofdm_t *q, 
                                  srslte_cp_t cp, 
                                  int symbol_sz, 
-                                 int max_prb,
-                                 srslte_dft_dir_t dir); 
+                                 int nof_prb, 
+                                 srslte_dft_dir_t dir);
+
+SRSLTE_API int srslte_ofdm_init_mbsfn_(srslte_ofdm_t *q, 
+                                 srslte_cp_t cp, 
+                                 int symbol_sz, 
+                                 int nof_prb, 
+                                 srslte_dft_dir_t dir,
+                                 srslte_sf_t sf_type);
+
+SRSLTE_API int srslte_ofdm_rx_init_mbsfn(srslte_ofdm_t *q,
+                                         srslte_cp_t cp_type,
+                                         uint32_t nof_prb);
+
+
 
 SRSLTE_API int srslte_ofdm_rx_init(srslte_ofdm_t *q, 
                                srslte_cp_t cp_type, 
@@ -95,12 +114,22 @@ SRSLTE_API int srslte_ofdm_tx_init(srslte_ofdm_t *q,
                                     srslte_cp_t cp_type, 
                                     uint32_t nof_prb);
 
+SRSLTE_API int srslte_ofdm_tx_init_mbsfn(srslte_ofdm_t *q,
+                                        srslte_cp_t cp, 
+                                        uint32_t nof_prb);
+
+
 SRSLTE_API void srslte_ofdm_tx_free(srslte_ofdm_t *q);
 
 SRSLTE_API void srslte_ofdm_tx_slot(srslte_ofdm_t *q, 
                                   cf_t *input, 
                                   cf_t *output);
 
+SRSLTE_API void srslte_ofdm_tx_slot_mbsfn(srslte_ofdm_t *q,
+                                         cf_t *input,
+                                         cf_t *output);
+
+
 SRSLTE_API void srslte_ofdm_tx_sf(srslte_ofdm_t *q, 
                                 cf_t *input, 
                                 cf_t *output);
@@ -111,4 +140,8 @@ SRSLTE_API int srslte_ofdm_set_freq_shift(srslte_ofdm_t *q,
 SRSLTE_API void srslte_ofdm_set_normalize(srslte_ofdm_t *q, 
                                          bool normalize_enable); 
 
-#endif
+SRSLTE_API void srslte_ofdm_set_non_mbsfn_region(srslte_ofdm_t *q,
+                                               uint8_t non_mbsfn_region);
+
+
+#endif
\ No newline at end of file
diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index f465d81e2..3f835a23f 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -76,7 +76,7 @@ typedef struct SRSLTE_API {
   srslte_pdsch_t  pdsch;
   srslte_phich_t  phich; 
   
-  srslte_refsignal_cs_t csr_signal;
+  srslte_refsignal_t csr_signal;
   srslte_pdsch_cfg_t pdsch_cfg; 
   srslte_ra_dl_dci_t dl_dci;
   
diff --git a/lib/include/srslte/phy/phch/pmch.h b/lib/include/srslte/phy/phch/pmch.h
new file mode 100644
index 000000000..dfff956f1
--- /dev/null
+++ b/lib/include/srslte/phy/phch/pmch.h
@@ -0,0 +1,152 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+/******************************************************************************
+ *  File:         pmch.h
+ *
+ *  Description:  Physical multicast channel
+ *
+ *  Reference:    3GPP TS 36.211 version 10.0.0 Release 10 Sec. 6.5
+ *****************************************************************************/
+
+#ifndef PMCH_
+#define PMCH_
+
+#include "srslte/config.h"
+#include "srslte/phy/common/phy_common.h"
+#include "srslte/phy/mimo/precoding.h"
+#include "srslte/phy/mimo/layermap.h"
+#include "srslte/phy/modem/mod.h"
+#include "srslte/phy/modem/demod_soft.h"
+#include "srslte/phy/scrambling/scrambling.h"
+#include "srslte/phy/phch/dci.h"
+#include "srslte/phy/phch/regs.h"
+#include "srslte/phy/phch/sch.h"
+#include "srslte/phy/common/sequence.h"
+
+typedef struct {
+  srslte_sequence_t seq[SRSLTE_NSUBFRAMES_X_FRAME];  
+} srslte_pmch_seq_t;
+
+typedef struct SRSLTE_API {
+  srslte_cbsegm_t cb_segm;
+  srslte_ra_dl_grant_t grant;
+  srslte_ra_nbits_t nbits[SRSLTE_MAX_CODEWORDS];
+  uint32_t sf_idx;
+} srslte_pmch_cfg_t;
+
+/* PMCH object */
+typedef struct SRSLTE_API {
+  srslte_cell_t cell;
+  
+  uint32_t nof_rx_antennas;
+  
+  uint32_t max_re;
+  
+  /* buffers */
+  // void buffers are shared for tx and rx
+  cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  cf_t *symbols[SRSLTE_MAX_PORTS];
+  cf_t *x[SRSLTE_MAX_PORTS];
+  cf_t *d;
+  void *e;
+
+  /* tx & rx objects */
+  srslte_modem_table_t mod[4];
+  
+  // This is to generate the scrambling seq for multiple MBSFN Area IDs
+  srslte_pmch_seq_t **seqs;
+  
+  srslte_sch_t dl_sch;
+  
+} srslte_pmch_t;
+
+
+SRSLTE_API int srslte_pmch_init(srslte_pmch_t *q,
+                                 uint32_t max_prb);
+
+SRSLTE_API int srslte_pmch_init_multi(srslte_pmch_t *q,
+                                       uint32_t max_prb, 
+                                       uint32_t nof_rx_antennas);
+
+SRSLTE_API void srslte_pmch_free(srslte_pmch_t *q);
+
+
+
+SRSLTE_API int srslte_pmch_set_area_id(srslte_pmch_t *q, uint16_t area_id);
+
+SRSLTE_API void srslte_pmch_free_area_id(srslte_pmch_t *q, uint16_t area_id);
+
+
+
+SRSLTE_API int srslte_pmch_get(srslte_pmch_t *q, cf_t *sf_symbols, cf_t *symbols, uint32_t lstart);
+
+SRSLTE_API int srslte_pmch_put(srslte_pmch_t *q, cf_t *symbols, cf_t *sf_symbols, uint32_t lstart);
+
+SRSLTE_API int srslte_pmch_cp(srslte_pmch_t *q, cf_t *input, cf_t *output, uint32_t lstart_grant, bool put);
+
+
+
+SRSLTE_API float srslte_pmch_coderate(uint32_t tbs,
+                                       uint32_t nof_re); 
+
+
+SRSLTE_API int srslte_pmch_cfg(srslte_pdsch_cfg_t *cfg,
+                               srslte_cell_t cell,
+                               srslte_ra_dl_grant_t *grant,
+                               uint32_t cfi,
+                               uint32_t sf_idx);
+
+SRSLTE_API int srslte_pmch_encode(srslte_pmch_t *q,
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_tx_t *softbuffer,
+                                   uint8_t *data, 
+                                   uint16_t area_id,
+                                   cf_t *sf_symbols[SRSLTE_MAX_PORTS]);
+
+SRSLTE_API int srslte_pmch_decode(srslte_pmch_t *q,
+                                   srslte_pdsch_cfg_t *cfg,
+                                   srslte_softbuffer_rx_t *softbuffer,
+                                   cf_t *sf_symbols, 
+                                   cf_t *ce[SRSLTE_MAX_PORTS],
+                                   float noise_estimate, 
+                                   uint16_t area_id,
+                                   uint8_t *data);
+
+SRSLTE_API int srslte_pmch_decode_multi(srslte_pmch_t *q,
+                                         srslte_pdsch_cfg_t *cfg,
+                                         srslte_softbuffer_rx_t *softbuffer,
+                                         cf_t *sf_symbols[SRSLTE_MAX_PORTS], 
+                                         cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS],
+                                         float noise_estimate, 
+                                         uint16_t area_id,
+                                         uint8_t *data);
+
+SRSLTE_API float srslte_pmch_average_noi(srslte_pmch_t *q);
+
+SRSLTE_API uint32_t srslte_pmch_last_noi(srslte_pmch_t *q);
+
+#endif
diff --git a/lib/include/srslte/phy/phch/ra.h b/lib/include/srslte/phy/phch/ra.h
index 94cc9a084..3039455ed 100644
--- a/lib/include/srslte/phy/phch/ra.h
+++ b/lib/include/srslte/phy/phch/ra.h
@@ -103,7 +103,11 @@ typedef struct SRSLTE_API {
   bool prb_idx[2][SRSLTE_MAX_PRB];
   uint32_t nof_prb;  
   uint32_t Qm[SRSLTE_MAX_CODEWORDS];
+  uint32_t Qm2[SRSLTE_MAX_CODEWORDS];
   srslte_ra_mcs_t mcs[SRSLTE_MAX_CODEWORDS];
+  srslte_ra_mcs_t mcs2[SRSLTE_MAX_CODEWORDS];
+  uint32_t nof_tb;
+  srslte_sf_t sf_type;
   bool tb_en[SRSLTE_MAX_CODEWORDS];
   uint32_t pinfo;
 } srslte_ra_dl_grant_t;
@@ -290,4 +294,9 @@ SRSLTE_API void srslte_ra_pusch_fprint(FILE *f,
 SRSLTE_API void srslte_ra_ul_grant_fprint(FILE *f, 
                                           srslte_ra_ul_grant_t *grant);
 
+SRSLTE_API int srslte_dl_fill_ra_mcs_pmch(srslte_ra_mcs_t *mcs, uint32_t nprb);
+
+SRSLTE_API int srslte_dl_fill_ra_mcs(srslte_ra_mcs_t *mcs, uint32_t nprb);
+
+
 #endif /* RB_ALLOC_H_ */
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index a03387448..a98ecf280 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -77,6 +77,7 @@ typedef struct SRSLTE_API {
   srslte_uci_cqi_pusch_t uci_cqi;
   
 } srslte_sch_t;
+#include "srslte/phy/phch/pmch.h"
 
 SRSLTE_API int srslte_sch_init(srslte_sch_t *q);
 
diff --git a/lib/include/srslte/phy/ue/ue_dl.h b/lib/include/srslte/phy/ue/ue_dl.h
index d88148e9e..0b0fc44d2 100644
--- a/lib/include/srslte/phy/ue/ue_dl.h
+++ b/lib/include/srslte/phy/ue/ue_dl.h
@@ -48,6 +48,7 @@
 #include "srslte/phy/phch/pcfich.h"
 #include "srslte/phy/phch/pdcch.h"
 #include "srslte/phy/phch/pdsch.h"
+#include "srslte/phy/phch/pmch.h"
 #include "srslte/phy/phch/pdsch_cfg.h"
 #include "srslte/phy/phch/phich.h"
 #include "srslte/phy/phch/ra.h"
@@ -76,14 +77,17 @@ typedef struct SRSLTE_API {
   srslte_pcfich_t pcfich;
   srslte_pdcch_t pdcch;
   srslte_pdsch_t pdsch;
+  srslte_pmch_t  pmch;
   srslte_phich_t phich; 
   srslte_regs_t regs;
   srslte_ofdm_t fft;
+  srslte_ofdm_t fft_mbsfn;
   srslte_chest_dl_t chest;
   
   srslte_cfo_t sfo_correct; 
   
-  srslte_pdsch_cfg_t pdsch_cfg; 
+  srslte_pdsch_cfg_t pdsch_cfg;
+  srslte_pdsch_cfg_t pmch_cfg;
   srslte_softbuffer_rx_t *softbuffers[SRSLTE_MAX_CODEWORDS];
   srslte_ra_dl_dci_t dl_dci;
   srslte_cell_t cell;
@@ -103,9 +107,14 @@ typedef struct SRSLTE_API {
   srslte_dci_format_t dci_format;
   uint64_t pkt_errors; 
   uint64_t pkts_total;
+  uint64_t pdsch_pkt_errors;
+  uint64_t pdsch_pkts_total;
+  uint64_t pmch_pkt_errors;
+  uint64_t pmch_pkts_total;
   uint64_t nof_detected; 
 
   uint16_t current_rnti;
+  uint16_t current_mbsfn_area_id;
   dci_blind_search_t current_ss_ue[3][10];
   dci_blind_search_t current_ss_common[3];
   srslte_dci_location_t last_location;
@@ -127,14 +136,26 @@ SRSLTE_API void srslte_ue_dl_free(srslte_ue_dl_t *q);
 SRSLTE_API int srslte_ue_dl_set_cell(srslte_ue_dl_t *q,
                                           srslte_cell_t cell);
 
-SRSLTE_API int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q,
+int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, 
+                                     cf_t *input[SRSLTE_MAX_PORTS], 
+                                     uint32_t sf_idx, 
+                                     uint32_t *cfi);
+
+SRSLTE_API int srslte_ue_dl_decode_fft_estimate_mbsfn(srslte_ue_dl_t *q,
                                                 cf_t *input[SRSLTE_MAX_PORTS],
                                                 uint32_t sf_idx, 
-                                                uint32_t *cfi); 
+                                                uint32_t *cfi,
+                                                srslte_sf_t sf_type); 
+
+
+int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, 
+                                 uint32_t sf_idx, 
+                                 uint32_t *cfi);
 
-SRSLTE_API int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, 
+SRSLTE_API int srslte_ue_dl_decode_estimate_mbsfn(srslte_ue_dl_t *q, 
                                             uint32_t sf_idx, 
-                                            uint32_t *cfi); 
+                                            uint32_t *cfi,
+                                            srslte_sf_t sf_type);  
 
 SRSLTE_API int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q,
                                       srslte_ra_dl_grant_t *grant,
@@ -184,6 +205,18 @@ SRSLTE_API int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q,
                                         uint16_t rnti,
                                         bool acks[SRSLTE_MAX_CODEWORDS]);
 
+/* Used by example applications - full PMCH decode for a given MBSFN area ID
+ * srslte_ue_dl_decode_fft_estimate_multi,
+ * srslte_chest_dl_get_noise_estimate,
+ * srslte_ue_dl_cfg_grant,
+ * srslte_pmch_decode_multi
+ */
+SRSLTE_API int srslte_ue_dl_decode_mbsfn(srslte_ue_dl_t * q,
+                                         cf_t *input[SRSLTE_MAX_PORTS],
+                                         uint8_t *data,
+                                         uint32_t tti);
+
+
 SRSLTE_API int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q,
                                           uint8_t *ri,
                                           uint8_t *pmi,
@@ -203,6 +236,15 @@ SRSLTE_API void srslte_ue_dl_reset(srslte_ue_dl_t *q);
 SRSLTE_API void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, 
                                       uint16_t rnti);
 
+/* Generate signals if required, store in q->current_mbsfn_area_id */
+SRSLTE_API int srslte_ue_dl_set_mbsfn_area_id(srslte_ue_dl_t *q,
+                                              uint16_t mbsfn_area_id);
+
+SRSLTE_API void srslte_ue_dl_set_non_mbsfn_region(srslte_ue_dl_t *q,
+                                                  uint8_t non_mbsfn_region_length);
+
+
+
 SRSLTE_API void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, 
                                          srslte_softbuffer_rx_t *softbuffer, 
                                          uint32_t tti, 
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index edb8c5ca2..b5107f9ab 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -81,29 +81,46 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
   {
     bzero(q, sizeof(srslte_chest_dl_t));
     
-    ret = srslte_refsignal_cs_init(&q->csr_signal, max_prb);
+
+    ret = srslte_refsignal_cs_init(&q->csr_refs, max_prb);
     if (ret != SRSLTE_SUCCESS) {
       fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
       goto clean_exit;
     }
     
-    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
+    q->mbsfn_refs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_refsignal_t*));
+    if (!q->mbsfn_refs) {
+      fprintf(stderr, "Calloc error initializing mbsfn_refs (%d)\n", ret);
+      goto clean_exit;
+    }
+    
+    int pilot_vec_size;
+    if(SRSLTE_REFSIGNAL_MAX_NUM_SF_MBSFN(max_prb)>SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb)) {
+      pilot_vec_size = SRSLTE_REFSIGNAL_MAX_NUM_SF_MBSFN(max_prb);
+    }else{
+      pilot_vec_size = SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb);
+    }
+
+    q->tmp_noise = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+
+
     if (!q->tmp_noise) {
       perror("malloc");
       goto clean_exit;
     }
+    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
 
-    q->pilot_estimates = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
     if (!q->pilot_estimates) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
+    q->pilot_estimates_average = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
     if (!q->pilot_estimates_average) {
       perror("malloc");
       goto clean_exit;
     }      
-    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_MAX_NUM_SF(max_prb));
+    q->pilot_recv_signal = srslte_vec_malloc(sizeof(cf_t) * pilot_vec_size);
+
     if (!q->pilot_recv_signal) {
       perror("malloc");
       goto clean_exit;
@@ -118,8 +135,13 @@ int srslte_chest_dl_init(srslte_chest_dl_t *q, uint32_t max_prb)
       fprintf(stderr, "Error initializing interpolator\n");
       goto clean_exit; 
     }
+
+    if (srslte_interp_linear_init(&q->srslte_interp_lin_mbsfn, 6*max_prb, SRSLTE_NRE/6)) {
+      fprintf(stderr, "Error initializing interpolator\n");
+      goto clean_exit;
+    }
     
-    q->noise_alg = SRSLTE_NOISE_ALG_REFS;
+    q->noise_alg = SRSLTE_NOISE_ALG_REFS; 
     
     q->smooth_filter_len = 3; 
     srslte_chest_dl_set_smooth_filter3_coeff(q, 0.1);
@@ -137,14 +159,25 @@ clean_exit:
 
 void srslte_chest_dl_free(srslte_chest_dl_t *q) 
 {
-  srslte_refsignal_cs_free(&q->csr_signal);
+  int i;
+  if(&q->csr_refs)
+    srslte_refsignal_free(&q->csr_refs);
+
+  if (q->mbsfn_refs) {
+    for (i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
+      if (q->mbsfn_refs[i]) {
+        srslte_refsignal_free(q->mbsfn_refs[i]);
+      }
+    }
+    free(q->mbsfn_refs);
+  }
 
   if (q->tmp_noise) {
     free(q->tmp_noise);
   }
   srslte_interp_linear_vector_free(&q->srslte_interp_linvec);
   srslte_interp_linear_free(&q->srslte_interp_lin);
-  
+  srslte_interp_linear_free(&q->srslte_interp_lin_mbsfn);
   if (q->pilot_estimates) {
     free(q->pilot_estimates);
   }      
@@ -157,6 +190,19 @@ void srslte_chest_dl_free(srslte_chest_dl_t *q)
   bzero(q, sizeof(srslte_chest_dl_t));
 }
 
+
+int srslte_chest_dl_set_mbsfn_area_id(srslte_chest_dl_t *q, uint16_t mbsfn_area_id){
+  if(!q->mbsfn_refs[mbsfn_area_id]){
+    q->mbsfn_refs[mbsfn_area_id] = calloc(1, sizeof(srslte_refsignal_t));
+  }
+  if(q->mbsfn_refs[mbsfn_area_id]) {
+    if(srslte_refsignal_mbsfn_init(q->mbsfn_refs[mbsfn_area_id], q->cell, mbsfn_area_id)) {
+      return SRSLTE_ERROR;
+    }
+  }  
+  return SRSLTE_SUCCESS;
+}
+
 int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
 {
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -165,7 +211,7 @@ int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
   {
     if (q->cell.id != cell.id || q->cell.nof_prb == 0) {
       memcpy(&q->cell, &cell, sizeof(srslte_cell_t));
-      ret = srslte_refsignal_cs_set_cell(&q->csr_signal, cell);
+      ret = srslte_refsignal_cs_set_cell(&q->csr_refs, cell);
       if (ret != SRSLTE_SUCCESS) {
         fprintf(stderr, "Error initializing CSR signal (%d)\n",ret);
         return SRSLTE_ERROR;
@@ -187,7 +233,6 @@ int srslte_chest_dl_set_cell(srslte_chest_dl_t *q, srslte_cell_t cell)
     }
     ret = SRSLTE_SUCCESS;
   }
-
   return ret;
 }
 
@@ -253,46 +298,70 @@ static float estimate_noise_empty_sc(srslte_chest_dl_t *q, cf_t *input) {
 
 #define cesymb(i) ce[SRSLTE_RE_IDX(q->cell.nof_prb,i,0)]
 
-static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t *ce, uint32_t port_id) 
+static void interpolate_pilots(srslte_chest_dl_t *q, cf_t *pilot_estimates, cf_t *ce, uint32_t port_id, srslte_sf_t ch_mode) 
 {
   /* interpolate the symbols with references in the freq domain */
   uint32_t l; 
-  uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(port_id); 
-  
+  uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN ) ? srslte_refsignal_mbsfn_nof_symbols() + 1 : srslte_refsignal_cs_nof_symbols(port_id);
+  uint32_t fidx_offset = 0;
   /* Interpolate in the frequency domain */
-  for (l=0;l<nsymbols;l++) {
-    uint32_t fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);    
-    srslte_interp_linear_offset(&q->srslte_interp_lin, &pilot_estimates[2*q->cell.nof_prb*l],
-                         &ce[srslte_refsignal_cs_nsymbol(l,q->cell.cp, port_id) * q->cell.nof_prb * SRSLTE_NRE], 
-                         fidx_offset, SRSLTE_NRE/2-fidx_offset); 
+ 
+   // we add one to nsymbols to allow for inclusion of the non-mbms references in the channel estimation       
+  for (l=0;l<(nsymbols);l++) {
+    if (ch_mode == SRSLTE_SF_MBSFN) {
+      if (l == 0) {
+        fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
+        srslte_interp_linear_offset(&q->srslte_interp_lin, &pilot_estimates[2*q->cell.nof_prb*l],
+                                    &ce[srslte_refsignal_cs_nsymbol(l,q->cell.cp, port_id) * q->cell.nof_prb * SRSLTE_NRE], 
+                                    fidx_offset, SRSLTE_NRE/2-fidx_offset); 
+      } else {
+        fidx_offset = srslte_refsignal_mbsfn_fidx(l - 1);
+        srslte_interp_linear_offset(&q->srslte_interp_lin_mbsfn, &pilot_estimates[(2*q->cell.nof_prb) + 6*q->cell.nof_prb*(l - 1)],
+                                    &ce[srslte_refsignal_mbsfn_nsymbol(l - 1) * q->cell.nof_prb * SRSLTE_NRE],
+                                    fidx_offset, SRSLTE_NRE/6-fidx_offset);
+      }
+    } else {
+      fidx_offset = srslte_refsignal_cs_fidx(q->cell, l, port_id, 0);
+      srslte_interp_linear_offset(&q->srslte_interp_lin, &pilot_estimates[2*q->cell.nof_prb*l],
+                                  &ce[srslte_refsignal_cs_nsymbol(l,q->cell.cp, port_id) * q->cell.nof_prb * SRSLTE_NRE], 
+                                  fidx_offset, SRSLTE_NRE/2-fidx_offset); 
+    }  
   }
-  
+ 
   /* Now interpolate in the time domain between symbols */
-  if (SRSLTE_CP_ISNORM(q->cell.cp)) {
-    if (nsymbols == 4) {
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
-      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
+  if (ch_mode == SRSLTE_SF_MBSFN) {
+    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(2), &cesymb(1), 2, 1);
+    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(2), &cesymb(6), &cesymb(3), 4, 3);
+    srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(7), 4, 3);
+    srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(10), &cesymb(10), &cesymb(11), 4, 1);
+   } else {
+    if (SRSLTE_CP_ISNORM(q->cell.cp)) {
+      if (nsymbols == 4) {
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(4),  &cesymb(1), 4, 3);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(4), &cesymb(7),  &cesymb(5), 3, 2);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(8), 4, 3);
+        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(11), &cesymb(11), &cesymb(12), 4, 2);
+      } else {
+        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
+      }    
     } else {
-      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(8), &cesymb(1), &cesymb(1), &cesymb(0), 7, 1);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(2), 7, 6);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(8), &cesymb(9), 7, 5);
-    }    
-  } else {
-    if (nsymbols == 4) {
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
-      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
-    } else {
-      srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
-      srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
-    }    
+      if (nsymbols == 4) {
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(0), &cesymb(3), &cesymb(1), 3, 2);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(3), &cesymb(6), &cesymb(4), 3, 2);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(7), 3, 2);
+        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(6), &cesymb(9), &cesymb(9), &cesymb(10), 3, 2);
+      } else {
+        srslte_interp_linear_vector2(&q->srslte_interp_linvec, &cesymb(7), &cesymb(1), &cesymb(1), &cesymb(0), 6, 1);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(2), 6, 5);
+        srslte_interp_linear_vector(&q->srslte_interp_linvec, &cesymb(1), &cesymb(7), &cesymb(8), 6, 4);
+      }    
+    }
   }
 }
 
+
 void srslte_chest_dl_set_smooth_filter(srslte_chest_dl_t *q, float *filter, uint32_t filter_len) {
   if (filter_len < SRSLTE_CHEST_MAX_SMOOTH_FIL_LEN) {
     if (filter) {
@@ -319,9 +388,9 @@ void srslte_chest_dl_set_smooth_filter3_coeff(srslte_chest_dl_t* q, float w)
   q->smooth_filter[1] = 1-2*w; 
 }
 
-static void average_pilots(srslte_chest_dl_t *q, cf_t *input, cf_t *output, uint32_t port_id) {
-  uint32_t nsymbols = srslte_refsignal_cs_nof_symbols(port_id); 
-  uint32_t nref = 2*q->cell.nof_prb;
+static void average_pilots(srslte_chest_dl_t *q, cf_t *input, cf_t *output, uint32_t port_id, srslte_sf_t ch_mode)  {
+  uint32_t nsymbols = (ch_mode == SRSLTE_SF_MBSFN)?srslte_refsignal_mbsfn_nof_symbols(port_id):srslte_refsignal_cs_nof_symbols(port_id);
+  uint32_t nref = (ch_mode == SRSLTE_SF_MBSFN)?6*q->cell.nof_prb:2*q->cell.nof_prb;
 
   // Average in the frequency domain
   for (int l=0;l<nsymbols;l++) {
@@ -341,25 +410,16 @@ float srslte_chest_dl_rssi(srslte_chest_dl_t *q, cf_t *input, uint32_t port_id)
   return rssi/nsymbols; 
 }
 
-int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id) 
-{
-  /* Get references from the input signal */
-  srslte_refsignal_cs_get_sf(q->cell, port_id, input, q->pilot_recv_signal);
-  
-  /* Use the known CSR signal to compute Least-squares estimates */
-  srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_signal.pilots[port_id/2][sf_idx], 
-              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id));
-
-  if (ce != NULL) {
-    
+void chest_interpolate_noise_est(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, srslte_sf_t ch_mode){
+ if (ce != NULL) {
     /* Smooth estimates (if applicable) and interpolate */
     if (q->smooth_filter_len == 0 || (q->smooth_filter_len == 3 && q->smooth_filter[0] == 0)) {
-      interpolate_pilots(q, q->pilot_estimates, ce, port_id);            
+      interpolate_pilots(q, q->pilot_estimates, ce, port_id, ch_mode);
     } else {
-      average_pilots(q, q->pilot_estimates, q->pilot_estimates_average, port_id);
-      interpolate_pilots(q, q->pilot_estimates_average, ce, port_id);              
+      average_pilots(q, q->pilot_estimates, q->pilot_estimates_average, port_id, ch_mode);
+      interpolate_pilots(q, q->pilot_estimates_average, ce, port_id, ch_mode);
     }
-    
+  
     /* Estimate noise power */
     if (q->noise_alg == SRSLTE_NOISE_ALG_REFS && q->smooth_filter_len > 0) {
       q->noise_estimate[rxant_id][port_id] = estimate_noise_pilots(q, port_id);                  
@@ -371,8 +431,7 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
       if (sf_idx == 0 || sf_idx == 5) {
         q->noise_estimate[rxant_id][port_id] = estimate_noise_empty_sc(q, input);        
       }
-    }
-    
+    } 
   }
     
   /* Compute RSRP for the channel estimates in this port */
@@ -381,10 +440,42 @@ int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, u
     /* compute rssi only for port 0 */
     q->rssi[rxant_id][port_id] = srslte_chest_dl_rssi(q, input, port_id);     
   }
+}
+
+int srslte_chest_dl_estimate_port(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id) 
+{
+  /* Get references from the input signal */
+  srslte_refsignal_cs_get_sf(q->cell, port_id, input, q->pilot_recv_signal);
+  
+  /* Use the known CSR signal to compute Least-squares estimates */
+  srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_refs.pilots[port_id/2][sf_idx], 
+              q->pilot_estimates, SRSLTE_REFSIGNAL_NUM_SF(q->cell.nof_prb, port_id)); 
+
+  chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_NORM);
+  
+  return 0;
+}
+int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t *ce, uint32_t sf_idx, uint32_t port_id, uint32_t rxant_id, uint16_t mbsfn_area_id)
+{
+
+  /* Use the known CSR signal to compute Least-squares estimates */
+  srslte_refsignal_mbsfn_get_sf(q->cell, port_id, input, q->pilot_recv_signal);
+  // estimate for non-mbsfn section of subframe
+  srslte_vec_prod_conj_ccc(q->pilot_recv_signal, q->csr_refs.pilots[port_id/2][sf_idx],
+                           q->pilot_estimates, (2*q->cell.nof_prb));
+  
+  srslte_vec_prod_conj_ccc(q->pilot_recv_signal+(2*q->cell.nof_prb), q->mbsfn_refs[mbsfn_area_id]->pilots[port_id/2][sf_idx],
+                           q->pilot_estimates+(2*q->cell.nof_prb), SRSLTE_REFSIGNAL_NUM_SF_MBSFN(q->cell.nof_prb, port_id)-(2*q->cell.nof_prb));
+  
+  chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_MBSFN);
       
   return 0;
 }
 
+
+
+
+
 int srslte_chest_dl_estimate_multi(srslte_chest_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t nof_rx_antennas) 
 {
   for (uint32_t rxant_id=0;rxant_id<nof_rx_antennas;rxant_id++) {
@@ -411,6 +502,21 @@ int srslte_chest_dl_estimate(srslte_chest_dl_t *q, cf_t *input, cf_t *ce[SRSLTE_
   return SRSLTE_SUCCESS;
 }
 
+int srslte_chest_dl_estimate_multi_mbsfn(srslte_chest_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t nof_rx_antennas, uint16_t mbsfn_area_id)
+{
+  for (uint32_t rxant_id=0;rxant_id<nof_rx_antennas;rxant_id++) {
+    for (uint32_t port_id=0;port_id<q->cell.nof_ports;port_id++) {
+      if (srslte_chest_dl_estimate_port_mbsfn(q, input[rxant_id], ce[port_id][rxant_id], sf_idx, port_id, rxant_id, mbsfn_area_id)) {
+        return SRSLTE_ERROR;
+      }
+    }
+  }
+  q->last_nof_antennas = nof_rx_antennas;
+  return SRSLTE_SUCCESS;
+}
+
+
+
 float srslte_chest_dl_get_noise_estimate(srslte_chest_dl_t *q) {
   float n = 0; 
   for (int i=0;i<q->last_nof_antennas;i++) {
diff --git a/lib/src/phy/ch_estimation/refsignal_dl.c b/lib/src/phy/ch_estimation/refsignal_dl.c
index fa8a27968..4de00039e 100644
--- a/lib/src/phy/ch_estimation/refsignal_dl.c
+++ b/lib/src/phy/ch_estimation/refsignal_dl.c
@@ -75,17 +75,44 @@ uint32_t srslte_refsignal_cs_v(uint32_t port_id, uint32_t ref_symbol_idx)
 
 uint32_t srslte_refsignal_cs_nof_symbols(uint32_t port_id) 
 {
+  uint32_t ret;
   if (port_id < 2) {
-    return 4; 
+    ret = 4; 
   } else {
-    return 2;
+    ret = 2;
   }
+  return ret;
 }
 
+uint32_t srslte_refsignal_mbsfn_nof_symbols()
+{
+  if(false){
+   return 3;
+  }else{
+   return 3;
+  }
+}
+
+
 inline uint32_t srslte_refsignal_cs_fidx(srslte_cell_t cell, uint32_t l, uint32_t port_id, uint32_t m) {
   return 6*m + ((srslte_refsignal_cs_v(port_id, l) + (cell.id % 6)) % 6);
 }
 
+inline uint32_t srslte_refsignal_mbsfn_fidx(uint32_t l)
+{
+  
+  uint32_t ret = 0;
+  if(l == 0){
+    ret = 0;
+  }else if (l == 1){
+    ret = 1;
+  }else if(l == 2){
+    ret = 0;
+  }
+  
+  return ret;
+}
+
 inline uint32_t srslte_refsignal_cs_nsymbol(uint32_t l, srslte_cp_t cp, uint32_t port_id) {
   if (port_id < 2) {
     if (l % 2) {
@@ -97,11 +124,104 @@ inline uint32_t srslte_refsignal_cs_nsymbol(uint32_t l, srslte_cp_t cp, uint32_t
     return 1+l*SRSLTE_CP_NSYMB(cp);
   }
 }
+inline uint32_t srslte_refsignal_mbsfn_nsymbol(uint32_t l)
+{
+  uint32_t ret = 0;
+    if(l == 0){
+      ret = 2;
+    } else if (l == 1) {
+      ret = 6;
+    } else if (l == 2){
+      ret = 10;
+    }
+  
+    return ret;
+}
+
+int srslte_refsignal_mbsfn_gen_seq(srslte_refsignal_t * q, srslte_cell_t cell, uint32_t N_mbsfn_id)
+{
+  uint32_t c_init;
+  uint32_t i, ns, l, p;
+  uint32_t mp;
+  int ret = SRSLTE_ERROR;
+
+  srslte_sequence_t seq_mbsfn;
+  bzero(&seq_mbsfn, sizeof(srslte_sequence_t));
+  if (srslte_sequence_init(&seq_mbsfn, 20* SRSLTE_MAX_PRB)) {
+    goto free_and_exit;
+  }
+
+  for(ns=0; ns<SRSLTE_NSUBFRAMES_X_FRAME;ns++){
+    for(p=0;p<2;p++) {
+      uint32_t nsymbols = 3; // replace with function
+      for(l=0;l<nsymbols;l++) {
+        uint32_t lp = (srslte_refsignal_mbsfn_nsymbol(l))%6;
+        uint32_t slot =(l)?(ns*2+1):(ns*2);
+        c_init = 512*(7*(slot+1)+lp+1)*(2*N_mbsfn_id + 1) + N_mbsfn_id;
+        srslte_sequence_set_LTE_pr(&seq_mbsfn,SRSLTE_MAX_PRB*20 ,c_init);
+        for(i=0;i< 6*q->cell.nof_prb;i++) {
+          mp = i + 3*(SRSLTE_MAX_PRB - cell.nof_prb);
+          q->pilots[p][ns][ SRSLTE_REFSIGNAL_PILOT_IDX_MBSFN(i, l ,q->cell)] = (1 - 2 * (float) seq_mbsfn.c[2 * mp]) / sqrt(2) +_Complex_I * (1 - 2 * (float) seq_mbsfn.c[2 * mp + 1]) / sqrt(2);
+        }
+      }
+    }
+  }
+   
+  srslte_sequence_free(&seq_mbsfn);
+  ret = SRSLTE_SUCCESS;
+    
+free_and_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_sequence_free(&seq_mbsfn);
+    srslte_refsignal_free(q);
+  }
+  return ret;
+  
+}
+
+
+int srslte_refsignal_mbsfn_init(srslte_refsignal_t * q, srslte_cell_t cell, uint16_t mbsfn_area_id)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  uint32_t i, p;
+  if (q != NULL &&
+      srslte_cell_isvalid(&cell))
+  {
+    ret = SRSLTE_ERROR;
+    bzero(q, sizeof(srslte_refsignal_t));
+    q->cell = cell;
+    q->type = SRSLTE_SF_MBSFN;
+    q->mbsfn_area_id = mbsfn_area_id;
+
+    for (p=0;p<2;p++) {
+      for (i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
+        q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * q->cell.nof_prb * 18);
+        if (!q->pilots[p][i]) {
+          perror("malloc");
+          goto free_and_exit;
+        }
+      }
+    }
+
+    if(srslte_refsignal_mbsfn_gen_seq(q, q->cell, q->mbsfn_area_id)) {
+      goto free_and_exit;
+    }
+    ret = SRSLTE_SUCCESS;
+  }
+
+free_and_exit:
+  if (ret == SRSLTE_ERROR) {
+    srslte_refsignal_free(q);
+  }
+  return ret;
+}
+
+
 
 
 /** Allocates memory for the 20 slots in a subframe
  */
-int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, uint32_t max_prb)
+int srslte_refsignal_cs_init(srslte_refsignal_t * q, uint32_t max_prb)
 {
 
   int ret = SRSLTE_ERROR_INVALID_INPUTS;
@@ -109,7 +229,6 @@ int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, uint32_t max_prb)
   if (q != NULL)
   {
     ret = SRSLTE_ERROR; 
-
     for (int p=0;p<2;p++) {
       for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
         q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(max_prb, 2*p));
@@ -123,7 +242,7 @@ int srslte_refsignal_cs_init(srslte_refsignal_cs_t * q, uint32_t max_prb)
   }
 free_and_exit:
   if (ret == SRSLTE_ERROR) {
-    srslte_refsignal_cs_free(q);
+    srslte_refsignal_free(q);
   }
   return ret;
 }
@@ -131,7 +250,7 @@ free_and_exit:
 /** Allocates and precomputes the Cell-Specific Reference (CSR) signal for
  * the 20 slots in a subframe
  */
-int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q, srslte_cell_t cell)
+int srslte_refsignal_cs_set_cell(srslte_refsignal_t * q, srslte_cell_t cell)
 {
 
   uint32_t c_init;
@@ -188,7 +307,7 @@ int srslte_refsignal_cs_set_cell(srslte_refsignal_cs_t * q, srslte_cell_t cell)
 }
 
 /** Deallocates a srslte_refsignal_cs_t object allocated with srslte_refsignal_cs_init */
-void srslte_refsignal_cs_free(srslte_refsignal_cs_t * q)
+void srslte_refsignal_free(srslte_refsignal_t * q)
 {
   for (int p=0;p<2;p++) {
     for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
@@ -197,10 +316,12 @@ void srslte_refsignal_cs_free(srslte_refsignal_cs_t * q)
       }
     }      
   }
-  bzero(q, sizeof(srslte_refsignal_cs_t));
+  bzero(q, sizeof(srslte_refsignal_t));
 }
 
 
+
+
 /* Maps a reference signal initialized with srslte_refsignal_cs_init() into an array of subframe symbols */
 int srslte_refsignal_cs_put_sf(srslte_cell_t cell, uint32_t port_id, cf_t *pilots, cf_t *sf_symbols)
 {
@@ -228,6 +349,45 @@ int srslte_refsignal_cs_put_sf(srslte_cell_t cell, uint32_t port_id, cf_t *pilot
   }
 }
 
+SRSLTE_API int srslte_refsignal_mbsfn_put_sf(srslte_cell_t cell,
+                                             uint32_t port_id,
+                                             cf_t *cs_pilots,
+                                             cf_t *mbsfn_pilots,
+                                             cf_t *sf_symbols)
+{
+  uint32_t i, l;
+  uint32_t fidx;
+  
+  if (srslte_cell_isvalid(&cell)             &&
+      srslte_portid_isvalid(port_id)         &&
+      cs_pilots                      != NULL &&
+      mbsfn_pilots                   != NULL &&
+      sf_symbols                     != NULL) 
+  {
+    // adding CS refs for the non-mbsfn section of the sub-frame
+    fidx = ((srslte_refsignal_cs_v(port_id, 0) + (cell.id % 6)) % 6); 
+    for (i = 0; i < 2*cell.nof_prb; i++) {
+      sf_symbols[SRSLTE_RE_IDX(cell.nof_prb, 0, fidx)] = cs_pilots[SRSLTE_REFSIGNAL_PILOT_IDX(i,0,cell)];
+      fidx += SRSLTE_NRE/2;       // 1 reference every 6 RE        
+    }     
+    
+    for (l = 0; l<srslte_refsignal_mbsfn_nof_symbols(); l++) {
+       uint32_t nsymbol = srslte_refsignal_mbsfn_nsymbol(l);
+       fidx = srslte_refsignal_mbsfn_fidx(l);
+       for (i = 0; i < 6*cell.nof_prb; i++) {
+        sf_symbols[SRSLTE_RE_IDX(cell.nof_prb, nsymbol, fidx)] = mbsfn_pilots[SRSLTE_REFSIGNAL_PILOT_IDX_MBSFN(i,l,cell)];
+        fidx += SRSLTE_NRE/6;
+       }
+    }
+    
+      return SRSLTE_SUCCESS;      
+  } else {
+      return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+  
+}
+
+
 /** Copies the RE containing references from an array of subframe symbols to the pilots array. */
 int srslte_refsignal_cs_get_sf(srslte_cell_t cell, uint32_t port_id, cf_t *sf_symbols, cf_t *pilots)
 {
@@ -254,4 +414,39 @@ int srslte_refsignal_cs_get_sf(srslte_cell_t cell, uint32_t port_id, cf_t *sf_sy
   }  
 }
 
+int srslte_refsignal_mbsfn_get_sf(srslte_cell_t cell, uint32_t port_id, cf_t *sf_symbols, cf_t *pilots)
+{
+  uint32_t i, l;
+  uint32_t fidx;
+  uint32_t nsymbol;
+  if (srslte_cell_isvalid(&cell)             &&
+      srslte_portid_isvalid(port_id)         &&
+      pilots                  != NULL && 
+      sf_symbols                  != NULL) 
+  {       
+   // getting refs from non mbsfn section of subframe
+    nsymbol = srslte_refsignal_cs_nsymbol(0, cell.cp, port_id);
+    fidx = ((srslte_refsignal_cs_v(port_id, 0) + (cell.id % 6)) % 6); 
+    for (i = 0; i < 2*cell.nof_prb; i++) {
+      pilots[SRSLTE_REFSIGNAL_PILOT_IDX(i,0,cell)]   = sf_symbols[SRSLTE_RE_IDX(cell.nof_prb, nsymbol, fidx)];
+      fidx += SRSLTE_NRE/2;       // 2 references per PRB
+    }
+    
+    for (l = 0; l< srslte_refsignal_mbsfn_nof_symbols() ;l++){
+      nsymbol = srslte_refsignal_mbsfn_nsymbol(l);
+      fidx = srslte_refsignal_mbsfn_fidx(l);
+      for (i = 0; i < 6*cell.nof_prb; i++) {
+        pilots[SRSLTE_REFSIGNAL_PILOT_IDX_MBSFN(i,l,cell) + (2*cell.nof_prb)] = sf_symbols[SRSLTE_RE_IDX(cell.nof_prb, nsymbol, fidx)];
+        fidx += SRSLTE_NRE/6;
+      }  
+    }
+    
+    return SRSLTE_SUCCESS;      
+  } else {
+      return SRSLTE_ERROR_INVALID_INPUTS;
+  }  
+}
+
+
+
 
diff --git a/lib/src/phy/ch_estimation/test/chest_test_dl.c b/lib/src/phy/ch_estimation/test/chest_test_dl.c
index f91600872..0f5a6d6ea 100644
--- a/lib/src/phy/ch_estimation/test/chest_test_dl.c
+++ b/lib/src/phy/ch_estimation/test/chest_test_dl.c
@@ -152,7 +152,7 @@ int main(int argc, char **argv) {
         bzero(h, sizeof(cf_t) * num_re);
 
         srslte_refsignal_cs_put_sf(cell, n_port, 
-                            est.csr_signal.pilots[n_port/2][sf_idx], input);
+                            est.csr_refs.pilots[n_port/2][sf_idx], input);
 
         for (i=0;i<2*SRSLTE_CP_NSYMB(cell.cp);i++) {
           for (j=0;j<cell.nof_prb * SRSLTE_NRE;j++) {
diff --git a/lib/src/phy/dft/ofdm.c b/lib/src/phy/dft/ofdm.c
index 825aff1e0..db5939274 100644
--- a/lib/src/phy/dft/ofdm.c
+++ b/lib/src/phy/dft/ofdm.c
@@ -37,7 +37,13 @@
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
 
+
 int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_prb, srslte_dft_dir_t dir) {
+  return srslte_ofdm_init_mbsfn_(q, cp, symbol_sz, nof_prb, dir, SRSLTE_SF_NORM);
+}
+
+
+int srslte_ofdm_init_mbsfn_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_prb, srslte_dft_dir_t dir, srslte_sf_t sf_type) {
 
   if (srslte_dft_plan_c(&q->fft_plan, symbol_sz, dir)) {
     fprintf(stderr, "Error: Creating DFT plan\n");
@@ -60,6 +66,7 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
 
   q->symbol_sz = (uint32_t) symbol_sz;
   q->nof_symbols = SRSLTE_CP_NSYMB(cp);
+  q->nof_symbols_mbsfn = SRSLTE_CP_NSYMB(SRSLTE_CP_EXT);
   q->cp = cp;
   q->freq_shift = false;
   q->nof_re = nof_prb * SRSLTE_NRE;
@@ -69,10 +76,20 @@ int srslte_ofdm_init_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_p
   DEBUG("Init %s symbol_sz=%d, nof_symbols=%d, cp=%s, nof_re=%d, nof_guards=%d\n",
       dir==SRSLTE_DFT_FORWARD?"FFT":"iFFT", q->symbol_sz, q->nof_symbols,
           q->cp==SRSLTE_CP_NORM?"Normal":"Extended", q->nof_re, q->nof_guards);
-
+  
+  // MBSFN logic
+  if (sf_type == SRSLTE_SF_MBSFN) {
+    q->mbsfn_subframe = true;
+    q->non_mbsfn_region = 2; // default set to 2
+  }
+  
   return SRSLTE_SUCCESS;
 }
 
+void srslte_ofdm_set_non_mbsfn_region(srslte_ofdm_t *q, uint8_t non_mbsfn_region)
+{
+  q->non_mbsfn_region = non_mbsfn_region;
+}
 
 int srslte_ofdm_replan_(srslte_ofdm_t *q, srslte_cp_t cp, int symbol_sz, int nof_prb) {
 
@@ -120,6 +137,17 @@ int srslte_ofdm_rx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
   return srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_FORWARD);
 }
 
+int srslte_ofdm_rx_init_mbsfn(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb)
+{
+  int symbol_sz = srslte_symbol_sz(nof_prb);
+  if (symbol_sz < 0) {
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    return -1;
+  }
+  return srslte_ofdm_init_mbsfn_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_FORWARD, SRSLTE_SF_MBSFN);
+}
+
+
 int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
   uint32_t i;
   int ret;
@@ -130,8 +158,33 @@ int srslte_ofdm_tx_init(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t max_prb) {
     return -1;
   }
   q->max_prb = max_prb;
+  ret = srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_BACKWARD); 
 
-  ret = srslte_ofdm_init_(q, cp, symbol_sz, max_prb, SRSLTE_DFT_BACKWARD);
+  
+  if (ret == SRSLTE_SUCCESS) {
+    srslte_dft_plan_set_norm(&q->fft_plan, false);
+    
+    /* set now zeros at CP */
+    for (i=0;i<q->nof_symbols;i++) {
+      bzero(q->tmp, q->nof_guards * sizeof(cf_t));
+      bzero(&q->tmp[q->nof_re + q->nof_guards], q->nof_guards * sizeof(cf_t));
+    }
+  }
+  return ret;
+}
+
+int srslte_ofdm_tx_init_mbsfn(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb)
+{
+  uint32_t i;
+  int ret;
+  
+  int symbol_sz = srslte_symbol_sz(nof_prb);
+  if (symbol_sz < 0) { 
+    fprintf(stderr, "Error: Invalid nof_prb=%d\n", nof_prb);
+    return -1;
+  }
+
+  ret = srslte_ofdm_init_mbsfn_(q, cp, symbol_sz, nof_prb, SRSLTE_DFT_BACKWARD, SRSLTE_SF_MBSFN);
   
   if (ret == SRSLTE_SUCCESS) {
     srslte_dft_plan_set_norm(&q->fft_plan, false);
@@ -190,7 +243,6 @@ int srslte_ofdm_tx_set_prb(srslte_ofdm_t *q, srslte_cp_t cp, uint32_t nof_prb) {
 void srslte_ofdm_rx_free(srslte_ofdm_t *q) {
   srslte_ofdm_free_(q);
 }
-
 /* Shifts the signal after the iFFT or before the FFT. 
  * Freq_shift is relative to inter-carrier spacing.
  * Caution: This function shall not be called during run-time 
@@ -233,6 +285,23 @@ void srslte_ofdm_rx_slot(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
   }
 }
 
+void srslte_ofdm_rx_slot_mbsfn(srslte_ofdm_t *q, cf_t *input, cf_t *output)
+{
+  uint32_t i;
+  for(i = 0; i < q->nof_symbols_mbsfn; i++){
+    if(i == q->non_mbsfn_region) {
+      input += SRSLTE_NON_MBSFN_REGION_GUARD_LENGTH(q->non_mbsfn_region,q->symbol_sz);
+    }
+    input += (i>=q->non_mbsfn_region)?SRSLTE_CP_LEN_EXT(q->symbol_sz):SRSLTE_CP_LEN_NORM(i, q->symbol_sz);
+    srslte_dft_run_c(&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->nof_re;
+  }
+}
+
+
+
 void srslte_ofdm_rx_slot_zerocopy(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
   uint32_t i;
   for (i=0;i<q->nof_symbols;i++) {
@@ -250,8 +319,14 @@ void srslte_ofdm_rx_sf(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
   if (q->freq_shift) {
     srslte_vec_prod_ccc(input, q->shift_buffer, input, 2*q->slot_sz);
   }
-  for (n=0;n<2;n++) {
-    srslte_ofdm_rx_slot(q, &input[n*q->slot_sz], &output[n*q->nof_re*q->nof_symbols]);
+  if(!q->mbsfn_subframe){
+    for (n=0;n<2;n++) {
+      srslte_ofdm_rx_slot(q, &input[n*q->slot_sz], &output[n*q->nof_re*q->nof_symbols]);
+    }
+  }
+  else{
+    srslte_ofdm_rx_slot_mbsfn(q, &input[0*q->slot_sz], &output[0*q->nof_re*q->nof_symbols]);
+    srslte_ofdm_rx_slot(q, &input[1*q->slot_sz], &output[1*q->nof_re*q->nof_symbols]);
   }
 }
 
@@ -271,16 +346,43 @@ void srslte_ofdm_tx_slot(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
   }
 }
 
+void srslte_ofdm_tx_slot_mbsfn(srslte_ofdm_t *q, cf_t *input, cf_t *output)
+{
+  uint32_t i, cp_len;
+  
+  for(i=0;i<q->nof_symbols_mbsfn;i++) {
+    cp_len = ( i>(q->non_mbsfn_region-1) )?SRSLTE_CP_LEN_EXT(q->symbol_sz):SRSLTE_CP_LEN_NORM(i, q->symbol_sz);
+    memcpy(&q->tmp[q->nof_guards], input, q->nof_re * sizeof(cf_t));
+    srslte_dft_run_c(&q->fft_plan, q->tmp, &output[cp_len]);
+    input += q->nof_re;
+    /* add CP */
+    memcpy(output, &output[q->symbol_sz], cp_len * sizeof(cf_t));
+    output += q->symbol_sz + cp_len;
+    
+    /*skip the small section between the non mbms region and the mbms region*/
+    if(i == (q->non_mbsfn_region - 1))
+      output += SRSLTE_NON_MBSFN_REGION_GUARD_LENGTH(q->non_mbsfn_region,q->symbol_sz);
+  }  
+}
+
 void srslte_ofdm_set_normalize(srslte_ofdm_t *q, bool normalize_enable) {
   srslte_dft_plan_set_norm(&q->fft_plan, normalize_enable);
 }
 
-void srslte_ofdm_tx_sf(srslte_ofdm_t *q, cf_t *input, cf_t *output) {
+void srslte_ofdm_tx_sf(srslte_ofdm_t *q, cf_t *input, cf_t *output)
+{
   uint32_t n; 
-  for (n=0;n<2;n++) {
-    srslte_ofdm_tx_slot(q, &input[n*q->nof_re*q->nof_symbols], &output[n*q->slot_sz]);
+  if(!q->mbsfn_subframe){
+    for (n=0;n<2;n++) {
+      srslte_ofdm_tx_slot(q, &input[n*q->nof_re*q->nof_symbols], &output[n*q->slot_sz]);
+    }
+  }
+  else{
+     srslte_ofdm_tx_slot_mbsfn(q, &input[0*q->nof_re*q->nof_symbols], &output[0*q->slot_sz]);
+     srslte_ofdm_tx_slot(q, &input[1*q->nof_re*q->nof_symbols], &output[1*q->slot_sz]);
   }
   if (q->freq_shift) {
     srslte_vec_prod_ccc(output, q->shift_buffer, output, 2*q->slot_sz);
   }
 }
+
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index fb9fe7d87..ec470ce3d 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -122,7 +122,7 @@ void srslte_enb_dl_free(srslte_enb_dl_t *q)
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
     
-    srslte_refsignal_cs_free(&q->csr_signal);
+    srslte_refsignal_free(&q->csr_signal);
     
     for (int i=0;i<SRSLTE_MAX_PORTS;i++) {
       if (q->sf_symbols[i]) {
diff --git a/lib/src/phy/phch/pmch.c b/lib/src/phy/phch/pmch.c
new file mode 100644
index 000000000..ee1b3554f
--- /dev/null
+++ b/lib/src/phy/phch/pmch.c
@@ -0,0 +1,486 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <strings.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <math.h>
+
+#include "prb_dl.h"
+#include "srslte/phy/phch/sch.h"
+#include "srslte/phy/common/phy_common.h"
+#include "srslte/phy/utils/bit.h"
+#include "srslte/phy/utils/debug.h"
+#include "srslte/phy/utils/vector.h"
+
+
+#define MAX_PMCH_RE (2 * SRSLTE_CP_EXT_NSYMB * 12)
+
+
+const static srslte_mod_t modulations[4] =
+    { SRSLTE_MOD_BPSK, SRSLTE_MOD_QPSK, SRSLTE_MOD_16QAM, SRSLTE_MOD_64QAM };
+    
+//#define DEBUG_IDX
+
+#ifdef DEBUG_IDX    
+cf_t *offset_original=NULL;
+extern int indices[100000];
+extern int indices_ptr; 
+#endif
+
+float srslte_pmch_coderate(uint32_t tbs, uint32_t nof_re)
+{
+  return (float) (tbs + 24)/(nof_re);
+}
+
+int srslte_pmch_cp(srslte_pmch_t *q, cf_t *input, cf_t *output, uint32_t lstart_grant, bool put)
+{
+  uint32_t s, n, l, lp, lstart, lend, nof_refs;
+  cf_t *in_ptr = input, *out_ptr = output;
+  uint32_t offset = 0;
+
+  #ifdef DEBUG_IDX
+  indices_ptr = 0;
+  if (put) {
+    offset_original = output;
+  } else {
+    offset_original = input;
+  }
+  #endif
+  nof_refs = 6;
+  for (s = 0; s < 2; s++) {
+    for (l = 0; l < SRSLTE_CP_EXT_NSYMB; l++) {
+      for (n = 0; n < q->cell.nof_prb; n++) {
+        // If this PRB is assigned
+        if (true) {
+          if (s == 0) {
+            lstart = lstart_grant;
+          } else {
+            lstart = 0;
+          }
+          lend = SRSLTE_CP_EXT_NSYMB;
+          lp = l + s * SRSLTE_CP_EXT_NSYMB;
+          if (put) {
+            out_ptr = &output[(lp * q->cell.nof_prb + n) * SRSLTE_NRE];
+          } else {
+            in_ptr = &input[(lp * q->cell.nof_prb + n) * SRSLTE_NRE];
+          }
+          // This is a symbol in a normal PRB with or without references
+          if (l >= lstart && l < lend) {
+            if (SRSLTE_SYMBOL_HAS_REF_MBSFN(l,s)) {
+              if (l == 0 && s == 1) {
+                offset = 1;
+              } else {
+                offset = 0;
+              }
+              prb_cp_ref(&in_ptr, &out_ptr, offset, nof_refs, nof_refs, put);
+            } else {
+              prb_cp(&in_ptr, &out_ptr, 1);
+            }
+          }
+        }
+      }
+    }
+  }
+
+  int r;
+  if (put) {
+    r = abs((int) (input - in_ptr));
+  } else {
+    r = abs((int) (output - out_ptr));
+  }
+
+  return r;
+}
+
+/**
+ * Puts PMCH in slot number 1
+ *
+ * Returns the number of symbols written to sf_symbols
+ *
+ * 36.211 10.3 section 6.3.5
+ */
+int srslte_pmch_put(srslte_pmch_t *q, cf_t *symbols, cf_t *sf_symbols, uint32_t lstart)
+{
+  return srslte_pmch_cp(q, symbols, sf_symbols, lstart, true);
+}
+
+/**
+ * Extracts PMCH from slot number 1
+ *
+ * Returns the number of symbols written to PMCH
+ *
+ * 36.211 10.3 section 6.3.5
+ */
+int srslte_pmch_get(srslte_pmch_t *q, cf_t *sf_symbols, cf_t *symbols, uint32_t lstart)
+{
+  return srslte_pmch_cp(q, sf_symbols, symbols, lstart, false);
+}
+
+int srslte_pmch_init(srslte_pmch_t *q, uint32_t max_prb)
+{
+  return srslte_pmch_init_multi(q, max_prb, 1);
+}
+
+int srslte_pmch_init_multi(srslte_pmch_t *q, uint32_t max_prb, uint32_t nof_rx_antennas)
+{
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  int i;
+
+ if (q != NULL                  &&
+     nof_rx_antennas <= SRSLTE_MAX_PORTS) 
+  {   
+    
+    bzero(q, sizeof(srslte_pmch_t));
+    ret = SRSLTE_ERROR;
+    
+    q->cell.nof_prb = max_prb;
+    q->cell.nof_ports = 1;
+    q->max_re = max_prb * MAX_PMCH_RE;
+    q->nof_rx_antennas = nof_rx_antennas; 
+    
+    INFO("Init PMCH: %d PRBs, max_symbols: %d\n",
+        max_prb, q->max_re);
+
+    for (i = 0; i < 4; i++) {
+      if (srslte_modem_table_lte(&q->mod[i], modulations[i])) {
+        goto clean;
+      }
+      srslte_modem_table_bytes(&q->mod[i]);
+    }
+    
+    srslte_sch_init(&q->dl_sch);
+    
+    // Allocate int16_t for reception (LLRs)
+    q->e = srslte_vec_malloc(sizeof(int16_t) * q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM));
+    if (!q->e) {
+      goto clean;
+    }
+    
+    q->d = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+    if (!q->d) {
+      goto clean;
+    }
+
+    for (i = 0; i < SRSLTE_MAX_PORTS; i++) {
+      q->x[i] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->x[i]) {
+        goto clean;
+      }
+      for (int j=0;j<q->nof_rx_antennas;j++) {
+        q->ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+        if (!q->ce[i][j]) {
+          goto clean;
+        }
+      }
+    }
+    for (int j=0;j<q->nof_rx_antennas;j++) {
+      q->symbols[j] = srslte_vec_malloc(sizeof(cf_t) * q->max_re);
+      if (!q->symbols[j]) {
+        goto clean;
+      }              
+    }
+    
+    q->seqs = calloc(SRSLTE_MAX_MBSFN_AREA_IDS, sizeof(srslte_pmch_seq_t*));
+    if (!q->seqs) {
+      perror("calloc");
+      goto clean;
+    }
+    
+    ret = SRSLTE_SUCCESS;
+  }
+  clean: 
+  if (ret == SRSLTE_ERROR) {
+    srslte_pmch_free(q);
+  }
+  return ret;
+}
+
+void srslte_pmch_free(srslte_pmch_t *q) {
+  uint16_t i;
+
+  if (q->e) {
+    free(q->e);
+  }
+  if (q->d) {
+    free(q->d);
+  }
+  for (i = 0; i < q->cell.nof_ports; i++) {
+    if (q->x[i]) {
+      free(q->x[i]);
+    }
+    for (int j=0;j<q->nof_rx_antennas;j++) {
+      if (q->ce[i][j]) {
+        free(q->ce[i][j]);
+      }
+    }
+  }
+  for (i=0;i<q->nof_rx_antennas;i++) {
+    if (q->symbols[i]) {
+      free(q->symbols[i]);
+    }          
+  }
+  if (q->seqs) {
+    for (i=0; i<SRSLTE_MAX_MBSFN_AREA_IDS; i++) {
+      if (q->seqs[i]) {
+        srslte_pmch_free_area_id(q, i);
+      }
+    }      
+    free(q->seqs);
+  }
+  for (i = 0; i < 4; i++) {
+    srslte_modem_table_free(&q->mod[i]);
+  }
+
+  srslte_sch_free(&q->dl_sch);
+
+  bzero(q, sizeof(srslte_pmch_t));
+
+}
+
+
+/* Precalculate the scramble sequences for a given MBSFN area ID. This function takes a while
+ * to execute.
+ */
+int srslte_pmch_set_area_id(srslte_pmch_t *q, uint16_t area_id) {
+  uint32_t i;  
+  if (!q->seqs[area_id]) {
+    q->seqs[area_id] = calloc(1, sizeof(srslte_pmch_seq_t));
+    if (q->seqs[area_id]) {
+      for (i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+        if (srslte_sequence_pmch(&q->seqs[area_id]->seq[i], 2 * i , area_id, q->max_re * srslte_mod_bits_x_symbol(SRSLTE_MOD_64QAM))) {
+          return SRSLTE_ERROR; 
+        }
+      }
+    }
+  }
+  return SRSLTE_SUCCESS;
+}
+
+void srslte_pmch_free_area_id(srslte_pmch_t* q, uint16_t area_id)
+{
+  if (q->seqs[area_id]) {
+    for (int i = 0; i < SRSLTE_NSUBFRAMES_X_FRAME; i++) {
+      srslte_sequence_free(&q->seqs[area_id]->seq[i]);
+    }
+    free(q->seqs[area_id]);
+    q->seqs[area_id] = NULL;
+  }
+}
+
+int srslte_pmch_cfg(srslte_pdsch_cfg_t *cfg, srslte_cell_t cell, srslte_ra_dl_grant_t *grant, uint32_t cfi, uint32_t sf_idx)
+{
+  if (cfg) {
+    if (grant) {
+      memcpy(&cfg->grant, grant, sizeof(srslte_ra_dl_grant_t));
+    }
+    if (srslte_cbsegm(&cfg->cb_segm[0], cfg->grant.mcs[0].tbs)) {
+      fprintf(stderr, "Error computing Codeblock segmentation for TBS=%d\n", cfg->grant.mcs[0].tbs);
+      return SRSLTE_ERROR;
+    }
+    srslte_ra_dl_grant_to_nbits(&cfg->grant, cfi, cell, sf_idx, cfg->nbits);
+    cfg->sf_idx = sf_idx;
+    cfg->rv[0] = SRSLTE_PMCH_RV;
+
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
+
+
+int srslte_pmch_decode(srslte_pmch_t *q,
+                        srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                        cf_t *sf_symbols, cf_t *ce[SRSLTE_MAX_PORTS], float noise_estimate, 
+                        uint16_t area_id, uint8_t *data)
+{
+  cf_t *_sf_symbols[SRSLTE_MAX_PORTS]; 
+  cf_t *_ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+  
+  _sf_symbols[0] = sf_symbols; 
+  for (int i=0;i<q->cell.nof_ports;i++) {
+    _ce[i][0] = ce[i]; 
+  }
+  return srslte_pmch_decode_multi(q, cfg, softbuffer, _sf_symbols, _ce, noise_estimate, area_id, data);
+}
+
+/** Decodes the pmch from the received symbols
+ */
+int srslte_pmch_decode_multi(srslte_pmch_t *q,
+                             srslte_pdsch_cfg_t *cfg, srslte_softbuffer_rx_t *softbuffer,
+                             cf_t *sf_symbols[SRSLTE_MAX_PORTS], cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS], float noise_estimate,
+                             uint16_t area_id, uint8_t *data)
+{
+
+  /* Set pointers for layermapping & precoding */
+  uint32_t i, n;
+  cf_t *x[SRSLTE_MAX_LAYERS];
+  
+  if (q            != NULL &&
+      sf_symbols   != NULL &&
+      data         != NULL && 
+      cfg          != NULL)
+  {
+    
+    INFO("Decoding PMCH SF: %d, MBSFN area ID: 0x%x, Mod %s, TBS: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d, C_prb=%d, cfi=%d\n",
+        cfg->sf_idx, area_id, srslte_mod_string(cfg->grant.mcs[0].mod), cfg->grant.mcs[0].tbs, cfg->nbits[0].nof_re,
+         cfg->nbits[0].nof_bits, 0, cfg->grant.nof_prb, cfg->nbits[0].lstart-1);
+
+    /* number of layers equals number of ports */
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      x[i] = q->x[i];
+    }
+    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
+     
+    for (int j=0;j<q->nof_rx_antennas;j++) {
+      /* extract symbols */
+      n = srslte_pmch_get(q, sf_symbols[j], q->symbols[j], cfg->nbits[0].lstart);
+      if (n != cfg->nbits[0].nof_re) {
+        fprintf(stderr, "PMCH 1 extract symbols error expecting %d symbols but got %d, lstart %d\n", cfg->nbits[0].nof_re, n, cfg->nbits[0].lstart);
+        return SRSLTE_ERROR;
+      }
+      
+      /* extract channel estimates */
+      for (i = 0; i < q->cell.nof_ports; i++) {
+        n = srslte_pmch_get(q, ce[i][j], q->ce[i][j], cfg->nbits[0].lstart);
+        if (n != cfg->nbits[0].nof_re) {
+          fprintf(stderr, "PMCH 2 extract chest error expecting %d symbols but got %d\n", cfg->nbits[0].nof_re, n);
+          return SRSLTE_ERROR;
+        }
+      }      
+    }
+     
+    // No tx diversity in MBSFN
+    srslte_predecoding_single_multi(q->symbols, q->ce[0], q->d, q->nof_rx_antennas, cfg->nbits[0].nof_re, noise_estimate);
+    
+    if (SRSLTE_VERBOSE_ISDEBUG()) {
+      DEBUG("SAVED FILE subframe.dat: received subframe symbols\n",0);
+      srslte_vec_save_file("subframe.dat", sf_symbols, SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      DEBUG("SAVED FILE hest0.dat: channel estimates for port 4\n",0);
+      srslte_vec_save_file("hest0.dat", ce[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+      DEBUG("SAVED FILE pmch_symbols.dat: symbols after equalization\n",0);
+      srslte_vec_save_file("pmch_symbols.bin", q->d, cfg->nbits[0].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 thde LLRs normalization
+    */
+   
+    srslte_demod_soft_demodulate_s(cfg->grant.mcs[0].mod, q->d, q->e, cfg->nbits[0].nof_re);
+   
+    /* descramble */
+    srslte_scrambling_s_offset(&q->seqs[area_id]->seq[cfg->sf_idx], q->e, 0, cfg->nbits[0].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[0].nof_bits*sizeof(int16_t));
+    }
+    return srslte_dlsch_decode(&q->dl_sch, cfg, softbuffer, q->e, data);
+  } else {
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+}
+
+int srslte_pmch_encode(srslte_pmch_t *q,
+                       srslte_pdsch_cfg_t *cfg, srslte_softbuffer_tx_t *softbuffer,
+                       uint8_t *data, uint16_t area_id, cf_t *sf_symbols[SRSLTE_MAX_PORTS])
+{
+  
+  int i;
+  /* Set pointers for layermapping & precoding */
+  cf_t *x[SRSLTE_MAX_LAYERS];
+  int ret = SRSLTE_ERROR_INVALID_INPUTS; 
+   
+  if (q != NULL && cfg != NULL)
+  {
+    for (i=0;i<q->cell.nof_ports;i++) {
+      if (sf_symbols[i] == NULL) {
+        return SRSLTE_ERROR_INVALID_INPUTS;
+      }
+    }
+    
+    if (cfg->grant.mcs[0].tbs == 0) {
+      return SRSLTE_ERROR_INVALID_INPUTS;      
+    }
+    
+    if (cfg->nbits[0].nof_re > q->max_re) {
+      fprintf(stderr,
+          "Error too many RE per subframe (%d). PMCH configured for %d RE (%d PRB)\n",
+          cfg->nbits[0].nof_re, q->max_re, q->cell.nof_prb);
+      return SRSLTE_ERROR_INVALID_INPUTS;
+    }
+
+    INFO("Encoding PMCH SF: %d, Mod %s, NofBits: %d, NofSymbols: %d, NofBitsE: %d, rv_idx: %d\n",
+        cfg->sf_idx, srslte_mod_string(cfg->grant.mcs[0].mod), cfg->grant.mcs[0].tbs, 
+         cfg->nbits[0].nof_re, cfg->nbits[0].nof_bits, 0);
+
+    /* number of layers equals number of ports */
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      x[i] = q->x[i];
+    }
+    memset(&x[q->cell.nof_ports], 0, sizeof(cf_t*) * (SRSLTE_MAX_LAYERS - q->cell.nof_ports));
+
+    // TODO: use tb_encode directly
+    if (srslte_dlsch_encode(&q->dl_sch, cfg, softbuffer, data, q->e)) {
+      fprintf(stderr, "Error encoding TB\n");
+      return SRSLTE_ERROR;
+    }
+
+    /* scramble */
+    srslte_scrambling_bytes(&q->seqs[area_id]->seq[cfg->sf_idx], (uint8_t*) q->e, cfg->nbits[0].nof_bits);
+
+    srslte_mod_modulate_bytes(&q->mod[cfg->grant.mcs[0].mod], (uint8_t*) q->e, q->d, cfg->nbits[0].nof_bits);
+    
+    /* No tx diversity in MBSFN */
+    memcpy(q->symbols[0], q->d, cfg->nbits[0].nof_re * sizeof(cf_t));
+
+    /* mapping to resource elements */
+    for (i = 0; i < q->cell.nof_ports; i++) {
+      srslte_pmch_put(q, q->symbols[i], sf_symbols[i], cfg->nbits[0].lstart);
+    }
+    
+    ret = SRSLTE_SUCCESS;
+  } 
+  return ret; 
+}
+
+float srslte_pmch_average_noi(srslte_pmch_t *q)
+{
+  return q->dl_sch.average_nof_iterations;
+}
+
+uint32_t srslte_pmch_last_noi(srslte_pmch_t *q) {
+  return q->dl_sch.nof_iterations;
+}
+
+
+
+  
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index b85d2ea2f..418aa1260 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -41,15 +41,19 @@
 
 /* Returns the number of RE in a PRB in a slot and subframe */
 uint32_t ra_re_x_prb(uint32_t subframe, uint32_t slot, uint32_t prb_idx, uint32_t nof_prb,
-    uint32_t nof_ports, uint32_t nof_ctrl_symbols, srslte_cp_t cp) {
+    uint32_t nof_ports, uint32_t nof_ctrl_symbols, srslte_cp_t cp, srslte_sf_t sf_type) {
 
   uint32_t re;
-  bool skip_refs = false;
+  bool skip_refs = true;
+  srslte_cp_t cp_ = cp;
+  if(SRSLTE_SF_MBSFN == sf_type) {
+    cp_ = SRSLTE_CP_EXT;
+  }
 
   if (slot == 0) {
-    re = (SRSLTE_CP_NSYMB(cp) - nof_ctrl_symbols) * SRSLTE_NRE;
+    re = (SRSLTE_CP_NSYMB(cp_) - nof_ctrl_symbols) * SRSLTE_NRE;
   } else {
-    re = SRSLTE_CP_NSYMB(cp) * SRSLTE_NRE;
+    re = SRSLTE_CP_NSYMB(cp_) * SRSLTE_NRE;
   }
 
   /* if it's the prb in the middle, there are less RE due to PBCH and PSS/SSS */
@@ -57,18 +61,18 @@ uint32_t ra_re_x_prb(uint32_t subframe, uint32_t slot, uint32_t prb_idx, uint32_
       && (prb_idx >= nof_prb / 2 - 3 && prb_idx < nof_prb / 2 + 3 + (nof_prb%2))) {
     if (subframe == 0) {
       if (slot == 0) {
-        re = (SRSLTE_CP_NSYMB(cp) - nof_ctrl_symbols - 2) * SRSLTE_NRE;
+        re = (SRSLTE_CP_NSYMB(cp_) - nof_ctrl_symbols - 2) * SRSLTE_NRE;
       } else {
-        if (SRSLTE_CP_ISEXT(cp)) {
-          re = (SRSLTE_CP_NSYMB(cp) - 4) * SRSLTE_NRE;
-          skip_refs = true;
+        if (SRSLTE_CP_ISEXT(cp_)) {
+          re = (SRSLTE_CP_NSYMB(cp_) - 4) * SRSLTE_NRE;
+          skip_refs = false;
         } else {
-          re = (SRSLTE_CP_NSYMB(cp) - 4) * SRSLTE_NRE + 2 * nof_ports;
+          re = (SRSLTE_CP_NSYMB(cp_) - 4) * SRSLTE_NRE + 2 * nof_ports;
         }
       }
     } else if (subframe == 5) {
       if (slot == 0) {
-        re = (SRSLTE_CP_NSYMB(cp) - nof_ctrl_symbols - 2) * SRSLTE_NRE;
+        re = (SRSLTE_CP_NSYMB(cp_) - nof_ctrl_symbols - 2) * SRSLTE_NRE;
       }
     }
     if ((nof_prb % 2)
@@ -77,7 +81,7 @@ uint32_t ra_re_x_prb(uint32_t subframe, uint32_t slot, uint32_t prb_idx, uint32_
         re += 2 * SRSLTE_NRE / 2;
       } else if (subframe == 0) {
         re += 4 * SRSLTE_NRE / 2 - nof_ports;
-        if (SRSLTE_CP_ISEXT(cp)) {
+        if (SRSLTE_CP_ISEXT(cp_)) {
           re -= nof_ports > 2 ? 2 : nof_ports;
         }
       }
@@ -85,22 +89,27 @@ uint32_t ra_re_x_prb(uint32_t subframe, uint32_t slot, uint32_t prb_idx, uint32_
   }
 
   // remove references
-  if (!skip_refs) {
-    switch (nof_ports) {
-    case 1:
-    case 2:
-      re -= 2 * (slot + 1) * nof_ports;
-      break;
-    case 4:
-      if (slot == 1) {
-        re -= 12;
-      } else {
-        re -= 4;
-        if (nof_ctrl_symbols == 1) {
+  if (skip_refs) {
+    if(sf_type == SRSLTE_SF_NORM){
+      switch (nof_ports) {
+      case 1:
+      case 2:
+        re -= 2 * (slot + 1) * nof_ports;
+        break;
+      case 4:
+        if (slot == 1) {
+          re -= 12;
+        } else {
           re -= 4;
+          if (nof_ctrl_symbols == 1) {
+            re -= 4;
+          }
         }
+        break;
       }
-      break;
+    }
+    if(sf_type == SRSLTE_SF_MBSFN){
+      re -= 6*(slot + 1);
     }
   }
   return re;
@@ -278,23 +287,23 @@ uint32_t srslte_ra_dl_approx_nof_re(srslte_cell_t cell, uint32_t nof_prb, uint32
 
 /* Computes the number of RE for each PRB in the prb_dist structure */
 uint32_t srslte_ra_dl_grant_nof_re(srslte_ra_dl_grant_t *grant, srslte_cell_t cell, 
-                                      uint32_t sf_idx, uint32_t nof_ctrl_symbols) 
+                                      uint32_t sf_idx, uint32_t nof_ctrl_symbols)
 {
   uint32_t j, s;
-
   // Compute number of RE per PRB
   uint32_t nof_re = 0;
   for (s = 0; s < 2; s++) {
     for (j = 0; j < cell.nof_prb; j++) {
       if (grant->prb_idx[s][j]) {
-        nof_re += ra_re_x_prb(sf_idx, s, j,
-            cell.nof_prb, cell.nof_ports, nof_ctrl_symbols, cell.cp);          
+        nof_re += ra_re_x_prb(sf_idx, s, j, cell.nof_prb, cell.nof_ports,
+                              nof_ctrl_symbols, cell.cp, grant->sf_type);
       }
     }
-  }  
+  }
   return nof_re; 
 }
 
+
 /** Compute PRB allocation for Downlink as defined in 7.1.6 of 36.213
  * Decode dci->type?_alloc to grant
  * This function only reads dci->type?_alloc and dci->alloc_type fields.
@@ -432,7 +441,7 @@ int srslte_ra_dl_dci_to_grant_prb_allocation(srslte_ra_dl_dci_t *dci, srslte_ra_
   return SRSLTE_SUCCESS;
 }
 
-int dl_fill_ra_mcs(srslte_ra_mcs_t *mcs, uint32_t nprb) {
+int srslte_dl_fill_ra_mcs(srslte_ra_mcs_t *mcs, uint32_t nprb) {
   uint32_t i_tbs = 0; 
   int tbs = -1; 
   if (mcs->idx < 10) {
@@ -466,6 +475,52 @@ int dl_fill_ra_mcs(srslte_ra_mcs_t *mcs, uint32_t nprb) {
   return tbs; 
 }
 
+int srslte_dl_fill_ra_mcs_pmch(srslte_ra_mcs_t *mcs, uint32_t nprb) {
+  uint32_t i_tbs = 0; 
+  int tbs = -1; 
+  if (mcs->idx < 5) {
+    mcs->mod = SRSLTE_MOD_QPSK;
+    i_tbs = mcs->idx*2;
+  }else if (mcs->idx < 6) {
+    mcs->mod = SRSLTE_MOD_16QAM;
+    i_tbs = mcs->idx*2;
+  }else if (mcs->idx < 11) {
+    mcs->mod = SRSLTE_MOD_16QAM;
+    i_tbs = mcs->idx + 5;
+  }else if (mcs->idx < 20) {
+    mcs->mod = SRSLTE_MOD_64QAM;
+    i_tbs = mcs->idx + 5;
+  }else if (mcs->idx < 28) {
+    //mcs->mod = SRSLTE_MOD_256QAM; 
+    i_tbs = mcs->idx + 5;
+  }else if (mcs->idx == 28) {
+    mcs->mod = SRSLTE_MOD_QPSK;
+    tbs = 0;
+    i_tbs = 0;
+  }else if (mcs->idx == 29) {
+    mcs->mod = SRSLTE_MOD_16QAM;
+    tbs = 0;
+    i_tbs = 0;
+  }else if (mcs->idx == 30) {
+    mcs->mod = SRSLTE_MOD_64QAM;
+    tbs = 0;
+    i_tbs = 0;
+  }else if (mcs->idx == 31) {
+    mcs->mod = SRSLTE_MOD_64QAM;
+    tbs = 0;
+    i_tbs = 0;
+  }
+  
+  
+  if (tbs == -1) {
+    tbs = srslte_ra_tbs_from_idx(i_tbs, nprb);
+    if (tbs >= 0) {
+      mcs->tbs = tbs; 
+    }
+  }  
+  return tbs; 
+}
+
 /* Modulation order and transport block size determination 7.1.7 in 36.213
  * This looks at DCI type, type of RNTI and reads fields dci->type?_alloc, dci->mcs_idx,
  * dci->dci_is_1a and dci->dci_is_1c
@@ -496,21 +551,23 @@ static int dl_dci_to_grant_mcs(srslte_ra_dl_dci_t *dci, srslte_ra_dl_grant_t *gr
     grant->mcs[0].tbs = (uint32_t) tbs;
   } else {
     n_prb = grant->nof_prb;
+    grant->nof_tb = 0; 
     if (dci->tb_en[0]) {
       grant->mcs[0].idx = dci->mcs_idx;
-      tbs   = dl_fill_ra_mcs(&grant->mcs[0], n_prb);
+      tbs   = srslte_dl_fill_ra_mcs(&grant->mcs[0], n_prb);
       if (tbs) {
         last_dl_tbs[dci->harq_process%8] = tbs;
       } else {
         // For mcs>=29, set last TBS received for this PID
         grant->mcs[0].tbs = last_dl_tbs[dci->harq_process%8];
       }
+      grant->nof_tb++;
     } else {
       grant->mcs[0].tbs = 0;
     }
     if (dci->tb_en[1]) {
       grant->mcs[1].idx = dci->mcs_idx_1;
-      tbs = dl_fill_ra_mcs(&grant->mcs[1], n_prb);
+      tbs = srslte_dl_fill_ra_mcs(&grant->mcs[1], n_prb);
       if (tbs) {
         last_dl_tbs2[dci->harq_process%8] = tbs;
       } else {
@@ -545,7 +602,11 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
       /* Compute number of RE for first transport block */
       nbits[i].nof_re = srslte_ra_dl_grant_nof_re(grant, cell, sf_idx, cell.nof_prb < 10 ? (cfi + 1) : cfi);
       nbits[i].lstart = cell.nof_prb < 10 ? (cfi + 1) : cfi;
-      nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+      if (SRSLTE_SF_NORM == grant->sf_type) {
+        nbits[i].nof_symb = 2 * SRSLTE_CP_NSYMB(cell.cp) - nbits[0].lstart;
+      } else if (SRSLTE_SF_MBSFN == grant->sf_type) {
+        nbits[i].nof_symb = 2 * SRSLTE_CP_EXT_NSYMB - nbits[0].lstart;
+      }
       nbits[i].nof_bits = nbits[i].nof_re * grant->Qm[i];
     }
   }
@@ -554,7 +615,8 @@ void srslte_ra_dl_grant_to_nbits(srslte_ra_dl_grant_t *grant, uint32_t cfi, srsl
 /** Obtains a DL grant from a DCI grant for PDSCH */
 int srslte_ra_dl_dci_to_grant(srslte_ra_dl_dci_t *dci, 
                               uint32_t nof_prb, uint16_t msg_rnti, srslte_ra_dl_grant_t *grant) 
-{  
+{
+  grant->sf_type = SRSLTE_SF_NORM;
   bool crc_is_crnti = false; 
   if (msg_rnti >= SRSLTE_CRNTI_START && msg_rnti <= SRSLTE_CRNTI_END) {
     crc_is_crnti = true; 
@@ -844,6 +906,4 @@ void srslte_ra_prb_fprint(FILE *f, srslte_ra_dl_grant_t *grant) {
     }
   }
   
-}
-
-
+}
\ No newline at end of file
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index a44ced2d8..efdc38846 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -32,10 +32,10 @@
 #include <stdbool.h>
 #include <assert.h>
 #include <math.h>
-
 #include "srslte/phy/phch/pdsch.h"
 #include "srslte/phy/phch/pusch.h"
 #include "srslte/phy/phch/sch.h"
+#include "srslte/phy/phch/pmch.h"
 #include "srslte/phy/phch/uci.h"
 #include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/utils/bit.h"
diff --git a/lib/src/phy/phch/sequences.c b/lib/src/phy/phch/sequences.c
index 23ac410c4..dc9e45a7f 100644
--- a/lib/src/phy/phch/sequences.c
+++ b/lib/src/phy/phch/sequences.c
@@ -78,3 +78,9 @@ int srslte_sequence_pusch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot,
 int srslte_sequence_pucch(srslte_sequence_t *seq, uint16_t rnti, uint32_t nslot, uint32_t cell_id) {
   return srslte_sequence_LTE_pr(seq, 20, ((((nslot/2)+1)*(2*cell_id+1))<<16)+rnti);
 }
+
+int srslte_sequence_pmch(srslte_sequence_t *seq, uint32_t nslot, uint32_t mbsfn_id , uint32_t len){
+  bzero(seq,sizeof(srslte_sequence_t));
+  return srslte_sequence_LTE_pr(seq, len, (((nslot/2)<<9) + mbsfn_id));
+  
+}
\ No newline at end of file
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index fd04a4c85..cd7aadb4c 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -64,9 +64,11 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
     ret = SRSLTE_ERROR;
     
     bzero(q, sizeof(srslte_ue_dl_t));
-    
-    q->pkt_errors = 0;
-    q->pkts_total = 0;
+   
+    q->pdsch_pkt_errors = 0;
+    q->pdsch_pkts_total = 0;
+    q->pmch_pkt_errors = 0;
+    q->pmch_pkts_total = 0;
     q->pending_ul_dci_rnti = 0; 
     q->sample_offset = 0; 
     q->nof_rx_antennas = nof_rx_antennas;
@@ -75,6 +77,13 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
       fprintf(stderr, "Error initiating FFT\n");
       goto clean_exit;
     }
+        
+    if (srslte_ofdm_rx_init_mbsfn(&q->fft_mbsfn, SRSLTE_CP_EXT, max_prb)) {
+      fprintf(stderr, "Error initiating FFT for MBSFN subframes \n");
+      goto clean_exit;
+    }
+    srslte_ofdm_set_non_mbsfn_region(&q->fft_mbsfn, 2); // Set a default to init
+    
     if (srslte_chest_dl_init(&q->chest, max_prb)) {
       fprintf(stderr, "Error initiating channel estimator\n");
       goto clean_exit;
@@ -97,6 +106,11 @@ int srslte_ue_dl_init(srslte_ue_dl_t *q,
       fprintf(stderr, "Error creating PDSCH object\n");
       goto clean_exit;
     }
+
+    if (srslte_pmch_init_multi(&q->pmch, max_prb, nof_rx_antennas)) {
+      fprintf(stderr, "Error creating PMCH object\n");
+      goto clean_exit;
+    }
     for (int i = 0; i < SRSLTE_MAX_TB; i++) {
       q->softbuffers[i] = srslte_vec_malloc(sizeof(srslte_softbuffer_rx_t));
       if (!q->softbuffers[i]) {
@@ -151,12 +165,14 @@ clean_exit:
 void srslte_ue_dl_free(srslte_ue_dl_t *q) {
   if (q) {
     srslte_ofdm_rx_free(&q->fft);
+    srslte_ofdm_rx_free(&q->fft_mbsfn);
     srslte_chest_dl_free(&q->chest);
     srslte_regs_free(&q->regs);
     srslte_pcfich_free(&q->pcfich);
     srslte_phich_free(&q->phich);
     srslte_pdcch_free(&q->pdcch);
     srslte_pdsch_free(&q->pdsch);
+    srslte_pmch_free(&q->pmch);
     srslte_cfo_free(&q->sfo_correct);
     for (int i = 0; i < SRSLTE_MAX_TB; i++) {
       srslte_softbuffer_rx_free(q->softbuffers[i]);
@@ -258,6 +274,34 @@ void srslte_ue_dl_set_rnti(srslte_ue_dl_t *q, uint16_t rnti) {
   
   q->current_rnti = rnti; 
 }
+/* Set the area ID on pmch and chest_dl to generate scrambling sequence and reference
+ * signals.
+ */
+int srslte_ue_dl_set_mbsfn_area_id(srslte_ue_dl_t *q,
+                                   uint16_t mbsfn_area_id) {
+  int ret = SRSLTE_ERROR_INVALID_INPUTS;
+  if(q != NULL) {
+    ret = SRSLTE_ERROR;
+    if(srslte_chest_dl_set_mbsfn_area_id(&q->chest, mbsfn_area_id)) {
+      fprintf(stderr, "Error setting MBSFN area ID \n");
+      return ret;
+    }
+    if(srslte_pmch_set_area_id(&q->pmch, mbsfn_area_id)) {
+      fprintf(stderr, "Error setting MBSFN area ID \n");
+      return ret;
+    }
+    q->current_mbsfn_area_id = mbsfn_area_id;
+    ret = SRSLTE_SUCCESS;
+  }
+  return ret;
+}
+
+void srslte_ue_dl_set_non_mbsfn_region(srslte_ue_dl_t *q,
+                                       uint8_t non_mbsfn_region_length) {
+  srslte_ofdm_set_non_mbsfn_region(&q->fft_mbsfn, non_mbsfn_region_length);
+}
+
+
 
 void srslte_ue_dl_reset(srslte_ue_dl_t *q) {
   for(int i = 0; i < SRSLTE_MAX_CODEWORDS; i++){
@@ -279,39 +323,60 @@ void srslte_ue_dl_set_sample_offset(srslte_ue_dl_t * q, float sample_offset) {
  */
 int srslte_ue_dl_decode(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint8_t *data[SRSLTE_MAX_CODEWORDS],
                               uint32_t tm, uint32_t tti, bool acks[SRSLTE_MAX_CODEWORDS]) {
-  return srslte_ue_dl_decode_rnti(q, input, data, tm, tti, q->current_rnti, acks);
+    return srslte_ue_dl_decode_rnti(q, input, data, tm, tti, q->current_rnti, acks);
 }
 
-int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi)
+
+int srslte_ue_dl_decode_fft_estimate(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi){
+  
+  return srslte_ue_dl_decode_fft_estimate_mbsfn(q, input, sf_idx, cfi, SRSLTE_SF_NORM);
+} 
+
+int srslte_ue_dl_decode_fft_estimate_mbsfn(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS], uint32_t sf_idx, uint32_t *cfi, srslte_sf_t sf_type)
 {
   if (input && q && cfi && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
     
     /* Run FFT for all subframe data */
     for (int j=0;j<q->nof_rx_antennas;j++) {
-      srslte_ofdm_rx_sf(&q->fft, input[j], q->sf_symbols_m[j]);
+      if(sf_type == SRSLTE_SF_MBSFN ) {
+        srslte_ofdm_rx_sf(&q->fft_mbsfn, input[j], q->sf_symbols_m[j]);
+      }else{
+        srslte_ofdm_rx_sf(&q->fft, input[j], q->sf_symbols_m[j]);
+      }
 
       /* Correct SFO multiplying by complex exponential in the time domain */
       if (q->sample_offset) {
-        for (int i=0;i<2*SRSLTE_CP_NSYMB(q->cell.cp);i++) {
-          srslte_cfo_correct(&q->sfo_correct,
+        int nsym = (sf_type == SRSLTE_SF_MBSFN)?SRSLTE_CP_EXT_NSYMB:SRSLTE_CP_NSYMB(q->cell.cp);
+        for (int i=0;i<2*nsym;i++) {
+          srslte_cfo_correct(&q->sfo_correct, 
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           &q->sf_symbols_m[j][i*q->cell.nof_prb*SRSLTE_NRE], 
                           q->sample_offset / q->fft.symbol_sz);
         }
       }
     }
-    return srslte_ue_dl_decode_estimate(q, sf_idx, cfi); 
+    return srslte_ue_dl_decode_estimate_mbsfn(q, sf_idx, cfi, sf_type); 
   } else {
     return SRSLTE_ERROR_INVALID_INPUTS; 
   }
 }
-
 int srslte_ue_dl_decode_estimate(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *cfi) {
+  
+  return srslte_ue_dl_decode_estimate_mbsfn(q, sf_idx, cfi, SRSLTE_SF_NORM);
+}
+
+
+int srslte_ue_dl_decode_estimate_mbsfn(srslte_ue_dl_t *q, uint32_t sf_idx, uint32_t *cfi, srslte_sf_t sf_type) {
   float cfi_corr; 
   if (q && cfi && sf_idx < SRSLTE_NSUBFRAMES_X_FRAME) {
     
     /* Get channel estimates for each port */
-    srslte_chest_dl_estimate_multi(&q->chest, q->sf_symbols_m, q->ce_m, sf_idx, q->nof_rx_antennas);
+    if(sf_type == SRSLTE_SF_MBSFN){
+      srslte_chest_dl_estimate_multi_mbsfn(&q->chest, q->sf_symbols_m, q->ce_m, sf_idx, q->nof_rx_antennas, q->current_mbsfn_area_id);
+    }else{
+      srslte_chest_dl_estimate_multi(&q->chest, q->sf_symbols_m, q->ce_m, sf_idx, q->nof_rx_antennas);
+    }
+
 
     /* First decode PCFICH and obtain CFI */
     if (srslte_pcfich_decode_multi(&q->pcfich, q->sf_symbols_m, q->ce_m, 
@@ -358,7 +423,11 @@ int srslte_ue_dl_cfg_grant(srslte_ue_dl_t *q, srslte_ra_dl_grant_t *grant, uint3
       }
     }
   }
-  return srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
+  if(SRSLTE_SF_MBSFN == grant->sf_type) {
+    return srslte_pmch_cfg(&q->pmch_cfg, q->cell, grant, cfi, sf_idx);
+  } else {
+    return srslte_pdsch_cfg_mimo(&q->pdsch_cfg, q->cell, grant, cfi, sf_idx, rvidx, mimo_type, pmi);
+  }
 }
 
 int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
@@ -372,7 +441,7 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
   uint32_t cfi;
   uint32_t sf_idx = tti%10;
   
-  if ((ret = srslte_ue_dl_decode_fft_estimate(q, input, sf_idx, &cfi)) < 0) {
+  if ((ret = srslte_ue_dl_decode_fft_estimate_mbsfn(q, input, sf_idx, &cfi, SRSLTE_SF_NORM)) < 0) {
     return ret; 
   }
   
@@ -476,12 +545,13 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
                                 noise_estimate,
                                 rnti, data, acks);
 
+      
       for (int tb = 0; tb < SRSLTE_MAX_TB; tb++) {
         if (grant.tb_en[tb]) {
           if (!acks[tb]) {
-            q->pkt_errors++;
+            q->pdsch_pkt_errors++;
           }
-          q->pkts_total++;
+          q->pdsch_pkts_total++;
         }
       }
 
@@ -510,6 +580,69 @@ int srslte_ue_dl_decode_rnti(srslte_ue_dl_t *q, cf_t *input[SRSLTE_MAX_PORTS],
   }
 }
 
+
+
+int srslte_ue_dl_decode_mbsfn(srslte_ue_dl_t * q,
+                                    cf_t *input[SRSLTE_MAX_PORTS],
+                                    uint8_t *data,
+                                    uint32_t tti)
+{
+  srslte_ra_dl_grant_t grant; 
+  int ret = SRSLTE_ERROR; 
+  uint32_t cfi;
+  uint32_t sf_idx = tti%10; 
+  
+  if ((ret = srslte_ue_dl_decode_fft_estimate_mbsfn(q, input, sf_idx, &cfi, SRSLTE_SF_MBSFN)) < 0) {
+    return ret; 
+  }
+  
+  float noise_estimate = srslte_chest_dl_get_noise_estimate(&q->chest);
+  // Uncoment next line to do ZF by default in pdsch_ue example
+  //float noise_estimate = 0; 
+
+  grant.sf_type = SRSLTE_SF_MBSFN;
+  grant.nof_tb = 1;
+  grant.mcs[0].idx = 2;
+ 
+  grant.nof_prb = q->pmch.cell.nof_prb;
+  srslte_dl_fill_ra_mcs(&grant.mcs[0], grant.nof_prb);
+  srslte_softbuffer_rx_reset_tbs(q->softbuffers[0], (uint32_t) grant.mcs[0].tbs);
+  for(int j = 0; j < 2; j++){
+    for(int f = 0; f < grant.nof_prb; f++){
+      grant.prb_idx[j][f] = true;
+    }
+  }
+  grant.Qm[0] = srslte_mod_bits_x_symbol(grant.mcs[0].mod);
+
+  // redundancy version is set to 0 for the PMCH
+  if (srslte_ue_dl_cfg_grant(q, &grant, cfi, sf_idx, SRSLTE_PMCH_RV, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA)) {
+    return SRSLTE_ERROR;
+  }
+
+  if (q->pmch_cfg.grant.mcs[0].mod > 0 && q->pmch_cfg.grant.mcs[0].tbs >= 0) {
+    ret = srslte_pmch_decode_multi(&q->pmch, &q->pmch_cfg, q->softbuffers[0],
+                                   q->sf_symbols_m, q->ce_m,
+                                   noise_estimate,
+                                   q->current_mbsfn_area_id, data);
+    
+    if (ret == SRSLTE_ERROR) {
+      q->pmch_pkt_errors++;
+    } else if (ret == SRSLTE_ERROR_INVALID_INPUTS) {
+      fprintf(stderr, "Error calling srslte_pmch_decode()\n");
+    }
+  }
+printf("q->pmch_pkts_total %d \n", q->pmch_pkts_total);
+printf("qq->pmch_pkt_errors %d \n", q->pmch_pkt_errors);
+  q->pmch_pkts_total++;
+
+  if (ret == SRSLTE_SUCCESS) {
+    return q->pmch_cfg.grant.mcs[0].tbs;
+  } else {
+    return 0;
+  }
+}
+
+
 /* Compute the Rank Indicator (RI) and Precoder Matrix Indicator (PMI) by computing the Signal to Interference plus
  * Noise Ratio (SINR), valid for TM4 */
 int srslte_ue_dl_ri_pmi_select(srslte_ue_dl_t *q, uint8_t *ri, uint8_t *pmi, float *current_sinr) {
diff --git a/lib/src/phy/ue/ue_sync.c b/lib/src/phy/ue/ue_sync.c
index 3f5b131eb..f121c5bac 100644
--- a/lib/src/phy/ue/ue_sync.c
+++ b/lib/src/phy/ue/ue_sync.c
@@ -676,6 +676,7 @@ int srslte_ue_sync_zerocopy_multi(srslte_ue_sync_t *q, cf_t *input_buffer[SRSLTE
             /* Track PSS/SSS around the expected PSS position 
              * In tracking phase, the subframe carrying the PSS is always the last one of the frame
              */
+            
             switch(srslte_sync_find(&q->strack, input_buffer[0], 
                                     q->frame_len - q->sf_len/2 - q->fft_size - q->strack.max_offset/2, 
                                     &track_idx)) 

From f159aeeb579370cb4680033a355957645d449cee Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 25 Sep 2017 16:00:20 +0200
Subject: [PATCH 221/269] Update README.md

---
 README.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index f7c699ed3..a6eada615 100644
--- a/README.md
+++ b/README.md
@@ -72,6 +72,7 @@ Build Instructions
 
 * Mandatory requirements: 
   * Common:
+    * cmake              https://cmake.org/
     * libfftw            http://www.fftw.org/
     * PolarSSL/mbedTLS   https://tls.mbed.org
   * srsUE:
@@ -83,7 +84,7 @@ Build Instructions
 
 For example, on Ubuntu 17.04, one can install the required libraries with:
 ```
-sudo apt-get install libfftw3-dev libmbedtls-dev libboost-all-dev libconfig++-dev libsctp-dev
+sudo apt-get install cmake libfftw3-dev libmbedtls-dev libboost-all-dev libconfig++-dev libsctp-dev
 ```
 Note that depending on your flavor and version of Linux, the actual package names may be different.
 

From 3a7744af80325a2911d81037a58040b87bd34ff6 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 25 Sep 2017 22:19:03 +0200
Subject: [PATCH 222/269] fix typo

---
 srsenb/src/enb.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index d00fd4c0b..950510dcf 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -73,7 +73,7 @@ bool enb::init(all_args_t *args_)
     logger = &logger_stdout;
     printf("log name is output\n");
   } else {
-    printf("lgo name is %s\n", args->log.filename.c_str());
+    printf("log name is %s\n", args->log.filename.c_str());
     logger_file.init(args->log.filename);
     logger_file.log("\n\n");
     logger = &logger_file;

From 7be56a85ea10c22ecdaed262a4e52bfca89b0996 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 10:52:54 +0200
Subject: [PATCH 223/269] Changed order of stop in srsENB to avoid PHY errors
 at exit

---
 srsenb/src/enb.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index 2bf9c6b3f..df4c71ae9 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -228,8 +228,8 @@ void enb::stop()
 {
   if(started)
   {
-    mac.stop();
     phy.stop();
+    mac.stop();
     usleep(1e5);
 
     rlc.stop();

From 5d317454e3bdb0a019f3c364e92c10feb5880ba4 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Tue, 26 Sep 2017 12:06:07 +0200
Subject: [PATCH 224/269] initialize metrics buffer properly

---
 lib/include/srslte/common/metrics_hub.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/lib/include/srslte/common/metrics_hub.h b/lib/include/srslte/common/metrics_hub.h
index 6020271e2..7ea3c3190 100644
--- a/lib/include/srslte/common/metrics_hub.h
+++ b/lib/include/srslte/common/metrics_hub.h
@@ -47,7 +47,8 @@ public:
   
 private:
   void run_period() {
-    metrics_t metric; 
+    metrics_t metric;
+    bzero(&metric, sizeof(metrics_t));
     m->get_metrics(metric);
     for (uint32_t i=0;i<listeners.size();i++) {
       listeners[i]->set_metrics(metric);

From f15dbcdd7f5930aa78ec2f1078b455c3fffd1a2d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 13:08:05 +0200
Subject: [PATCH 225/269] Restored UCI ACK processing to single ACK

---
 lib/src/phy/phch/uci.c | 51 +++++++++++++++++++++++++++++++++++++-----
 1 file changed, 45 insertions(+), 6 deletions(-)

diff --git a/lib/src/phy/phch/uci.c b/lib/src/phy/phch/uci.c
index ed253d2cc..cb09ca08b 100644
--- a/lib/src/phy/phch/uci.c
+++ b/lib/src/phy/phch/uci.c
@@ -581,7 +581,49 @@ static uint32_t encode_ri_ack(uint8_t data[2], uint32_t data_len, srslte_uci_bit
 
   return i;
 }
-                       
+
+
+/* Decode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
+ *  Currently only supporting 1-bit HARQ
+ */
+#ifndef MIMO_ENB
+
+static int32_t decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos)
+{
+  uint32_t p0 = pos[0].position;
+  uint32_t p1 = pos[1].position;
+
+  uint32_t q0 = c_seq[p0]?q_bits[p0]:-q_bits[p0];
+  uint32_t q1 = c_seq[p0]?q_bits[p1]:-q_bits[p1];
+
+  return -(q0+q1);
+}
+int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_seq,
+                          float beta, uint32_t H_prime_total,
+                          uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t acks[2], uint32_t nof_acks)
+{
+  int32_t rx_ack = 0;
+
+  if (beta < 0) {
+    fprintf(stderr, "Error beta is reserved\n");
+    return -1;
+  }
+
+  uint32_t Qprime = Q_prime_ri_ack(cfg, 1, O_cqi, beta);
+
+  // Use the same interleaver function to get the HARQ bit position
+  for (uint32_t i=0;i<Qprime;i++) {
+    uci_ulsch_interleave_ack_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ack_bits[cfg->grant.Qm*i]);
+    rx_ack += (int32_t) decode_ri_ack(q_bits, c_seq, &ack_bits[cfg->grant.Qm*i]);
+  }
+
+  if (acks) {
+    acks[0] = rx_ack>0;
+  }
+  return (int) Qprime;
+}
+#else
+
 static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos, uint32_t Qm, int32_t data[3])
 {
   uint32_t p0 = pos[Qm * 0 + 0].position;
@@ -603,10 +645,6 @@ static void decode_ri_ack(int16_t *q_bits, uint8_t *c_seq, srslte_uci_bit_t *pos
   data[2] -= q2 + q5;
 }
 
-
-/* Decode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
- *  Currently only supporting 1-bit HARQ
- */
 int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_seq, 
                           float beta, uint32_t H_prime_total, 
                           uint32_t O_cqi, srslte_uci_bit_t *ack_bits, uint8_t acks[2], uint32_t nof_acks)
@@ -635,6 +673,7 @@ int srslte_uci_decode_ack(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_s
   }
   return (int) Qprime;  
 }
+#endif
 
 /* Encode UCI HARQ/ACK bits as described in 5.2.2.6 of 36.212 
  *  Currently only supporting 1-bit HARQ
@@ -681,7 +720,7 @@ int srslte_uci_decode_ri(srslte_pusch_cfg_t *cfg, int16_t *q_bits, uint8_t *c_se
   for (uint32_t i=0;i<Qprime;i++) {
     uci_ulsch_interleave_ri_gen(i, cfg->grant.Qm, H_prime_total, cfg->nbits.nof_symb, cfg->cp, &ri_bits[cfg->grant.Qm*i]);
     if ((i % 3 == 0) && i > 0) {
-      decode_ri_ack(q_bits, &c_seq[0], &ri_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, ri_sum);
+      //decode_ri_ack(q_bits, &c_seq[0], &ri_bits[cfg->grant.Qm*(i-3)], cfg->grant.Qm, ri_sum);
     }
   }
 

From 680ba2761c1566c6e8e10e345750b9300568f70b Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 14:48:59 +0200
Subject: [PATCH 226/269] Fixed number of iterations calculation

---
 lib/examples/cell_measurement.c     |  5 ++--
 lib/include/srslte/phy/phch/pdsch.h | 11 +++++---
 lib/include/srslte/phy/phch/sch.h   |  9 +++----
 lib/src/phy/phch/pdsch.c            | 16 +++++-------
 lib/src/phy/phch/pmch.c             |  5 ----
 lib/src/phy/phch/sch.c              | 40 +++++++++++++----------------
 6 files changed, 37 insertions(+), 49 deletions(-)

diff --git a/lib/examples/cell_measurement.c b/lib/examples/cell_measurement.c
index 143ee784e..37796682f 100644
--- a/lib/examples/cell_measurement.c
+++ b/lib/examples/cell_measurement.c
@@ -335,10 +335,9 @@ int main(int argc, char **argv) {
               fprintf(stderr, "Error decoding UE DL\n");fflush(stdout);
               return -1;
             } else if (n == 0) {
-              printf("CFO: %+6.4f kHz, SFO: %+6.4f kHz, NOI: %.2f, PDCCH-Det: %.3f\r",
+              printf("CFO: %+6.4f kHz, SFO: %+6.4f kHz, PDCCH-Det: %.3f\r",
                       srslte_ue_sync_get_cfo(&ue_sync)/1000, srslte_ue_sync_get_sfo(&ue_sync)/1000, 
-                      srslte_pdsch_average_noi(&ue_dl.pdsch),
-                      (float) ue_dl.nof_detected/nof_trials);                
+                      (float) ue_dl.nof_detected/nof_trials);
               nof_trials++; 
             } else {
               printf("Decoded SIB1. Payload: ");
diff --git a/lib/include/srslte/phy/phch/pdsch.h b/lib/include/srslte/phy/phch/pdsch.h
index 7ef2d9b3d..459bcc209 100644
--- a/lib/include/srslte/phy/phch/pdsch.h
+++ b/lib/include/srslte/phy/phch/pdsch.h
@@ -58,7 +58,8 @@ typedef struct SRSLTE_API {
   srslte_cell_t cell;
   
   uint32_t nof_rx_antennas;
-  
+  uint32_t last_nof_iterations[SRSLTE_MAX_CODEWORDS];
+
   uint32_t max_re;
 
   uint16_t ue_rnti;
@@ -149,10 +150,12 @@ SRSLTE_API int srslte_pdsch_cn_compute(srslte_pdsch_t *q,
                                        uint32_t nof_ce,
                                        float *cn);
 
-SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter);
+SRSLTE_API void srslte_pdsch_set_max_noi(srslte_pdsch_t *q,
+                                         uint32_t max_iter);
 
-SRSLTE_API float srslte_pdsch_average_noi(srslte_pdsch_t *q);
+SRSLTE_API float srslte_pdsch_last_noi(srslte_pdsch_t *q);
 
-SRSLTE_API uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q); 
+SRSLTE_API uint32_t srslte_pdsch_last_noi_cw(srslte_pdsch_t *q,
+                                             uint32_t cw_idx);
 
 #endif
diff --git a/lib/include/srslte/phy/phch/sch.h b/lib/include/srslte/phy/phch/sch.h
index a98ecf280..e8066e4cd 100644
--- a/lib/include/srslte/phy/phch/sch.h
+++ b/lib/include/srslte/phy/phch/sch.h
@@ -56,10 +56,9 @@
 /* DL-SCH AND UL-SCH common functions */
 typedef struct SRSLTE_API {
   
-  uint32_t max_iterations; 
-  uint32_t nof_iterations; 
-  float average_nof_iterations; 
-  
+  uint32_t max_iterations;
+  uint32_t nof_iterations;
+
   /* buffers */
   uint8_t *cb_in; 
   uint8_t *parity_bits;  
@@ -87,8 +86,6 @@ SRSLTE_API void srslte_sch_free(srslte_sch_t *q);
 SRSLTE_API void srslte_sch_set_max_noi(srslte_sch_t *q, 
                                        uint32_t max_iterations); 
 
-SRSLTE_API float srslte_sch_average_noi(srslte_sch_t *q);
-
 SRSLTE_API uint32_t srslte_sch_last_noi(srslte_sch_t *q);
 
 SRSLTE_API int srslte_dlsch_encode(srslte_sch_t *q, 
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index de553630e..bdb0a755a 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -29,15 +29,11 @@
 #include <string.h>
 #include <stdlib.h>
 #include <stdbool.h>
-#include <srslte/phy/phch/pdsch_cfg.h>
-#include <srslte/phy/common/sequence.h>
-#include <srslte/phy/phch/pdsch.h>
 
 #include "prb_dl.h"
 #include "srslte/phy/phch/pdsch.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
-#include "srslte/phy/utils/bit.h"
 
 
 #define MAX_PDSCH_RE(cp) (2 * SRSLTE_CP_NSYMB(cp) * 12)
@@ -606,6 +602,9 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
 
     /* Return  */
     ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
+
+    q->last_nof_iterations[codeword_idx] = srslte_sch_last_noi(&q->dl_sch);
+
     if (ret == SRSLTE_SUCCESS) {
       *ack = true;
     } else if (ret == SRSLTE_ERROR) {
@@ -828,14 +827,13 @@ void srslte_pdsch_set_max_noi(srslte_pdsch_t *q, uint32_t max_iter) {
   srslte_sch_set_max_noi(&q->dl_sch, max_iter);
 }
 
-float srslte_pdsch_average_noi(srslte_pdsch_t *q) {
-  return q->dl_sch.average_nof_iterations;
+float srslte_pdsch_last_noi(srslte_pdsch_t *q) {
+  return srslte_pdsch_last_noi_cw(q, 0);
 }
 
-uint32_t srslte_pdsch_last_noi(srslte_pdsch_t *q) {
-  return q->dl_sch.nof_iterations;
+uint32_t srslte_pdsch_last_noi_cw(srslte_pdsch_t *q, uint32_t cw_idx) {
+  return q->last_nof_iterations[cw_idx];
 }
 
 
-
   
diff --git a/lib/src/phy/phch/pmch.c b/lib/src/phy/phch/pmch.c
index ee1b3554f..4ce869b1c 100644
--- a/lib/src/phy/phch/pmch.c
+++ b/lib/src/phy/phch/pmch.c
@@ -472,11 +472,6 @@ int srslte_pmch_encode(srslte_pmch_t *q,
   return ret; 
 }
 
-float srslte_pmch_average_noi(srslte_pmch_t *q)
-{
-  return q->dl_sch.average_nof_iterations;
-}
-
 uint32_t srslte_pmch_last_noi(srslte_pmch_t *q) {
   return q->dl_sch.nof_iterations;
 }
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index efdc38846..b7909192f 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -33,11 +33,6 @@
 #include <assert.h>
 #include <math.h>
 #include "srslte/phy/phch/pdsch.h"
-#include "srslte/phy/phch/pusch.h"
-#include "srslte/phy/phch/sch.h"
-#include "srslte/phy/phch/pmch.h"
-#include "srslte/phy/phch/uci.h"
-#include "srslte/phy/common/phy_common.h"
 #include "srslte/phy/utils/bit.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
@@ -174,15 +169,10 @@ void srslte_sch_set_max_noi(srslte_sch_t *q, uint32_t max_iterations) {
   q->max_iterations = max_iterations;
 }
 
-float srslte_sch_average_noi(srslte_sch_t *q) {
-  return q->average_nof_iterations; 
-}
-
 uint32_t srslte_sch_last_noi(srslte_sch_t *q) {
   return q->nof_iterations;
 }
 
-
 /* Encode a transport block according to 36.212 5.3.2
  *
  */
@@ -340,7 +330,7 @@ bool decode_tb_cb(srslte_sch_t *q,
   
   for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
     cb_idx[i]        = i+first_cb; 
-    decoder_input[i] = false; 
+    decoder_input[i] = NULL;
   }
   
   for (int i=0;i<nof_cb;i++) {
@@ -349,8 +339,10 @@ bool decode_tb_cb(srslte_sch_t *q,
     
   srslte_tdec_reset(&q->decoder, cb_len);
   
-  uint32_t remaining_cb = nof_cb; 
-  
+  uint32_t remaining_cb = nof_cb;
+
+  q->nof_iterations = 0;
+
   while(remaining_cb>0) {
         
     // Unratematch the codeblocks left to decode 
@@ -384,11 +376,9 @@ bool decode_tb_cb(srslte_sch_t *q,
       }
     }
         
-    // Run 1 iteration for up to TDEC_NPAR codeblocks 
+    // Run 1 iteration for the codeblocks in queue
     srslte_tdec_iteration_par(&q->decoder, decoder_input, cb_len);
 
-    q->nof_iterations = srslte_tdec_get_nof_iterations_cb(&q->decoder, 0); 
-    
     // Decide output bits and compute CRC 
     for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
       if (decoder_input[i]) {        
@@ -409,24 +399,30 @@ bool decode_tb_cb(srslte_sch_t *q,
         if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
 
           memcpy(&data[(cb_idx[i]*rlen)/8], q->cb_in, rlen/8 * sizeof(uint8_t));
-          
+
+          q->nof_iterations += srslte_tdec_get_nof_iterations_cb(&q->decoder, i);
+
           // Reset number of iterations for that CB in the decoder 
           srslte_tdec_reset_cb(&q->decoder, i);
           remaining_cb--;        
           decoder_input[i] = NULL; 
           cb_idx[i] = 0; 
-          
-        // CRC is error and exceeded maximum iterations for this CB. 
+
+        // CRC is error and exceeded maximum iterations for this CB.
         // Early stop the whole transport block.
         } else if (srslte_tdec_get_nof_iterations_cb(&q->decoder, i) >= q->max_iterations) {
           INFO("CB %d: Error. CB is erroneous. remaining_cb=%d, i=%d, first_cb=%d, nof_cb=%d\n", 
-                cb_idx[i], remaining_cb, i, first_cb, nof_cb);          
-          return false; 
+                cb_idx[i], remaining_cb, i, first_cb, nof_cb);
+
+          q->nof_iterations += q->max_iterations;
+          q->nof_iterations /= (nof_cb-remaining_cb+1);
+          return false;
         }
       }
     }    
   }
-  
+
+  q->nof_iterations /= nof_cb;
   return true;  
 }
 

From 736b73bb4bb2d33bbf95f6156a40b5be7d6c6ff8 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 14:51:37 +0200
Subject: [PATCH 227/269] Print PDSCH iterations per codeword

---
 lib/include/srslte/phy/enb/enb_dl.h |  9 +++++++++
 lib/src/phy/enb/enb_dl.c            | 30 +++++++++++++++++++++++++++++
 lib/src/phy/ue/ue_dl.c              |  8 ++++----
 srsenb/src/phy/phch_worker.cc       |  4 ++--
 srsue/src/phy/phch_worker.cc        |  5 +++--
 5 files changed, 48 insertions(+), 8 deletions(-)

diff --git a/lib/include/srslte/phy/enb/enb_dl.h b/lib/include/srslte/phy/enb/enb_dl.h
index 3f835a23f..7c3166e5c 100644
--- a/lib/include/srslte/phy/enb/enb_dl.h
+++ b/lib/include/srslte/phy/enb/enb_dl.h
@@ -88,6 +88,8 @@ typedef struct SRSLTE_API {
   float sss_signal5[SRSLTE_SSS_LEN]; 
     
   float tx_amp;
+
+  uint8_t tmp[1024*128];
   
 } srslte_enb_dl_t;
 
@@ -176,5 +178,12 @@ SRSLTE_API int srslte_enb_dl_put_pdcch_ul(srslte_enb_dl_t *q,
                                           uint16_t rnti, 
                                           uint32_t sf_idx); 
 
+SRSLTE_API void srslte_enb_dl_save_signal(srslte_enb_dl_t *q,
+                                          srslte_softbuffer_tx_t *softbuffer,
+                                          uint8_t *data,
+                                          uint32_t tti,
+                                          uint32_t rv_idx,
+                                          uint16_t rnti,
+                                          uint32_t cfi);
 
 #endif
diff --git a/lib/src/phy/enb/enb_dl.c b/lib/src/phy/enb/enb_dl.c
index ec470ce3d..2ba179399 100644
--- a/lib/src/phy/enb/enb_dl.c
+++ b/lib/src/phy/enb/enb_dl.c
@@ -336,3 +336,33 @@ int srslte_enb_dl_put_pdsch(srslte_enb_dl_t *q, srslte_ra_dl_grant_t *grant, srs
   }        
   return SRSLTE_SUCCESS; 
 }
+
+
+
+void srslte_enb_dl_save_signal(srslte_enb_dl_t *q, srslte_softbuffer_tx_t *softbuffer, uint8_t *data, uint32_t tti, uint32_t rv_idx, uint16_t rnti, uint32_t cfi)
+{
+  char tmpstr[64];
+
+  snprintf(tmpstr,64,"output/sf_symbols_%d",tti);
+  srslte_vec_save_file(tmpstr, q->sf_symbols[0], SRSLTE_SF_LEN_RE(q->cell.nof_prb, q->cell.cp)*sizeof(cf_t));
+
+  snprintf(tmpstr,64,"output/e_%d",tti);
+  srslte_bit_unpack_vector(q->pdsch.e[0], q->tmp, q->pdsch_cfg.nbits[0].nof_bits);
+  srslte_vec_save_file(tmpstr, q->tmp, q->pdsch_cfg.nbits[0].nof_bits*sizeof(uint8_t));
+
+  /*
+  int cb_len = q->pdsch_cfg.cb_segm[0].K1;
+  for (int i=0;i<q->pdsch_cfg.cb_segm[0].C;i++) {
+    snprintf(tmpstr,64,"output/rmout_%d_%d",i,tti);
+    srslte_bit_unpack_vector(softbuffer->buffer_b[i], q->tmp, (3*cb_len+12));
+    srslte_vec_save_file(tmpstr, q->tmp, (3*cb_len+12)*sizeof(uint8_t));
+  }*/
+
+  snprintf(tmpstr,64,"output/data_%d",tti);
+  srslte_bit_unpack_vector(data, q->tmp, q->pdsch_cfg.grant.mcs[0].tbs);
+  srslte_vec_save_file(tmpstr, q->tmp, q->pdsch_cfg.grant.mcs[0].tbs*sizeof(uint8_t));
+
+  //printf("Saved files for tti=%d, sf=%d, cfi=%d, mcs=%d, tbs=%d, rv=%d, rnti=0x%x\n", tti, tti%10, cfi,
+  //       q->pdsch_cfg.grant.mcs[0].idx, q->pdsch_cfg.grant.mcs[0].tbs, rv_idx, rnti);
+}
+
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index cd7aadb4c..583368b53 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -940,16 +940,16 @@ void srslte_ue_dl_save_signal(srslte_ue_dl_t *q, srslte_softbuffer_rx_t *softbuf
   srslte_vec_save_file("pdcch_llr", q->pdcch.llr, q->pdcch.nof_cce*72*sizeof(float));
 
   
-  srslte_vec_save_file("pdsch_symbols", q->pdsch.d, q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
-  srslte_vec_save_file("llr", q->pdsch.e, q->pdsch_cfg.nbits[0].nof_bits*sizeof(cf_t));
+  srslte_vec_save_file("pdsch_symbols", q->pdsch.d[0], q->pdsch_cfg.nbits[0].nof_re*sizeof(cf_t));
+  srslte_vec_save_file("llr", q->pdsch.e[0], q->pdsch_cfg.nbits[0].nof_bits*sizeof(cf_t));
   int cb_len = q->pdsch_cfg.cb_segm[0].K1;
   for (int i=0;i<q->pdsch_cfg.cb_segm[0].C;i++) {
     char tmpstr[64];
     snprintf(tmpstr,64,"rmout_%d.dat",i);
     srslte_vec_save_file(tmpstr, softbuffer->buffer_f[i], (3*cb_len+12)*sizeof(int16_t));  
   }
-  printf("Saved files for tti=%d, sf=%d, cfi=%d, mcs=%d, rv=%d, rnti=0x%x\n", tti, tti%10, cfi, 
-         q->pdsch_cfg.grant.mcs[0].idx, rv_idx, rnti);
+  printf("Saved files for tti=%d, sf=%d, cfi=%d, mcs=%d, tbs=%d, rv=%d, rnti=0x%x\n", tti, tti%10, cfi,
+         q->pdsch_cfg.grant.mcs[0].idx, q->pdsch_cfg.grant.mcs[0].tbs, rv_idx, rnti);
 }
 
 
diff --git a/srsenb/src/phy/phch_worker.cc b/srsenb/src/phy/phch_worker.cc
index ab7df91af..3a1fb8ca4 100644
--- a/srsenb/src/phy/phch_worker.cc
+++ b/srsenb/src/phy/phch_worker.cc
@@ -434,8 +434,6 @@ int phch_worker::decode_pusch(srslte_enb_ul_pusch_t *grants, uint32_t nof_pusch,
       ue_db[rnti].phich_info.n_prb_lowest = enb_ul.pusch_cfg.grant.n_prb_tilde[0];                                           
       ue_db[rnti].phich_info.n_dmrs       = phy_grant.ncs_dmrs;                                           
 
-      
-      
       char cqi_str[64];
       if (cqi_enabled) {
         srslte_cqi_value_unpack(uci_data.uci_cqi, &cqi_value);
@@ -689,10 +687,12 @@ int phch_worker::encode_pdsch(srslte_enb_dl_pdsch_t *grants, uint32_t nof_grants
                              rnti, phy_grant.nof_prb, grant_str, grants[i].grant.harq_process, 
                              phy_grant.mcs[0].tbs/8, phy_grant.mcs[0].idx, grants[i].grant.rv_idx, tti_tx);
       }
+
       srslte_softbuffer_tx_t *sb[SRSLTE_MAX_CODEWORDS] = {grants[i].softbuffer, NULL};
       uint8_t                 *d[SRSLTE_MAX_CODEWORDS] = {grants[i].data, NULL};
       int                     rv[SRSLTE_MAX_CODEWORDS] = {grants[i].grant.rv_idx, 0};
 
+
       if (srslte_enb_dl_put_pdsch(&enb_dl, &phy_grant, sb, rnti, rv, sf_idx, d, SRSLTE_MIMO_TYPE_SINGLE_ANTENNA, 0))
       {
         fprintf(stderr, "Error putting PDSCH %d\n",i);
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 40d4775d3..1c9abc1ca 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -617,8 +617,9 @@ int phch_worker::decode_pdsch(srslte_ra_dl_grant_t *grant, uint8_t *payload[SRSL
 
         for (int i=0;i<SRSLTE_MAX_CODEWORDS;i++) {
           if (grant->tb_en[i]) {
-            snprintf(tbstr[i], 128, ", TB%d: tbs=%d, mcs=%d, rv=%d, crc=%s",
-                     i, grant->mcs[i].tbs/8, grant->mcs[i].idx, rv[i], acks[i] ? "OK" : "KO");
+            snprintf(tbstr[i], 128, ", TB%d: tbs=%d, mcs=%d, rv=%d, crc=%s, it=%d",
+                     i, grant->mcs[i].tbs/8, grant->mcs[i].idx, rv[i], acks[i] ? "OK" : "KO",
+                     srslte_pdsch_last_noi_cw(&ue_dl.pdsch, i));
           }
         }
 

From b6f905e2dfeea009bd9bbf5ca307641ed80bad13 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 16:45:02 +0200
Subject: [PATCH 228/269] Handle RRC ConnectionReject at eNodeB and UE

---
 .../srslte/interfaces/enb_interfaces.h        |  3 +-
 lib/include/srslte/interfaces/ue_interfaces.h | 10 +++
 srsenb/hdr/upper/rrc.h                        |  3 +-
 srsenb/hdr/upper/s1ap.h                       |  3 +-
 srsenb/src/upper/rrc.cc                       | 30 +++++--
 srsenb/src/upper/s1ap.cc                      |  6 +-
 srsue/hdr/upper/nas.h                         |  6 ++
 srsue/hdr/upper/rrc.h                         |  8 +-
 srsue/src/upper/nas.cc                        | 41 ++++++---
 srsue/src/upper/rrc.cc                        | 86 +++++++++++++++----
 10 files changed, 154 insertions(+), 42 deletions(-)

diff --git a/lib/include/srslte/interfaces/enb_interfaces.h b/lib/include/srslte/interfaces/enb_interfaces.h
index ed2478896..d2cfbca08 100644
--- a/lib/include/srslte/interfaces/enb_interfaces.h
+++ b/lib/include/srslte/interfaces/enb_interfaces.h
@@ -259,7 +259,8 @@ public:
   virtual bool user_exists(uint16_t rnti) = 0; 
   virtual void user_inactivity(uint16_t rnti) = 0;
   virtual void release_eutran(uint16_t rnti) = 0; 
-  virtual bool user_link_lost(uint16_t rnti) = 0; 
+  virtual bool user_link_lost(uint16_t rnti) = 0;
+  virtual bool is_connected() = 0;
   virtual void ue_ctxt_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_INITIALCONTEXTSETUPRESPONSE_STRUCT *res) = 0;
   virtual void ue_erab_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_E_RABSETUPRESPONSE_STRUCT *res) = 0;
   // virtual void ue_capabilities(uint16_t rnti, LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *caps) = 0;
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 2c29bd4d4..770c44738 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -97,9 +97,19 @@ public:
 class nas_interface_rrc
 {
 public:
+
+  typedef enum {
+    BARRING_MT_ACCESS       = 1,
+    BARRING_MO_CALL         = 2,
+    BARRING_MO_SIGNALLING   = 4,
+    BARRING_ALL             = 7
+  } cell_barring_type_t;
+
   virtual bool      is_attached() = 0;
   virtual bool      is_attaching() = 0;
   virtual void      notify_connection_setup() = 0;
+  virtual void      notify_connection_failure() = 0;
+  virtual void      network_barring_state(bool is_barred, uint32_t type_mask = BARRING_ALL) = 0;
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
diff --git a/srsenb/hdr/upper/rrc.h b/srsenb/hdr/upper/rrc.h
index 49a02d0b9..29eb87e9f 100644
--- a/srsenb/hdr/upper/rrc.h
+++ b/srsenb/hdr/upper/rrc.h
@@ -172,7 +172,8 @@ public:
     rrc_state_t get_state();
     
     void send_connection_setup(bool is_setup = true);
-    void send_connection_reest(); 
+    void send_connection_rej();
+    void send_connection_reest();
     void send_connection_release();
     void send_connection_reest_rej(); 
     void send_connection_reconf(srslte::byte_buffer_t *sdu);
diff --git a/srsenb/hdr/upper/s1ap.h b/srsenb/hdr/upper/s1ap.h
index 02e5e207e..42c5cbb26 100644
--- a/srsenb/hdr/upper/s1ap.h
+++ b/srsenb/hdr/upper/s1ap.h
@@ -79,7 +79,8 @@ public:
   bool user_exists(uint16_t rnti); 
   void user_inactivity(uint16_t rnti);
   bool user_link_lost(uint16_t rnti);
-  void release_eutran(uint16_t rnti); 
+  void release_eutran(uint16_t rnti);
+  bool is_connected();
   void ue_ctxt_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_INITIALCONTEXTSETUPRESPONSE_STRUCT *res);
   void ue_erab_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_E_RABSETUPRESPONSE_STRUCT *res);
   //void ue_capabilities(uint16_t rnti, LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *caps);
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index f803500f2..21dce98af 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -833,14 +833,20 @@ void rrc::ue::parse_ul_dcch(uint32_t lcid, byte_buffer_t *pdu)
 void rrc::ue::handle_rrc_con_req(LIBLTE_RRC_CONNECTION_REQUEST_STRUCT *msg)
 {
   set_activity();
-  
-  if(msg->ue_id_type == LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI) {
-    mmec      = msg->ue_id.s_tmsi.mmec;
-    m_tmsi    = msg->ue_id.s_tmsi.m_tmsi;
-    has_tmsi = true;
+
+  if (parent->s1ap->is_connected()) {
+    if(msg->ue_id_type == LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI) {
+      mmec      = msg->ue_id.s_tmsi.mmec;
+      m_tmsi    = msg->ue_id.s_tmsi.m_tmsi;
+      has_tmsi = true;
+    }
+    send_connection_setup();
+    state = RRC_STATE_WAIT_FOR_CON_SETUP_COMPLETE;
+  } else {
+    parent->rrc_log->info("Received ConnectionRequest with no active S1 link\n");
+    parent->rrc_log->console("Received ConnectionRequest with no active S1 link\n");
+    send_connection_rej();
   }
-  send_connection_setup();
-  state = RRC_STATE_WAIT_FOR_CON_SETUP_COMPLETE;
 }
 
 void rrc::ue::handle_rrc_con_reest_req(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_REQUEST_STRUCT *msg)
@@ -1054,6 +1060,16 @@ void rrc::ue::send_connection_reest_rej()
   
 }
 
+void rrc::ue::send_connection_rej()
+{
+  LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
+  bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
+
+  dl_ccch_msg.msg_type = LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ;
+  dl_ccch_msg.msg.rrc_con_rej.wait_time = 10; 
+  send_dl_ccch(&dl_ccch_msg);
+}
+
 void rrc::ue::send_connection_setup(bool is_setup)
 {
   LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg; 
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index 44a564d09..ca6581189 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -170,6 +170,11 @@ void s1ap::build_tai_cgi()
 /*******************************************************************************
 /* RRC interface
 ********************************************************************************/
+
+bool s1ap::is_connected() {
+  return mme_connected;
+}
+
 void s1ap::initial_ue(uint16_t rnti, srslte::byte_buffer_t *pdu)
 {
   ue_ctxt_map[rnti].eNB_UE_S1AP_ID = next_eNB_UE_S1AP_ID++;
@@ -226,7 +231,6 @@ void s1ap::user_inactivity(uint16_t rnti)
   send_uectxtreleaserequest(rnti, &cause);
 }
 
-
 void s1ap::release_eutran(uint16_t rnti)
 {
   s1ap_log->info("Release by EUTRAN - RNTI:0x%x\n", rnti);
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 68d00ba06..1fa168305 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -80,6 +80,9 @@ public:
 
   // RRC interface
   void notify_connection_setup();
+  void notify_connection_failure();
+  void network_barring_state(bool is_barred, uint32_t type_mask);
+
 
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
 
@@ -135,6 +138,9 @@ private:
   uint8_t k_nas_enc[32];
   uint8_t k_nas_int[32];
 
+  bool network_is_barred;
+  uint32_t network_barring_type;
+
   srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
   srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 8dfa7f70d..136924af8 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -127,7 +127,7 @@ private:
   srslte::mac_interface_timers *mac_timers;
   uint32_t n310_cnt, N310;
   uint32_t n311_cnt, N311;
-  uint32_t t301, t310, t311;
+  uint32_t t300, t301, t302, t310, t311;
   int ue_category;
 
   typedef struct {
@@ -240,15 +240,15 @@ private:
   // Parsers
   void          parse_dl_ccch(byte_buffer_t *pdu);
   void          parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
-  void          parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
 
   // Helpers
+  void          timer_t300_expiry();
+  void          timer_barring_expiry(uint32_t timer_id);
   void          rrc_connection_release();
   void          radio_link_failure(); 
-  static void*  start_sib_thread(void *rrc_);
-  void          sib_search();
   void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
+  void          handle_con_rej(LIBLTE_RRC_CONNECTION_REJECT_STRUCT *reject);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
   void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
   void          add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index 91b35ce01..ead5e9d30 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -66,22 +66,27 @@ UE interface
 *******************************************************************************/
 void nas::attach_request() {
   nas_log->info("Attach Request\n");
-  if (state == EMM_STATE_DEREGISTERED) {
-    state = EMM_STATE_REGISTERED_INITIATED;
-    if (plmn_selection == PLMN_NOT_SELECTED) {
-      nas_log->info("Starting PLMN Search...\n");
-      rrc->plmn_search();
-    } else if (plmn_selection == PLMN_SELECTED) {
-      nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
+  if (!network_is_barred) {
+    if (state == EMM_STATE_DEREGISTERED) {
+      state = EMM_STATE_REGISTERED_INITIATED;
+      if (plmn_selection == PLMN_NOT_SELECTED) {
+        nas_log->info("Starting PLMN Search...\n");
+        rrc->plmn_search();
+      } else if (plmn_selection == PLMN_SELECTED) {
+        nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
+        rrc->plmn_select(current_plmn);
+        selecting_plmn = current_plmn;
+      }
+    } else if (state == EMM_STATE_REGISTERED) {
+      nas_log->info("NAS state is registered, connecting to same PLMN\n");
       rrc->plmn_select(current_plmn);
       selecting_plmn = current_plmn;
+    } else {
+      nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
     }
-  } else if (state == EMM_STATE_REGISTERED) {
-    nas_log->info("NAS state is registered, connecting to same PLMN\n");
-    rrc->plmn_select(current_plmn);
-    selecting_plmn = current_plmn;
   } else {
-    nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
+    nas_log->info("Requested connection while network is barred\n");
+    nas_log->console("Requested connection while network is barred\n");
   }
 }
 
@@ -94,6 +99,12 @@ void nas::deattach_request() {
 RRC interface
 *******************************************************************************/
 
+void nas::network_barring_state(bool is_barred, uint32_t type_mask)
+{
+  network_is_barred    = is_barred;
+  network_barring_type = type_mask;
+}
+
 void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
 
   // Store PLMN if not registered
@@ -125,6 +136,12 @@ bool nas::is_attaching() {
   return state == EMM_STATE_REGISTERED_INITIATED;
 }
 
+void nas::notify_connection_failure() {
+  nas_log->info("Received RRC connection request failure\n");
+  nas_log->console("Connection Request Failure\n");
+  state = EMM_STATE_DEREGISTERED;
+}
+
 void nas::notify_connection_setup() {
   nas_log->debug("State = %s\n", emm_state_text[state]);
   if (EMM_STATE_REGISTERED_INITIATED == state) {
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 3bb3e0017..529d99f8f 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -93,7 +93,9 @@ void rrc::init(phy_interface_rrc *phy_,
   pthread_mutex_init(&mutex, NULL);
 
   ue_category = SRSLTE_UE_CATEGORY;
+  t300 = mac_timers->timer_get_unique_id();
   t301 = mac_timers->timer_get_unique_id();
+  t302 = mac_timers->timer_get_unique_id();
   t310 = mac_timers->timer_get_unique_id();
   t311 = mac_timers->timer_get_unique_id();
 
@@ -538,14 +540,20 @@ void rrc::max_retx_attempted() {
 }
 
 void rrc::timer_expired(uint32_t timeout_id) {
-  if (timeout_id == t310) {
+  if (timeout_id == t300) {
+    rrc_log->info("Timer T300 expired: ConnectionRequest timeout\n");
+    timer_t300_expiry();
+  } else if (timeout_id == t301) {
+    rrc_log->info("Timer T301 expired: ConnectionReestablishmentRequest timeout\n");
+    state = RRC_STATE_LEAVE_CONNECTED;
+  } else if (timeout_id == t302) {
+    rrc_log->info("Timer T302 expired: ConnectionReject Wait time expired\n");
+    timer_barring_expiry(timeout_id);
+  } else if (timeout_id == t310) {
     rrc_log->info("Timer T310 expired: Radio Link Failure\n");
     radio_link_failure();
   } else if (timeout_id == t311) {
-    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
-    state = RRC_STATE_LEAVE_CONNECTED;
-  } else if (timeout_id == t301) {
-    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
+    rrc_log->info("Timer T311 expired: Start of Reestablishment procedure timeout\n");
     state = RRC_STATE_LEAVE_CONNECTED;
   } else {
     rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
@@ -608,6 +616,10 @@ void rrc::send_con_request() {
 
   mac->set_contention_id(uecri);
 
+  // Start t300 timer
+  mac_timers->timer_get(t300)->reset();
+  mac_timers->timer_get(t300)->run();
+
   rrc_log->info("Sending RRC Connection Request on SRB0\n");
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
@@ -750,7 +762,6 @@ void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
   pdcp_buf->N_bytes = bit_buf.N_bits / 8;
   pdcp_buf->set_timestamp();
 
-  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Setup Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
@@ -857,13 +868,25 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
   }
 }
 
-  /* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
+/* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
 void rrc::rrc_connection_release() {
   // Save idleModeMobilityControlInfo, etc.
   state = RRC_STATE_LEAVE_CONNECTED;
   rrc_log->console("Received RRC Connection Release\n");
 }
 
+// Actions upon expiry of t300 timer 5.3.3.6
+void rrc::timer_t300_expiry() {
+  mac->reset();
+  set_mac_default();
+  nas->notify_connection_failure();
+  state = RRC_STATE_IDLE;
+}
+
+// Actions upon expiry of timers related to cell barring 5.3.3.7
+void rrc::timer_barring_expiry(uint32_t timer_id) {
+  nas->network_barring_state(false);
+}
 
 
 
@@ -1082,20 +1105,14 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
 
   switch (dl_ccch_msg.msg_type) {
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
-      rrc_log->info("Connection Reject received. Wait time: %d\n",
-                    dl_ccch_msg.msg.rrc_con_rej.wait_time);
-      state = RRC_STATE_IDLE;
+      transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
+      handle_con_rej(&dl_ccch_msg.msg.rrc_con_rej);
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
-      rrc_log->info("Connection Setup received\n");
       transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
       handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
-      rrc_log->info("Notifying NAS of connection setup\n");
-      nas->notify_connection_setup();
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
-      rrc_log->info("Connection Reestablishment received\n");
-      rrc_log->console("Reestablishment OK\n");
       transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
       handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
       break;
@@ -1342,9 +1359,11 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
                 liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
                 sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
 
+  mac_timers->timer_get(t300)->set(this, liblte_rrc_t300_num[sib2->ue_timers_and_constants.t300]);
   mac_timers->timer_get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
   mac_timers->timer_get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
   mac_timers->timer_get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
+
   N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
   N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
 
@@ -1581,13 +1600,50 @@ void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg)
   }
 }
 
+// 5.3.3.4
 void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
+  rrc_log->info("Connection Setup received\n");
+
   // Apply the Radio Resource configuration
   apply_rr_config_dedicated(&setup->rr_cnfg);
+
+  // stop timers
+  mac_timers->timer_get(t300)->stop();
+
+  // enter RRC CONNECTED
+  state = RRC_STATE_CONNECTED;
+
+  rrc_log->info("Notifying NAS of connection setup\n");
+  nas->notify_connection_setup();
+
+}
+
+// 5.3.3.8
+void rrc::handle_con_rej(LIBLTE_RRC_CONNECTION_REJECT_STRUCT *reject) {
+  rrc_log->info("Connection Reject received. Wait time: %d s\n",
+                dl_ccch_msg.msg.rrc_con_rej.wait_time);
+
+  rrc_log->console("Received Connection Reject. S1 link may not be active at the eNodeB\n");
+
+  mac_timers->timer_get(t300)->stop();
+  mac->reset();
+  set_mac_default();
+
+  mac_timers->timer_get(t302)->set(this, dl_ccch_msg.msg.rrc_con_rej.wait_time*1000);
+  mac_timers->timer_get(t302)->run();
+
+  nas->notify_connection_failure();
+  nas->network_barring_state(true);
+
+  state = RRC_STATE_IDLE;
 }
 
 /* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
 void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
+  rrc_log->info("Connection Reestablishment received\n");
+  rrc_log->console("Reestablishment OK\n");
+
+
   mac_timers->timer_get(t301)->stop();
 
   // TODO: Restablish DRB1. Not done because never was suspended

From 23179262c3d031587d1b6700589f3ace63387404 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 16:59:29 +0200
Subject: [PATCH 229/269] Set waiting time on Reject

---
 srsenb/src/upper/rrc.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 21dce98af..de34ab136 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -1066,7 +1066,7 @@ void rrc::ue::send_connection_rej()
   bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
 
   dl_ccch_msg.msg_type = LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ;
-  dl_ccch_msg.msg.rrc_con_rej.wait_time = 10; 
+  dl_ccch_msg.msg.rrc_con_rej.wait_time = 10;
   send_dl_ccch(&dl_ccch_msg);
 }
 

From db17c67194e32636e3212dd117101d768ac73998 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 17:26:29 +0200
Subject: [PATCH 230/269] Fixed bug in SSE turbo decoder

---
 lib/include/srslte/phy/fec/turbodecoder.h     | 12 ++++----
 .../srslte/phy/fec/turbodecoder_simd.h        | 30 ++++++++++---------
 .../srslte/phy/fec/turbodecoder_simd_inter.h  | 16 +++++-----
 lib/src/phy/fec/test/turbodecoder_test.c      | 10 +++----
 lib/src/phy/fec/turbodecoder.c                | 30 ++++++++++++-------
 lib/src/phy/fec/turbodecoder_avx.c            |  2 +-
 lib/src/phy/fec/turbodecoder_simd.c           | 30 +++++++++----------
 lib/src/phy/fec/turbodecoder_simd_inter.c     |  8 ++---
 lib/src/phy/phch/pdsch.c                      |  3 +-
 lib/src/phy/phch/sch.c                        | 21 ++++++++-----
 lib/src/phy/phch/test/pusch_test.c            |  2 +-
 11 files changed, 90 insertions(+), 74 deletions(-)

diff --git a/lib/include/srslte/phy/fec/turbodecoder.h b/lib/include/srslte/phy/fec/turbodecoder.h
index 4a24804fa..ac05422bc 100644
--- a/lib/include/srslte/phy/fec/turbodecoder.h
+++ b/lib/include/srslte/phy/fec/turbodecoder.h
@@ -74,7 +74,7 @@ SRSLTE_API int srslte_tdec_reset_cb(srslte_tdec_t * h,
 SRSLTE_API int srslte_tdec_get_nof_iterations_cb(srslte_tdec_t * h, 
                                                  uint32_t cb_idx);
 
-SRSLTE_API int srslte_tdec_get_nof_parallel(srslte_tdec_t * h); 
+SRSLTE_API uint32_t srslte_tdec_get_nof_parallel(srslte_tdec_t * h);
 
 SRSLTE_API void srslte_tdec_iteration(srslte_tdec_t * h, 
                                       int16_t* input, 
@@ -95,15 +95,15 @@ SRSLTE_API int srslte_tdec_run_all(srslte_tdec_t * h,
                                    uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_iteration_par(srslte_tdec_t * h, 
-                                          int16_t* input[SRSLTE_TDEC_NPAR], 
+                                          int16_t* input[SRSLTE_TDEC_MAX_NPAR],
                                           uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_decision_par(srslte_tdec_t * h, 
-                                         uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                         uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                          uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_decision_byte_par(srslte_tdec_t * h, 
-                                              uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                              uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                               uint32_t long_cb); 
 
 SRSLTE_API void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, 
@@ -112,8 +112,8 @@ SRSLTE_API void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h,
                                                  uint32_t long_cb);
 
 SRSLTE_API int srslte_tdec_run_all_par(srslte_tdec_t * h, 
-                                       int16_t * input[SRSLTE_TDEC_NPAR], 
-                                       uint8_t *output[SRSLTE_TDEC_NPAR],
+                                       int16_t * input[SRSLTE_TDEC_MAX_NPAR],
+                                       uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                        uint32_t nof_iterations, 
                                        uint32_t long_cb);
 
diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd.h b/lib/include/srslte/phy/fec/turbodecoder_simd.h
index 8cafbc5f8..a899a5964 100644
--- a/lib/include/srslte/phy/fec/turbodecoder_simd.h
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd.h
@@ -44,7 +44,7 @@
 #include "srslte/phy/fec/cbsegm.h"
 
 // Define maximum number of CB decoded in parallel (2 for AVX2)
-#define SRSLTE_TDEC_NPAR 2
+#define SRSLTE_TDEC_MAX_NPAR 2
 
 #define SRSLTE_TCOD_RATE 3
 #define SRSLTE_TCOD_TOTALTAIL 12
@@ -65,18 +65,18 @@ typedef struct SRSLTE_API {
   
   map_gen_t dec;
 
-  int16_t *app1[SRSLTE_TDEC_NPAR];
-  int16_t *app2[SRSLTE_TDEC_NPAR];
-  int16_t *ext1[SRSLTE_TDEC_NPAR];
-  int16_t *ext2[SRSLTE_TDEC_NPAR];
-  int16_t *syst[SRSLTE_TDEC_NPAR];
-  int16_t *parity0[SRSLTE_TDEC_NPAR];
-  int16_t *parity1[SRSLTE_TDEC_NPAR];
+  int16_t *app1[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *app2[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *ext1[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *ext2[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *syst[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *parity0[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *parity1[SRSLTE_TDEC_MAX_NPAR];
   
   int cb_mask; 
   int current_cbidx; 
   srslte_tc_interl_t interleaver[SRSLTE_NOF_TC_CB_SIZES];
-  int n_iter[SRSLTE_TDEC_NPAR];
+  int n_iter[SRSLTE_TDEC_MAX_NPAR];
 } srslte_tdec_simd_t;
 
 SRSLTE_API int srslte_tdec_simd_init(srslte_tdec_simd_t * h, 
@@ -88,6 +88,8 @@ SRSLTE_API void srslte_tdec_simd_free(srslte_tdec_simd_t * h);
 SRSLTE_API int srslte_tdec_simd_reset(srslte_tdec_simd_t * h, 
                                       uint32_t long_cb);
 
+SRSLTE_API
+
 SRSLTE_API int srslte_tdec_simd_get_nof_iterations_cb(srslte_tdec_simd_t * h, 
                                                       uint32_t cb_idx);
 
@@ -95,15 +97,15 @@ SRSLTE_API int srslte_tdec_simd_reset_cb(srslte_tdec_simd_t * h,
                                          uint32_t cb_idx);
 
 SRSLTE_API void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, 
-                                           int16_t * input[SRSLTE_TDEC_NPAR], 
+                                           int16_t * input[SRSLTE_TDEC_MAX_NPAR],
                                            uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, 
-                                          uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                          uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                           uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, 
-                                               uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                               uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                                uint32_t long_cb); 
 
 SRSLTE_API void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h, 
@@ -112,8 +114,8 @@ SRSLTE_API void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h,
                                                   uint32_t long_cb); 
 
 SRSLTE_API int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, 
-                                        int16_t * input[SRSLTE_TDEC_NPAR], 
-                                        uint8_t *output[SRSLTE_TDEC_NPAR],
+                                        int16_t * input[SRSLTE_TDEC_MAX_NPAR],
+                                        uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                         uint32_t nof_iterations, 
                                         uint32_t long_cb);
 
diff --git a/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h b/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
index 402faa314..c80785809 100644
--- a/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
+++ b/lib/include/srslte/phy/fec/turbodecoder_simd_inter.h
@@ -49,9 +49,9 @@
 #include "srslte/phy/fec/cbsegm.h"
 
 #if LV_HAVE_AVX2
-  #define SRSLTE_TDEC_NPAR 16
+  #define SRSLTE_TDEC_MAX_NPAR 16
 #else
-  #define SRSLTE_TDEC_NPAR 8
+  #define SRSLTE_TDEC_MAX_NPAR 8
 #endif
 
 
@@ -71,7 +71,7 @@ typedef struct SRSLTE_API {
   int current_cbidx; 
   uint32_t current_long_cb;
   srslte_tc_interl_t interleaver[SRSLTE_NOF_TC_CB_SIZES];
-  int n_iter[SRSLTE_TDEC_NPAR];
+  int n_iter[SRSLTE_TDEC_MAX_NPAR];
 } srslte_tdec_simd_inter_t;
 
 SRSLTE_API int srslte_tdec_simd_inter_init(srslte_tdec_simd_inter_t * h, 
@@ -90,17 +90,17 @@ SRSLTE_API int srslte_tdec_simd_inter_reset_cb(srslte_tdec_simd_inter_t * h,
                                          uint32_t cb_idx);
 
 SRSLTE_API void srslte_tdec_simd_inter_iteration(srslte_tdec_simd_inter_t * h, 
-                                           int16_t * input[SRSLTE_TDEC_NPAR], 
+                                           int16_t * input[SRSLTE_TDEC_MAX_NPAR],
                                            uint32_t nof_cb,
                                            uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_simd_inter_decision(srslte_tdec_simd_inter_t * h, 
-                                          uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                          uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                           uint32_t nof_cb,
                                           uint32_t long_cb);
 
 SRSLTE_API void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, 
-                                               uint8_t *output[SRSLTE_TDEC_NPAR], 
+                                               uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                                uint32_t nof_cb,
                                                uint32_t long_cb); 
 
@@ -110,8 +110,8 @@ SRSLTE_API void srslte_tdec_simd_inter_decision_byte_cb(srslte_tdec_simd_inter_t
                                                   uint32_t long_cb); 
 
 SRSLTE_API int srslte_tdec_simd_inter_run_all(srslte_tdec_simd_inter_t * h, 
-                                        int16_t *input[SRSLTE_TDEC_NPAR], 
-                                        uint8_t *output[SRSLTE_TDEC_NPAR],
+                                        int16_t *input[SRSLTE_TDEC_MAX_NPAR],
+                                        uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                         uint32_t nof_iterations, 
                                         uint32_t nof_cb,
                                         uint32_t long_cb);
diff --git a/lib/src/phy/fec/test/turbodecoder_test.c b/lib/src/phy/fec/test/turbodecoder_test.c
index df1d4b884..aeac21433 100644
--- a/lib/src/phy/fec/test/turbodecoder_test.c
+++ b/lib/src/phy/fec/test/turbodecoder_test.c
@@ -117,7 +117,7 @@ int main(int argc, char **argv) {
   float *llr;
   short *llr_s;
   uint8_t *llr_c;
-  uint8_t *data_tx, *data_rx, *data_rx_bytes[SRSLTE_TDEC_NPAR], *symbols;
+  uint8_t *data_tx, *data_rx, *data_rx_bytes[SRSLTE_TDEC_MAX_NPAR], *symbols;
   uint32_t i, j;
   float var[SNR_POINTS];
   uint32_t snr_points;
@@ -159,7 +159,7 @@ int main(int argc, char **argv) {
     perror("malloc");
     exit(-1);
   }
-  for (int cb=0;cb<SRSLTE_TDEC_NPAR;cb++) {
+  for (int cb=0;cb<SRSLTE_TDEC_MAX_NPAR;cb++) {
     data_rx_bytes[cb] = srslte_vec_malloc(frame_length * sizeof(uint8_t));
     if (!data_rx_bytes[cb]) {
       perror("malloc");
@@ -254,10 +254,10 @@ int main(int argc, char **argv) {
         t = nof_iterations;
       }
 
-      int16_t *input[SRSLTE_TDEC_NPAR];
-      uint8_t *output[SRSLTE_TDEC_NPAR];        
+      int16_t *input[SRSLTE_TDEC_MAX_NPAR];
+      uint8_t *output[SRSLTE_TDEC_MAX_NPAR];
       
-      for (int n=0;n<SRSLTE_TDEC_NPAR;n++) {
+      for (int n=0;n<SRSLTE_TDEC_MAX_NPAR;n++) {
         if (n < nof_cb) {
           input[n] = llr_s;         
         } else {
diff --git a/lib/src/phy/fec/turbodecoder.c b/lib/src/phy/fec/turbodecoder.c
index b19466a53..7803fde1b 100644
--- a/lib/src/phy/fec/turbodecoder.c
+++ b/lib/src/phy/fec/turbodecoder.c
@@ -43,7 +43,7 @@
 
 int srslte_tdec_init(srslte_tdec_t * h, uint32_t max_long_cb) {
 #ifdef LV_HAVE_SSE
-  return srslte_tdec_simd_init(&h->tdec_simd, SRSLTE_TDEC_NPAR, max_long_cb);
+  return srslte_tdec_simd_init(&h->tdec_simd, SRSLTE_TDEC_MAX_NPAR, max_long_cb);
 #else
   h->input_conv = srslte_vec_malloc(sizeof(float) * (3*max_long_cb+12));
   if (!h->input_conv) {
@@ -91,7 +91,7 @@ int srslte_tdec_get_nof_iterations_cb(srslte_tdec_t * h, uint32_t cb_idx)
 #endif  
 }
 
-void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
+void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
   srslte_tdec_simd_iteration(&h->tdec_simd, input, long_cb);      
 #else
@@ -101,12 +101,12 @@ void srslte_tdec_iteration_par(srslte_tdec_t * h, int16_t* input[SRSLTE_TDEC_NPA
 }
 
 void srslte_tdec_iteration(srslte_tdec_t * h, int16_t* input, uint32_t long_cb) {
-  int16_t *input_par[SRSLTE_TDEC_NPAR]; 
+  int16_t *input_par[SRSLTE_TDEC_MAX_NPAR];
   input_par[0] = input; 
   return srslte_tdec_iteration_par(h, input_par, long_cb);
 }
 
-void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
+void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
   return srslte_tdec_simd_decision(&h->tdec_simd, output, long_cb);
 #else
@@ -114,13 +114,21 @@ void srslte_tdec_decision_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPA
 #endif
 }
 
+uint32_t srslte_tdec_get_nof_parallel(srslte_tdec_t *h) {
+#ifdef LV_HAVE_AVX2
+  return 2;
+#else
+  return 1;
+#endif
+}
+
 void srslte_tdec_decision(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
-  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  uint8_t *output_par[SRSLTE_TDEC_MAX_NPAR];
   output_par[0] = output; 
   srslte_tdec_decision_par(h, output_par, long_cb);
 }
 
-void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb) {
+void srslte_tdec_decision_byte_par(srslte_tdec_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
   srslte_tdec_simd_decision_byte(&h->tdec_simd, output, long_cb);  
 #else
@@ -137,13 +145,13 @@ void srslte_tdec_decision_byte_par_cb(srslte_tdec_t * h, uint8_t *output, uint32
 }
 
 void srslte_tdec_decision_byte(srslte_tdec_t * h, uint8_t *output, uint32_t long_cb) {
-  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  uint8_t *output_par[SRSLTE_TDEC_MAX_NPAR];
   output_par[0] = output; 
   srslte_tdec_decision_byte_par(h, output_par, long_cb);
 }
 
-int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_NPAR], 
-                            uint8_t *output[SRSLTE_TDEC_NPAR], 
+int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_MAX_NPAR],
+                            uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                             uint32_t nof_iterations, uint32_t long_cb) {
 #ifdef LV_HAVE_SSE
   return srslte_tdec_simd_run_all(&h->tdec_simd, input, output, nof_iterations, long_cb);  
@@ -155,9 +163,9 @@ int srslte_tdec_run_all_par(srslte_tdec_t * h, int16_t * input[SRSLTE_TDEC_NPAR]
 
 int srslte_tdec_run_all(srslte_tdec_t * h, int16_t * input, uint8_t *output, uint32_t nof_iterations, uint32_t long_cb)
 {
-  uint8_t *output_par[SRSLTE_TDEC_NPAR]; 
+  uint8_t *output_par[SRSLTE_TDEC_MAX_NPAR];
   output_par[0] = output;   
-  int16_t *input_par[SRSLTE_TDEC_NPAR]; 
+  int16_t *input_par[SRSLTE_TDEC_MAX_NPAR];
   input_par[0] = input; 
  
   return srslte_tdec_run_all_par(h, input_par, output_par, nof_iterations, long_cb);
diff --git a/lib/src/phy/fec/turbodecoder_avx.c b/lib/src/phy/fec/turbodecoder_avx.c
index 2a2f6f925..2e877cbde 100644
--- a/lib/src/phy/fec/turbodecoder_avx.c
+++ b/lib/src/phy/fec/turbodecoder_avx.c
@@ -81,7 +81,7 @@ static inline int16_t hMax1(__m256i masked_value)
 }
 
 /* Computes beta values */
-void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
+void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb)
 {
   int k;
   uint32_t end = long_cb + 3;
diff --git a/lib/src/phy/fec/turbodecoder_simd.c b/lib/src/phy/fec/turbodecoder_simd.c
index e245c84a4..a32d52962 100644
--- a/lib/src/phy/fec/turbodecoder_simd.c
+++ b/lib/src/phy/fec/turbodecoder_simd.c
@@ -54,13 +54,13 @@ void map_sse_alpha(map_gen_t * s, uint32_t long_cb);
 void map_sse_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t long_cb);
 
 #ifdef LV_HAVE_AVX2
-void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t long_cb);
+void map_avx_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb);
 void map_avx_alpha(map_gen_t * s, uint32_t long_cb);
 void map_avx_gamma(map_gen_t * h, int16_t *input, int16_t *app, int16_t *parity, uint32_t cbidx, uint32_t long_cb);
 #endif
 
 
-void map_simd_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void map_simd_beta(map_gen_t * s, int16_t * output[SRSLTE_TDEC_MAX_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
   if (nof_cb == 1) {
     map_sse_beta(s, output[0], long_cb);
@@ -128,12 +128,12 @@ void map_simd_free(map_gen_t * h)
 }
 
 /* Runs one instance of a decoder */
-void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_NPAR], int16_t *app[SRSLTE_TDEC_NPAR], int16_t * parity[SRSLTE_TDEC_NPAR], 
-                  int16_t *output[SRSLTE_TDEC_NPAR], uint32_t cb_mask, uint32_t long_cb)
+void map_simd_dec(map_gen_t * h, int16_t * input[SRSLTE_TDEC_MAX_NPAR], int16_t *app[SRSLTE_TDEC_MAX_NPAR], int16_t * parity[SRSLTE_TDEC_MAX_NPAR],
+                  int16_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t cb_mask, uint32_t long_cb)
 {
   
   uint32_t nof_cb = 1;
-  int16_t *outptr[SRSLTE_TDEC_NPAR];
+  int16_t *outptr[SRSLTE_TDEC_MAX_NPAR];
   
   // Compute branch metrics
   switch(cb_mask) {
@@ -354,21 +354,21 @@ void deinterleave_input_simd(srslte_tdec_simd_t *h, int16_t *input, uint32_t cbi
 }
 
 /* Runs 1 turbo decoder iteration */
-void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint32_t long_cb)
+void srslte_tdec_simd_iteration(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb)
 {
 
-  int16_t *tmp_app[SRSLTE_TDEC_NPAR];
+  int16_t *tmp_app[SRSLTE_TDEC_MAX_NPAR];
 
   if (h->current_cbidx >= 0) {
     uint16_t *inter   = h->interleaver[h->current_cbidx].forward;
     uint16_t *deinter = h->interleaver[h->current_cbidx].reverse;
     
-#if SRSLTE_TDEC_NPAR == 2
-    h->cb_mask = (input[0]?1:0) | (input[1]?2:0);
-#else
-    h->cb_mask = input[0]?1:0;
+#ifndef LV_HAVE_AVX2
+    input[1] = NULL;
 #endif
-    
+
+    h->cb_mask = (input[0]?1:0) | (input[1]?2:0);
+
     for (int i=0;i<h->max_par_cb;i++) {
       if (h->n_iter[i] == 0 && input[i]) {
         //printf("deinterleaveing %d\n",i);
@@ -484,7 +484,7 @@ void tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output, uint32_t cbidx,
   }
 }
 
-void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
+void srslte_tdec_simd_decision(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb)
 {
   for (int i=0;i<h->max_par_cb;i++) {    
     tdec_simd_decision(h, output[i], i, long_cb);
@@ -510,7 +510,7 @@ void srslte_tdec_simd_decision_byte_cb(srslte_tdec_simd_t * h, uint8_t *output,
   }
 }
 
-void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t long_cb)
+void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t long_cb)
 {
   for (int i=0;i<h->max_par_cb;i++) {
     srslte_tdec_simd_decision_byte_cb(h, output[i], i, long_cb);
@@ -519,7 +519,7 @@ void srslte_tdec_simd_decision_byte(srslte_tdec_simd_t * h, uint8_t *output[SRSL
 
 
 /* Runs nof_iterations iterations and decides the output bits */
-int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_NPAR], uint8_t *output[SRSLTE_TDEC_NPAR],
+int srslte_tdec_simd_run_all(srslte_tdec_simd_t * h, int16_t * input[SRSLTE_TDEC_MAX_NPAR], uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                             uint32_t nof_iterations, uint32_t long_cb)
 {
   if (srslte_tdec_simd_reset(h, long_cb)) {
diff --git a/lib/src/phy/fec/turbodecoder_simd_inter.c b/lib/src/phy/fec/turbodecoder_simd_inter.c
index 05d8b2cf5..3c04e2136 100644
--- a/lib/src/phy/fec/turbodecoder_simd_inter.c
+++ b/lib/src/phy/fec/turbodecoder_simd_inter.c
@@ -172,7 +172,7 @@ void extract_input(srslte_tdec_simd_inter_t *h, int16_t *input, uint32_t cbidx,
   }
 }
 
-void srslte_tdec_simd_inter_iteration(srslte_tdec_simd_inter_t * h, int16_t *input[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_inter_iteration(srslte_tdec_simd_inter_t * h, int16_t *input[SRSLTE_TDEC_MAX_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
 
   if (h->current_cbidx >= 0) {
@@ -239,7 +239,7 @@ void srslte_tdec_simd_inter_decision_cb(srslte_tdec_simd_inter_t * h, uint8_t *o
   }
 }
 
-void srslte_tdec_simd_inter_decision(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_inter_decision(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
   for (int i=0;i<nof_cb;i++) {
     srslte_tdec_simd_inter_decision_cb(h, output[i], i, long_cb);
@@ -269,7 +269,7 @@ void srslte_tdec_simd_inter_decision_byte_cb(srslte_tdec_simd_inter_t * h, uint8
   }
 }
 
-void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_NPAR], uint32_t nof_cb, uint32_t long_cb)
+void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, uint8_t *output[SRSLTE_TDEC_MAX_NPAR], uint32_t nof_cb, uint32_t long_cb)
 {
   for (int i=0;i<nof_cb;i++) {
     srslte_tdec_simd_inter_decision_byte_cb(h, output[i], i, long_cb);
@@ -277,7 +277,7 @@ void srslte_tdec_simd_inter_decision_byte(srslte_tdec_simd_inter_t * h, uint8_t
 }
 
 int srslte_tdec_simd_inter_run_all(srslte_tdec_simd_inter_t * h, 
-                                  int16_t *input[SRSLTE_TDEC_NPAR], uint8_t *output[SRSLTE_TDEC_NPAR],
+                                  int16_t *input[SRSLTE_TDEC_MAX_NPAR], uint8_t *output[SRSLTE_TDEC_MAX_NPAR],
                                   uint32_t nof_iterations, uint32_t nof_cb, uint32_t long_cb)
 {
   uint32_t iter = 0;
diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index bdb0a755a..63bfa222d 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -600,6 +600,8 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
     /* Bit scrambling */
     srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
 
+    printf("e: "); srslte_vec_fprint_s(stdout, q->e[codeword_idx], 10);
+
     /* Return  */
     ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
 
@@ -685,7 +687,6 @@ int srslte_pdsch_decode(srslte_pdsch_t *q,
         srslte_layerdemap_type(x, q->d, cfg->nof_layers, nof_tb,
                              nof_symbols[0], nof_symbols, cfg->mimo_type);
     }
-
     // Codeword decoding
     for (uint32_t tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb ++) {
       /* Decode only if transport block is enabled and the default ACK is not true */
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index b7909192f..3921c8562 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -310,8 +310,8 @@ bool decode_tb_cb(srslte_sch_t *q,
 
   bool cb_map[SRSLTE_MAX_CODEBLOCKS];
     
-  uint32_t cb_idx[SRSLTE_TDEC_NPAR];
-  int16_t *decoder_input[SRSLTE_TDEC_NPAR];
+  uint32_t cb_idx[SRSLTE_TDEC_MAX_NPAR];
+  int16_t *decoder_input[SRSLTE_TDEC_MAX_NPAR];
   
   uint32_t nof_cb     = cb_size_group?cb_segm->C2:cb_segm->C1;
   uint32_t first_cb   = cb_size_group?cb_segm->C1:0;
@@ -328,7 +328,7 @@ bool decode_tb_cb(srslte_sch_t *q,
     return false; 
   }
   
-  for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+  for (int i=0;i<srslte_tdec_get_nof_parallel(&q->decoder);i++) {
     cb_idx[i]        = i+first_cb; 
     decoder_input[i] = NULL;
   }
@@ -346,7 +346,7 @@ bool decode_tb_cb(srslte_sch_t *q,
   while(remaining_cb>0) {
         
     // Unratematch the codeblocks left to decode 
-    for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+    for (int i=0;i<srslte_tdec_get_nof_parallel(&q->decoder);i++) {
       
       if (!decoder_input[i] && remaining_cb > 0) {        
         // Find an unprocessed CB 
@@ -364,14 +364,17 @@ bool decode_tb_cb(srslte_sch_t *q,
             n_e2 = n_e+Qm;
             rp   = (cb_segm->C - gamma)*n_e + (cb_idx[i]-(cb_segm->C - gamma))*n_e2;
           }
-                
+
           INFO("CB %d: rp=%d, n_e=%d, i=%d\n", cb_idx[i], rp, n_e2, i);
           if (srslte_rm_turbo_rx_lut(&e_bits[rp], softbuffer->buffer_f[cb_idx[i]], n_e2, cb_len_idx, rv)) {
             fprintf(stderr, "Error in rate matching\n");
             return SRSLTE_ERROR;
           }
 
-          decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];          
+          decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
+
+          printf("input: "); srslte_vec_fprint_s(stdout, decoder_input[i], 10);
+
         }
       }
     }
@@ -380,7 +383,7 @@ bool decode_tb_cb(srslte_sch_t *q,
     srslte_tdec_iteration_par(&q->decoder, decoder_input, cb_len);
 
     // Decide output bits and compute CRC 
-    for (int i=0;i<SRSLTE_TDEC_NPAR;i++) {
+    for (int i=0;i<srslte_tdec_get_nof_parallel(&q->decoder);i++) {
       if (decoder_input[i]) {        
         srslte_tdec_decision_byte_par_cb(&q->decoder, q->cb_in, i, cb_len);
 
@@ -395,6 +398,8 @@ bool decode_tb_cb(srslte_sch_t *q,
           crc_ptr = &q->crc_tb; 
         }
 
+        printf("output: %d", i); srslte_vec_fprint_b(stdout, q->cb_in, 10);
+
         // CRC is OK
         if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
 
@@ -474,7 +479,7 @@ static int decode_tb(srslte_sch_t *q,
     data[cb_segm->tbs/8+1] = 0; 
     data[cb_segm->tbs/8+2] = 0; 
     
-    // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_NPAR
+    // Process Codeblocks in groups of equal CB size to parallelize according to SRSLTE_TDEC_MAX_NPAR
     for (uint32_t i=0;i<nof_cb_groups && crc_ok;i++) {
       crc_ok = decode_tb_cb(q, softbuffer, cb_segm, Qm, rv, nof_e_bits, e_bits, data, i);            
     }
diff --git a/lib/src/phy/phch/test/pusch_test.c b/lib/src/phy/phch/test/pusch_test.c
index ece098dcf..9d048e680 100644
--- a/lib/src/phy/phch/test/pusch_test.c
+++ b/lib/src/phy/phch/test/pusch_test.c
@@ -190,7 +190,7 @@ int main(int argc, char **argv) {
   bzero(&uci_data_tx, sizeof(srslte_uci_data_t));
   uci_data_tx.uci_cqi_len = 4; 
   uci_data_tx.uci_ri_len = 0; 
-  uci_data_tx.uci_ack_len = 2;
+  uci_data_tx.uci_ack_len = 1;
   memcpy(&uci_data_rx, &uci_data_tx, sizeof(srslte_uci_data_t));
     
   for (uint32_t i=0;i<20;i++) {

From fbae23b2acd5e023db4c5131d1890ea92db016b6 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 17:30:35 +0200
Subject: [PATCH 231/269] Removed debugging printf

---
 lib/src/phy/phch/pdsch.c | 2 --
 lib/src/phy/phch/sch.c   | 5 -----
 2 files changed, 7 deletions(-)

diff --git a/lib/src/phy/phch/pdsch.c b/lib/src/phy/phch/pdsch.c
index 63bfa222d..b438a2957 100644
--- a/lib/src/phy/phch/pdsch.c
+++ b/lib/src/phy/phch/pdsch.c
@@ -600,8 +600,6 @@ static int srslte_pdsch_codeword_decode(srslte_pdsch_t *q, srslte_pdsch_cfg_t *c
     /* Bit scrambling */
     srslte_scrambling_s_offset(seq, q->e[codeword_idx], 0, nbits->nof_bits);
 
-    printf("e: "); srslte_vec_fprint_s(stdout, q->e[codeword_idx], 10);
-
     /* Return  */
     ret = srslte_dlsch_decode2(&q->dl_sch, cfg, softbuffer, q->e[codeword_idx], data, codeword_idx);
 
diff --git a/lib/src/phy/phch/sch.c b/lib/src/phy/phch/sch.c
index 3921c8562..e76fa6aef 100644
--- a/lib/src/phy/phch/sch.c
+++ b/lib/src/phy/phch/sch.c
@@ -372,9 +372,6 @@ bool decode_tb_cb(srslte_sch_t *q,
           }
 
           decoder_input[i] = softbuffer->buffer_f[cb_idx[i]];
-
-          printf("input: "); srslte_vec_fprint_s(stdout, decoder_input[i], 10);
-
         }
       }
     }
@@ -398,8 +395,6 @@ bool decode_tb_cb(srslte_sch_t *q,
           crc_ptr = &q->crc_tb; 
         }
 
-        printf("output: %d", i); srslte_vec_fprint_b(stdout, q->cb_in, 10);
-
         // CRC is OK
         if (!srslte_crc_checksum_byte(crc_ptr, q->cb_in, len_crc)) {
 

From 92144ce15de0b58f019ee9fdea4f32eda22af80a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Tue, 26 Sep 2017 18:26:57 +0200
Subject: [PATCH 232/269] Revert "Handle RRC ConnectionReject at eNodeB and UE"

This reverts commit b6f905e2dfeea009bd9bbf5ca307641ed80bad13.
---
 .../srslte/interfaces/enb_interfaces.h        |  3 +-
 lib/include/srslte/interfaces/ue_interfaces.h | 10 ---
 srsenb/hdr/upper/rrc.h                        |  3 +-
 srsenb/hdr/upper/s1ap.h                       |  3 +-
 srsenb/src/upper/rrc.cc                       | 30 ++-----
 srsenb/src/upper/s1ap.cc                      |  6 +-
 srsue/hdr/upper/nas.h                         |  6 --
 srsue/hdr/upper/rrc.h                         |  8 +-
 srsue/src/upper/nas.cc                        | 41 +++------
 srsue/src/upper/rrc.cc                        | 86 ++++---------------
 10 files changed, 42 insertions(+), 154 deletions(-)

diff --git a/lib/include/srslte/interfaces/enb_interfaces.h b/lib/include/srslte/interfaces/enb_interfaces.h
index d2cfbca08..ed2478896 100644
--- a/lib/include/srslte/interfaces/enb_interfaces.h
+++ b/lib/include/srslte/interfaces/enb_interfaces.h
@@ -259,8 +259,7 @@ public:
   virtual bool user_exists(uint16_t rnti) = 0; 
   virtual void user_inactivity(uint16_t rnti) = 0;
   virtual void release_eutran(uint16_t rnti) = 0; 
-  virtual bool user_link_lost(uint16_t rnti) = 0;
-  virtual bool is_connected() = 0;
+  virtual bool user_link_lost(uint16_t rnti) = 0; 
   virtual void ue_ctxt_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_INITIALCONTEXTSETUPRESPONSE_STRUCT *res) = 0;
   virtual void ue_erab_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_E_RABSETUPRESPONSE_STRUCT *res) = 0;
   // virtual void ue_capabilities(uint16_t rnti, LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *caps) = 0;
diff --git a/lib/include/srslte/interfaces/ue_interfaces.h b/lib/include/srslte/interfaces/ue_interfaces.h
index 770c44738..2c29bd4d4 100644
--- a/lib/include/srslte/interfaces/ue_interfaces.h
+++ b/lib/include/srslte/interfaces/ue_interfaces.h
@@ -97,19 +97,9 @@ public:
 class nas_interface_rrc
 {
 public:
-
-  typedef enum {
-    BARRING_MT_ACCESS       = 1,
-    BARRING_MO_CALL         = 2,
-    BARRING_MO_SIGNALLING   = 4,
-    BARRING_ALL             = 7
-  } cell_barring_type_t;
-
   virtual bool      is_attached() = 0;
   virtual bool      is_attaching() = 0;
   virtual void      notify_connection_setup() = 0;
-  virtual void      notify_connection_failure() = 0;
-  virtual void      network_barring_state(bool is_barred, uint32_t type_mask = BARRING_ALL) = 0;
   virtual void      write_pdu(uint32_t lcid, srslte::byte_buffer_t *pdu) = 0;
   virtual uint32_t  get_ul_count() = 0;
   virtual bool      get_s_tmsi(LIBLTE_RRC_S_TMSI_STRUCT *s_tmsi) = 0;
diff --git a/srsenb/hdr/upper/rrc.h b/srsenb/hdr/upper/rrc.h
index 29eb87e9f..49a02d0b9 100644
--- a/srsenb/hdr/upper/rrc.h
+++ b/srsenb/hdr/upper/rrc.h
@@ -172,8 +172,7 @@ public:
     rrc_state_t get_state();
     
     void send_connection_setup(bool is_setup = true);
-    void send_connection_rej();
-    void send_connection_reest();
+    void send_connection_reest(); 
     void send_connection_release();
     void send_connection_reest_rej(); 
     void send_connection_reconf(srslte::byte_buffer_t *sdu);
diff --git a/srsenb/hdr/upper/s1ap.h b/srsenb/hdr/upper/s1ap.h
index 42c5cbb26..02e5e207e 100644
--- a/srsenb/hdr/upper/s1ap.h
+++ b/srsenb/hdr/upper/s1ap.h
@@ -79,8 +79,7 @@ public:
   bool user_exists(uint16_t rnti); 
   void user_inactivity(uint16_t rnti);
   bool user_link_lost(uint16_t rnti);
-  void release_eutran(uint16_t rnti);
-  bool is_connected();
+  void release_eutran(uint16_t rnti); 
   void ue_ctxt_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_INITIALCONTEXTSETUPRESPONSE_STRUCT *res);
   void ue_erab_setup_complete(uint16_t rnti, LIBLTE_S1AP_MESSAGE_E_RABSETUPRESPONSE_STRUCT *res);
   //void ue_capabilities(uint16_t rnti, LIBLTE_RRC_UE_EUTRA_CAPABILITY_STRUCT *caps);
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index de34ab136..f803500f2 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -833,20 +833,14 @@ void rrc::ue::parse_ul_dcch(uint32_t lcid, byte_buffer_t *pdu)
 void rrc::ue::handle_rrc_con_req(LIBLTE_RRC_CONNECTION_REQUEST_STRUCT *msg)
 {
   set_activity();
-
-  if (parent->s1ap->is_connected()) {
-    if(msg->ue_id_type == LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI) {
-      mmec      = msg->ue_id.s_tmsi.mmec;
-      m_tmsi    = msg->ue_id.s_tmsi.m_tmsi;
-      has_tmsi = true;
-    }
-    send_connection_setup();
-    state = RRC_STATE_WAIT_FOR_CON_SETUP_COMPLETE;
-  } else {
-    parent->rrc_log->info("Received ConnectionRequest with no active S1 link\n");
-    parent->rrc_log->console("Received ConnectionRequest with no active S1 link\n");
-    send_connection_rej();
+  
+  if(msg->ue_id_type == LIBLTE_RRC_CON_REQ_UE_ID_TYPE_S_TMSI) {
+    mmec      = msg->ue_id.s_tmsi.mmec;
+    m_tmsi    = msg->ue_id.s_tmsi.m_tmsi;
+    has_tmsi = true;
   }
+  send_connection_setup();
+  state = RRC_STATE_WAIT_FOR_CON_SETUP_COMPLETE;
 }
 
 void rrc::ue::handle_rrc_con_reest_req(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_REQUEST_STRUCT *msg)
@@ -1060,16 +1054,6 @@ void rrc::ue::send_connection_reest_rej()
   
 }
 
-void rrc::ue::send_connection_rej()
-{
-  LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg;
-  bzero(&dl_ccch_msg, sizeof(LIBLTE_RRC_DL_CCCH_MSG_STRUCT));
-
-  dl_ccch_msg.msg_type = LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ;
-  dl_ccch_msg.msg.rrc_con_rej.wait_time = 10;
-  send_dl_ccch(&dl_ccch_msg);
-}
-
 void rrc::ue::send_connection_setup(bool is_setup)
 {
   LIBLTE_RRC_DL_CCCH_MSG_STRUCT dl_ccch_msg; 
diff --git a/srsenb/src/upper/s1ap.cc b/srsenb/src/upper/s1ap.cc
index ca6581189..44a564d09 100644
--- a/srsenb/src/upper/s1ap.cc
+++ b/srsenb/src/upper/s1ap.cc
@@ -170,11 +170,6 @@ void s1ap::build_tai_cgi()
 /*******************************************************************************
 /* RRC interface
 ********************************************************************************/
-
-bool s1ap::is_connected() {
-  return mme_connected;
-}
-
 void s1ap::initial_ue(uint16_t rnti, srslte::byte_buffer_t *pdu)
 {
   ue_ctxt_map[rnti].eNB_UE_S1AP_ID = next_eNB_UE_S1AP_ID++;
@@ -231,6 +226,7 @@ void s1ap::user_inactivity(uint16_t rnti)
   send_uectxtreleaserequest(rnti, &cause);
 }
 
+
 void s1ap::release_eutran(uint16_t rnti)
 {
   s1ap_log->info("Release by EUTRAN - RNTI:0x%x\n", rnti);
diff --git a/srsue/hdr/upper/nas.h b/srsue/hdr/upper/nas.h
index 1fa168305..68d00ba06 100644
--- a/srsue/hdr/upper/nas.h
+++ b/srsue/hdr/upper/nas.h
@@ -80,9 +80,6 @@ public:
 
   // RRC interface
   void notify_connection_setup();
-  void notify_connection_failure();
-  void network_barring_state(bool is_barred, uint32_t type_mask);
-
 
   void write_pdu(uint32_t lcid, byte_buffer_t *pdu);
 
@@ -138,9 +135,6 @@ private:
   uint8_t k_nas_enc[32];
   uint8_t k_nas_int[32];
 
-  bool network_is_barred;
-  uint32_t network_barring_type;
-
   srslte::CIPHERING_ALGORITHM_ID_ENUM cipher_algo;
   srslte::INTEGRITY_ALGORITHM_ID_ENUM integ_algo;
 
diff --git a/srsue/hdr/upper/rrc.h b/srsue/hdr/upper/rrc.h
index 136924af8..8dfa7f70d 100644
--- a/srsue/hdr/upper/rrc.h
+++ b/srsue/hdr/upper/rrc.h
@@ -127,7 +127,7 @@ private:
   srslte::mac_interface_timers *mac_timers;
   uint32_t n310_cnt, N310;
   uint32_t n311_cnt, N311;
-  uint32_t t300, t301, t302, t310, t311;
+  uint32_t t301, t310, t311;
   int ue_category;
 
   typedef struct {
@@ -240,15 +240,15 @@ private:
   // Parsers
   void          parse_dl_ccch(byte_buffer_t *pdu);
   void          parse_dl_dcch(uint32_t lcid, byte_buffer_t *pdu);
+  void          parse_dl_info_transfer(uint32_t lcid, byte_buffer_t *pdu);
 
   // Helpers
-  void          timer_t300_expiry();
-  void          timer_barring_expiry(uint32_t timer_id);
   void          rrc_connection_release();
   void          radio_link_failure(); 
+  static void*  start_sib_thread(void *rrc_);
+  void          sib_search();
   void          apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2);
   void          handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup);
-  void          handle_con_rej(LIBLTE_RRC_CONNECTION_REJECT_STRUCT *reject);
   void          handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup);
   void          handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGURATION_STRUCT *reconfig, byte_buffer_t *pdu);
   void          add_srb(LIBLTE_RRC_SRB_TO_ADD_MOD_STRUCT *srb_cnfg);
diff --git a/srsue/src/upper/nas.cc b/srsue/src/upper/nas.cc
index ead5e9d30..91b35ce01 100644
--- a/srsue/src/upper/nas.cc
+++ b/srsue/src/upper/nas.cc
@@ -66,27 +66,22 @@ UE interface
 *******************************************************************************/
 void nas::attach_request() {
   nas_log->info("Attach Request\n");
-  if (!network_is_barred) {
-    if (state == EMM_STATE_DEREGISTERED) {
-      state = EMM_STATE_REGISTERED_INITIATED;
-      if (plmn_selection == PLMN_NOT_SELECTED) {
-        nas_log->info("Starting PLMN Search...\n");
-        rrc->plmn_search();
-      } else if (plmn_selection == PLMN_SELECTED) {
-        nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
-        rrc->plmn_select(current_plmn);
-        selecting_plmn = current_plmn;
-      }
-    } else if (state == EMM_STATE_REGISTERED) {
-      nas_log->info("NAS state is registered, connecting to same PLMN\n");
+  if (state == EMM_STATE_DEREGISTERED) {
+    state = EMM_STATE_REGISTERED_INITIATED;
+    if (plmn_selection == PLMN_NOT_SELECTED) {
+      nas_log->info("Starting PLMN Search...\n");
+      rrc->plmn_search();
+    } else if (plmn_selection == PLMN_SELECTED) {
+      nas_log->info("Selecting PLMN %s\n", plmn_id_to_c_str(current_plmn).c_str());
       rrc->plmn_select(current_plmn);
       selecting_plmn = current_plmn;
-    } else {
-      nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
     }
+  } else if (state == EMM_STATE_REGISTERED) {
+    nas_log->info("NAS state is registered, connecting to same PLMN\n");
+    rrc->plmn_select(current_plmn);
+    selecting_plmn = current_plmn;
   } else {
-    nas_log->info("Requested connection while network is barred\n");
-    nas_log->console("Requested connection while network is barred\n");
+    nas_log->info("Attach request ignored. State = %s\n", emm_state_text[state]);
   }
 }
 
@@ -99,12 +94,6 @@ void nas::deattach_request() {
 RRC interface
 *******************************************************************************/
 
-void nas::network_barring_state(bool is_barred, uint32_t type_mask)
-{
-  network_is_barred    = is_barred;
-  network_barring_type = type_mask;
-}
-
 void nas::plmn_found(LIBLTE_RRC_PLMN_IDENTITY_STRUCT plmn_id, uint16_t tracking_area_code) {
 
   // Store PLMN if not registered
@@ -136,12 +125,6 @@ bool nas::is_attaching() {
   return state == EMM_STATE_REGISTERED_INITIATED;
 }
 
-void nas::notify_connection_failure() {
-  nas_log->info("Received RRC connection request failure\n");
-  nas_log->console("Connection Request Failure\n");
-  state = EMM_STATE_DEREGISTERED;
-}
-
 void nas::notify_connection_setup() {
   nas_log->debug("State = %s\n", emm_state_text[state]);
   if (EMM_STATE_REGISTERED_INITIATED == state) {
diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 529d99f8f..3bb3e0017 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -93,9 +93,7 @@ void rrc::init(phy_interface_rrc *phy_,
   pthread_mutex_init(&mutex, NULL);
 
   ue_category = SRSLTE_UE_CATEGORY;
-  t300 = mac_timers->timer_get_unique_id();
   t301 = mac_timers->timer_get_unique_id();
-  t302 = mac_timers->timer_get_unique_id();
   t310 = mac_timers->timer_get_unique_id();
   t311 = mac_timers->timer_get_unique_id();
 
@@ -540,20 +538,14 @@ void rrc::max_retx_attempted() {
 }
 
 void rrc::timer_expired(uint32_t timeout_id) {
-  if (timeout_id == t300) {
-    rrc_log->info("Timer T300 expired: ConnectionRequest timeout\n");
-    timer_t300_expiry();
-  } else if (timeout_id == t301) {
-    rrc_log->info("Timer T301 expired: ConnectionReestablishmentRequest timeout\n");
-    state = RRC_STATE_LEAVE_CONNECTED;
-  } else if (timeout_id == t302) {
-    rrc_log->info("Timer T302 expired: ConnectionReject Wait time expired\n");
-    timer_barring_expiry(timeout_id);
-  } else if (timeout_id == t310) {
+  if (timeout_id == t310) {
     rrc_log->info("Timer T310 expired: Radio Link Failure\n");
     radio_link_failure();
   } else if (timeout_id == t311) {
-    rrc_log->info("Timer T311 expired: Start of Reestablishment procedure timeout\n");
+    rrc_log->info("Timer T311 expired: Going to RRC IDLE\n");
+    state = RRC_STATE_LEAVE_CONNECTED;
+  } else if (timeout_id == t301) {
+    rrc_log->info("Timer T301 expired: Going to RRC IDLE\n");
     state = RRC_STATE_LEAVE_CONNECTED;
   } else {
     rrc_log->error("Timeout from unknown timer id %d\n", timeout_id);
@@ -616,10 +608,6 @@ void rrc::send_con_request() {
 
   mac->set_contention_id(uecri);
 
-  // Start t300 timer
-  mac_timers->timer_get(t300)->reset();
-  mac_timers->timer_get(t300)->run();
-
   rrc_log->info("Sending RRC Connection Request on SRB0\n");
   pdcp->write_sdu(RB_ID_SRB0, pdcp_buf);
 }
@@ -762,6 +750,7 @@ void rrc::send_con_setup_complete(byte_buffer_t *nas_msg) {
   pdcp_buf->N_bytes = bit_buf.N_bits / 8;
   pdcp_buf->set_timestamp();
 
+  state = RRC_STATE_CONNECTED;
   rrc_log->console("RRC Connected\n");
   rrc_log->info("Sending RRC Connection Setup Complete\n");
   pdcp->write_sdu(RB_ID_SRB1, pdcp_buf);
@@ -868,25 +857,13 @@ void rrc::handle_rrc_con_reconfig(uint32_t lcid, LIBLTE_RRC_CONNECTION_RECONFIGU
   }
 }
 
-/* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
+  /* Actions upon reception of RRCConnectionRelease 5.3.8.3 */
 void rrc::rrc_connection_release() {
   // Save idleModeMobilityControlInfo, etc.
   state = RRC_STATE_LEAVE_CONNECTED;
   rrc_log->console("Received RRC Connection Release\n");
 }
 
-// Actions upon expiry of t300 timer 5.3.3.6
-void rrc::timer_t300_expiry() {
-  mac->reset();
-  set_mac_default();
-  nas->notify_connection_failure();
-  state = RRC_STATE_IDLE;
-}
-
-// Actions upon expiry of timers related to cell barring 5.3.3.7
-void rrc::timer_barring_expiry(uint32_t timer_id) {
-  nas->network_barring_state(false);
-}
 
 
 
@@ -1105,14 +1082,20 @@ void rrc::parse_dl_ccch(byte_buffer_t *pdu) {
 
   switch (dl_ccch_msg.msg_type) {
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REJ:
-      transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
-      handle_con_rej(&dl_ccch_msg.msg.rrc_con_rej);
+      rrc_log->info("Connection Reject received. Wait time: %d\n",
+                    dl_ccch_msg.msg.rrc_con_rej.wait_time);
+      state = RRC_STATE_IDLE;
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_SETUP:
+      rrc_log->info("Connection Setup received\n");
       transaction_id = dl_ccch_msg.msg.rrc_con_setup.rrc_transaction_id;
       handle_con_setup(&dl_ccch_msg.msg.rrc_con_setup);
+      rrc_log->info("Notifying NAS of connection setup\n");
+      nas->notify_connection_setup();
       break;
     case LIBLTE_RRC_DL_CCCH_MSG_TYPE_RRC_CON_REEST:
+      rrc_log->info("Connection Reestablishment received\n");
+      rrc_log->console("Reestablishment OK\n");
       transaction_id = dl_ccch_msg.msg.rrc_con_reest.rrc_transaction_id;
       handle_con_reest(&dl_ccch_msg.msg.rrc_con_reest);
       break;
@@ -1359,11 +1342,9 @@ void rrc::apply_sib2_configs(LIBLTE_RRC_SYS_INFO_BLOCK_TYPE_2_STRUCT *sib2) {
                 liblte_rrc_srs_subfr_config_num[sib2->rr_config_common_sib.srs_ul_cnfg.subfr_cnfg],
                 sib2->rr_config_common_sib.srs_ul_cnfg.ack_nack_simul_tx ? "yes" : "no");
 
-  mac_timers->timer_get(t300)->set(this, liblte_rrc_t300_num[sib2->ue_timers_and_constants.t300]);
   mac_timers->timer_get(t301)->set(this, liblte_rrc_t301_num[sib2->ue_timers_and_constants.t301]);
   mac_timers->timer_get(t310)->set(this, liblte_rrc_t310_num[sib2->ue_timers_and_constants.t310]);
   mac_timers->timer_get(t311)->set(this, liblte_rrc_t311_num[sib2->ue_timers_and_constants.t311]);
-
   N310 = liblte_rrc_n310_num[sib2->ue_timers_and_constants.n310];
   N311 = liblte_rrc_n311_num[sib2->ue_timers_and_constants.n311];
 
@@ -1600,50 +1581,13 @@ void rrc::apply_rr_config_dedicated(LIBLTE_RRC_RR_CONFIG_DEDICATED_STRUCT *cnfg)
   }
 }
 
-// 5.3.3.4
 void rrc::handle_con_setup(LIBLTE_RRC_CONNECTION_SETUP_STRUCT *setup) {
-  rrc_log->info("Connection Setup received\n");
-
   // Apply the Radio Resource configuration
   apply_rr_config_dedicated(&setup->rr_cnfg);
-
-  // stop timers
-  mac_timers->timer_get(t300)->stop();
-
-  // enter RRC CONNECTED
-  state = RRC_STATE_CONNECTED;
-
-  rrc_log->info("Notifying NAS of connection setup\n");
-  nas->notify_connection_setup();
-
-}
-
-// 5.3.3.8
-void rrc::handle_con_rej(LIBLTE_RRC_CONNECTION_REJECT_STRUCT *reject) {
-  rrc_log->info("Connection Reject received. Wait time: %d s\n",
-                dl_ccch_msg.msg.rrc_con_rej.wait_time);
-
-  rrc_log->console("Received Connection Reject. S1 link may not be active at the eNodeB\n");
-
-  mac_timers->timer_get(t300)->stop();
-  mac->reset();
-  set_mac_default();
-
-  mac_timers->timer_get(t302)->set(this, dl_ccch_msg.msg.rrc_con_rej.wait_time*1000);
-  mac_timers->timer_get(t302)->run();
-
-  nas->notify_connection_failure();
-  nas->network_barring_state(true);
-
-  state = RRC_STATE_IDLE;
 }
 
 /* Reception of RRCConnectionReestablishment by the UE 5.3.7.5 */
 void rrc::handle_con_reest(LIBLTE_RRC_CONNECTION_REESTABLISHMENT_STRUCT *setup) {
-  rrc_log->info("Connection Reestablishment received\n");
-  rrc_log->console("Reestablishment OK\n");
-
-
   mac_timers->timer_get(t301)->stop();
 
   // TODO: Restablish DRB1. Not done because never was suspended

From d836defd57234cd9aea2118ee45ca416ec256ff6 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 27 Sep 2017 18:33:20 +0200
Subject: [PATCH 233/269] add field to CSV metrics that contains the UE
 connection state

---
 srsue/src/metrics_csv.cc   | 12 ++++--------
 srsue/test/metrics_test.cc |  2 +-
 2 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/srsue/src/metrics_csv.cc b/srsue/src/metrics_csv.cc
index 18c761263..f9bdce213 100644
--- a/srsue/src/metrics_csv.cc
+++ b/srsue/src/metrics_csv.cc
@@ -55,14 +55,9 @@ void metrics_csv::set_ue_handle(ue_metrics_interface *ue_)
 
 void metrics_csv::set_metrics(ue_metrics_t &metrics, float metrics_report_period)
 {
-  if (file.is_open()) {
-    if (!ue->is_attached()) {
-      file << "# disconnected" << endl;
-      return;
-    }
-
+  if (file.is_open() && ue != NULL) {
     if(n_reports == 0) {
-      file << "time;rsrp;pl;cfo;dl_mcs;dl_snr;dl_turbo;dl_brate;dl_bler;ul_mcs;ul_buff;ul_brate;ul_bler;rf_o;rf_u;rf_l" << endl;
+      file << "time;rsrp;pl;cfo;dl_mcs;dl_snr;dl_turbo;dl_brate;dl_bler;ul_mcs;ul_buff;ul_brate;ul_bler;rf_o;rf_u;rf_l;is_attached" << endl;
     }
     file << (metrics_report_period*n_reports) << ";";
     file << float_to_string(metrics.phy.dl.rsrp, 2);
@@ -87,7 +82,8 @@ void metrics_csv::set_metrics(ue_metrics_t &metrics, float metrics_report_period
     }
     file << float_to_string(metrics.rf.rf_o, 2);
     file << float_to_string(metrics.rf.rf_u, 2);
-    file << float_to_string(metrics.rf.rf_l, 2, false);
+    file << float_to_string(metrics.rf.rf_l, 2);
+    file << (ue->is_attached() ? "1.0" : "0.0");
     file << endl;
 
     n_reports++;
diff --git a/srsue/test/metrics_test.cc b/srsue/test/metrics_test.cc
index 9cd17f824..52a5f98f9 100644
--- a/srsue/test/metrics_test.cc
+++ b/srsue/test/metrics_test.cc
@@ -56,7 +56,7 @@ public:
 
   bool is_attached()
   {
-    return true;
+    return (rand() % 2 == 0);
   }
 };
 }

From c66f5fc5708ef76a7e26ac267c33df9bba6b438e Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 28 Sep 2017 15:05:11 +0200
Subject: [PATCH 234/269] Reduced CQI more for PUSCH+UCI

---
 srsenb/src/mac/scheduler_ue.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index b9a3093c2..88719d843 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -815,7 +815,7 @@ int sched_ue::alloc_tbs(uint32_t nof_prb,
 
   // TODO: Compute real spectral efficiency based on PUSCH-UCI configuration
   if (has_pucch && is_ul) {
-    cqi-=2;
+    cqi-=3;
   }
 
   int tbs = cqi_to_tbs(cqi, nof_prb, nof_re, max_mcs, max_Qm, &sel_mcs)/8;

From 6029d25214dfa189291217d778fec88b93e4d709 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 28 Sep 2017 15:27:35 +0200
Subject: [PATCH 235/269] Fixed bug in get_required_nprb_ul

---
 srsenb/src/mac/scheduler_ue.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 88719d843..cd977d8b2 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -635,7 +635,7 @@ uint32_t sched_ue::get_required_prb_ul(uint32_t req_bytes)
     return 0; 
   }
   
-  for (n=1;n<=cell.nof_prb && nbytes < req_bytes + 4;n++) {
+  for (n=1;n<cell.nof_prb && nbytes < req_bytes + 4;n++) {
     uint32_t nof_re = (2*(SRSLTE_CP_NSYMB(cell.cp)-1) - N_srs)*n*SRSLTE_NRE;
     int tbs = 0; 
     if (fixed_mcs_ul < 0) {
@@ -647,8 +647,8 @@ uint32_t sched_ue::get_required_prb_ul(uint32_t req_bytes)
       nbytes = tbs; 
     }
   }
-  
-  while (!srslte_dft_precoding_valid_prb(n)) {
+
+  while (!srslte_dft_precoding_valid_prb(n) && n<=cell.nof_prb) {
     n++;
   }
   

From b625910b554b97dd17c7dfc88aef5b4537e89259 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 28 Sep 2017 16:03:26 +0200
Subject: [PATCH 236/269] Added 50% margin to PDCCH spectral efficiency for
 scheduling

---
 srsenb/src/mac/scheduler_ue.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index cd977d8b2..1534d12ef 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -705,7 +705,7 @@ uint32_t sched_ue::get_aggr_level(uint32_t nof_bits)
   do {
     coderate = srslte_pdcch_coderate(nof_bits, l);
     l++;
-  } while(l<3 && coderate > max_coderate);
+  } while(l<3 && 1.5*coderate > max_coderate);
   Debug("SCHED: CQI=%d, l=%d, nof_bits=%d, coderate=%.2f, max_coderate=%.2f\n", dl_cqi, l, nof_bits, coderate, max_coderate);
   return l; 
 }

From c23ceae2ec639c2bbaedaa5435071c508d6b5dfd Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 29 Sep 2017 13:38:47 +0200
Subject: [PATCH 237/269] Changes on enb::stop and rem_user to avoid segfaults

---
 srsenb/src/enb.cc       | 6 +++---
 srsenb/src/upper/rrc.cc | 3 ++-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/srsenb/src/enb.cc b/srsenb/src/enb.cc
index df4c71ae9..82b4e8145 100644
--- a/srsenb/src/enb.cc
+++ b/srsenb/src/enb.cc
@@ -228,15 +228,15 @@ void enb::stop()
 {
   if(started)
   {
+    gtpu.stop();
     phy.stop();
     mac.stop();
-    usleep(1e5);
+    usleep(100000);
 
     rlc.stop();
     pdcp.stop();
-    gtpu.stop();
     rrc.stop();
- 
+
     usleep(1e5);
     if(args->pcap.enable)
     {
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index f803500f2..89b38dd36 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -223,7 +223,8 @@ void rrc::rem_user(uint16_t rnti)
     rrc_log->console("Disconnecting rnti=0x%x.\n", rnti);
     rrc_log->info("Disconnecting rnti=0x%x.\n", rnti);
     /* **Caution** order of removal here is imporant: from bottom to top */
-    mac->ue_rem(rnti);  // MAC handles PHY 
+    mac->ue_rem(rnti);  // MAC handles PHY
+    usleep(50000);
     rlc->rem_user(rnti);
     pdcp->rem_user(rnti);
     gtpu->rem_user(rnti);

From 9437731c00c33960f8b42a6b9f8c741fbd601fe1 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 29 Sep 2017 13:38:55 +0200
Subject: [PATCH 238/269] Removed softbuffer TX reset

---
 srsenb/src/mac/mac.cc | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 87290a859..7cb30cc65 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -441,8 +441,7 @@ int mac::get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res)
       dl_sched_res->sched_grants[n].data     = ue_db[rnti]->generate_pdu(sched_result.data[i].pdu, 
                                                         sched_result.data[i].nof_pdu_elems, 
                                                         sched_result.data[i].tbs);
-      srslte_softbuffer_tx_reset_tbs(dl_sched_res->sched_grants[n].softbuffer, sched_result.data[i].tbs);
-      
+
       if (pcap) {
         pcap->write_dl_crnti(dl_sched_res->sched_grants[n].data, sched_result.data[i].tbs, rnti, true, tti);
       }
@@ -462,7 +461,6 @@ int mac::get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res)
 
     // Set softbuffer (there are no retx in RAR but a softbuffer is required)
     dl_sched_res->sched_grants[n].softbuffer = &rar_softbuffer_tx;    
-    srslte_softbuffer_tx_reset_tbs(&rar_softbuffer_tx, sched_result.rar[i].tbs); // TBS is usually 54-bit 
 
     // Assemble PDU 
     dl_sched_res->sched_grants[n].data = assemble_rar(sched_result.rar[i].grants, sched_result.rar[i].nof_grants, i, sched_result.rar[i].tbs);        
@@ -485,9 +483,6 @@ int mac::get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res)
     // Set softbuffer    
     if (sched_result.bc[i].type == sched_interface::dl_sched_bc_t::BCCH) {
       dl_sched_res->sched_grants[n].softbuffer = &bcch_softbuffer_tx[sched_result.bc[i].index];    
-      if (sched_result.bc[i].dci.rv_idx == 0) {
-        srslte_softbuffer_tx_reset_tbs(dl_sched_res->sched_grants[n].softbuffer, sched_result.bc[i].tbs*8);
-      }
       dl_sched_res->sched_grants[n].data = assemble_si(sched_result.bc[i].index);
 #ifdef WRITE_SIB_PCAP
       if (pcap) {
@@ -496,7 +491,6 @@ int mac::get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res)
 #endif
     } else {
       dl_sched_res->sched_grants[n].softbuffer = &pcch_softbuffer_tx;    
-      srslte_softbuffer_tx_reset_tbs(dl_sched_res->sched_grants[n].softbuffer, sched_result.bc[i].tbs*8);
       dl_sched_res->sched_grants[n].data = pcch_payload_buffer;
       rlc_h->read_pdu_pcch(pcch_payload_buffer, pcch_payload_buffer_len);
       

From 32a29f1d342b82f038efa311edbce12bc89d385a Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 29 Sep 2017 13:39:18 +0200
Subject: [PATCH 239/269] Do not reset radio on srsUE receive error

---
 srsue/src/phy/phch_recv.cc | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index cad5219f9..dad2c82b8 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -168,7 +168,13 @@ void phch_recv::radio_error() {
   log_h->error("SYNC:  Receiving from radio.\n");
   phy_state = IDLE;
   radio_is_resetting=true;
-  radio_h->reset();
+
+  // Need to find a method to effectively reset radio, reloading the driver does not work
+  //radio_h->reset();
+
+  fprintf(stdout, "Error while receiving samples. Restart srsUE\n");
+  exit(-1);
+
   reset();
   radio_is_resetting=false;
 }

From 602b6843521746e87d33720d81dfca48a4aefa48 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Sun, 1 Oct 2017 21:04:41 +0200
Subject: [PATCH 240/269] Reduced default TX/RX gains in srsUE/srsENB

---
 srsenb/enb.conf.example | 4 ++--
 srsue/ue.conf.example   | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 5e386ddc1..391eb8af6 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -58,8 +58,8 @@ drb_config = drb.conf
 #####################################################################
 [rf]
 dl_earfcn = 3400
-tx_gain = 80
-rx_gain = 50
+tx_gain = 70
+rx_gain = 40
 
 #device_name = auto
 #device_args = auto
diff --git a/srsue/ue.conf.example b/srsue/ue.conf.example
index 332d00e59..30c05069a 100644
--- a/srsue/ue.conf.example
+++ b/srsue/ue.conf.example
@@ -23,8 +23,8 @@
 [rf]
 dl_earfcn = 3400
 freq_offset = 0
-tx_gain = 80
-rx_gain = 50
+tx_gain = 70
+rx_gain = 40
 
 #nof_rx_ant = 1
 #device_name = auto

From d3916cec9187e768d79799cd3da1449a780c6e62 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Mon, 2 Oct 2017 10:50:06 +0200
Subject: [PATCH 241/269] fix segfault when printing help message

---
 lib/examples/pdsch_ue.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 38113dbcf..9d7282cdd 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -99,7 +99,7 @@ typedef struct {
   int net_port_signal; 
   char *net_address_signal;
   int decimate;
-  int mbsfn_area_id;
+  int32_t mbsfn_area_id;
   uint8_t  non_mbsfn_region;
   int verbose;
 }prog_args_t;
@@ -171,8 +171,8 @@ void usage(prog_args_t *args, char *prog) {
   printf("\t-S remote UDP address to send input signal [Default %s]\n", args->net_address_signal);
   printf("\t-u remote TCP port to send data (-1 does nothing with it) [Default %d]\n", args->net_port);
   printf("\t-U remote TCP address to send data [Default %s]\n", args->net_address);
-  printf("\t-M MBSFN area id [Default %s]\n", args->mbsfn_area_id);
-  printf("\t-N Non-MBSFN region [Default %s]\n", args->non_mbsfn_region);
+  printf("\t-M MBSFN area id [Default %d]\n", args->mbsfn_area_id);
+  printf("\t-N Non-MBSFN region [Default %d]\n", args->non_mbsfn_region);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 

From 428e5955df68d4967c1ec56a5d44e476f0d4ad7d Mon Sep 17 00:00:00 2001
From: Paul Sutton <suttonpd@gmail.com>
Date: Thu, 21 Sep 2017 16:49:33 +0100
Subject: [PATCH 242/269] Fix for SPGW address issue for GTP bearers - now
 using addresses from bearer setup requests

htonl fix
---
 .../srslte/interfaces/enb_interfaces.h        |  2 +-
 srsenb/hdr/upper/gtpu.h                       | 10 ++-
 srsenb/hdr/upper/rrc.h                        |  3 +
 srsenb/src/upper/gtpu.cc                      | 81 ++++++++++++++++---
 srsenb/src/upper/rrc.cc                       | 62 ++++++++------
 5 files changed, 116 insertions(+), 42 deletions(-)

diff --git a/lib/include/srslte/interfaces/enb_interfaces.h b/lib/include/srslte/interfaces/enb_interfaces.h
index ed2478896..9c85814f8 100644
--- a/lib/include/srslte/interfaces/enb_interfaces.h
+++ b/lib/include/srslte/interfaces/enb_interfaces.h
@@ -244,7 +244,7 @@ public:
 class gtpu_interface_rrc
 {
 public:
-  virtual void add_bearer(uint16_t rnti, uint32_t lcid, uint32_t teid_out, uint32_t *teid_in) = 0;
+  virtual void add_bearer(uint16_t rnti, uint32_t lcid, uint32_t addr, uint32_t teid_out, uint32_t *teid_in) = 0;
   virtual void rem_bearer(uint16_t rnti, uint32_t lcid) = 0;
   virtual void rem_user(uint16_t rnti) = 0;
 };
diff --git a/srsenb/hdr/upper/gtpu.h b/srsenb/hdr/upper/gtpu.h
index 9ad9441fa..6ec371655 100644
--- a/srsenb/hdr/upper/gtpu.h
+++ b/srsenb/hdr/upper/gtpu.h
@@ -75,7 +75,7 @@ public:
   void stop();
   
   // gtpu_interface_rrc
-  void add_bearer(uint16_t rnti, uint32_t lcid, uint32_t teid_out, uint32_t *teid_in);
+  void add_bearer(uint16_t rnti, uint32_t lcid, uint32_t addr, uint32_t teid_out, uint32_t *teid_in);
   void rem_bearer(uint16_t rnti, uint32_t lcid);
   void rem_user(uint16_t rnti);
   
@@ -86,7 +86,7 @@ public:
 private:
   static const int THREAD_PRIO = 7;
   static const int GTPU_PORT   = 2152;
-  srslte::byte_buffer_pool         *pool;
+  srslte::byte_buffer_pool     *pool;
   bool                         running;
   bool                         run_enable;
 
@@ -98,11 +98,13 @@ private:
   typedef struct{
     uint32_t teids_in[SRSENB_N_RADIO_BEARERS];
     uint32_t teids_out[SRSENB_N_RADIO_BEARERS];
+    uint32_t spgw_addrs[SRSENB_N_RADIO_BEARERS];
   }bearer_map;
   std::map<uint16_t, bearer_map> rnti_bearers;
 
-  srslte_netsink_t      snk;
-  srslte_netsource_t    src;
+  // Socket file descriptors
+  int snk_fd;
+  int src_fd;
 
   void run_thread();
   
diff --git a/srsenb/hdr/upper/rrc.h b/srsenb/hdr/upper/rrc.h
index 49a02d0b9..e0b9dd158 100644
--- a/srsenb/hdr/upper/rrc.h
+++ b/srsenb/hdr/upper/rrc.h
@@ -195,6 +195,9 @@ public:
 
     bool setup_erabs(LIBLTE_S1AP_E_RABTOBESETUPLISTCTXTSUREQ_STRUCT *e);
     bool setup_erabs(LIBLTE_S1AP_E_RABTOBESETUPLISTBEARERSUREQ_STRUCT *e);
+    void setup_erab(uint8_t id, LIBLTE_S1AP_E_RABLEVELQOSPARAMETERS_STRUCT *qos,
+                    LIBLTE_S1AP_TRANSPORTLAYERADDRESS_STRUCT *addr, uint32_t teid_out,
+                    LIBLTE_S1AP_NAS_PDU_STRUCT *nas_pdu);
     bool release_erabs();
 
     void notify_s1ap_ue_ctxt_setup_complete();
diff --git a/srsenb/src/upper/gtpu.cc b/srsenb/src/upper/gtpu.cc
index 83f7a1b75..ddd695ffe 100644
--- a/srsenb/src/upper/gtpu.cc
+++ b/srsenb/src/upper/gtpu.cc
@@ -26,6 +26,9 @@
 
 #include "upper/gtpu.h"
 #include <unistd.h>
+#include <sys/socket.h>
+#include <fcntl.h>
+#include <errno.h>
 
 using namespace srslte;
 
@@ -42,16 +45,51 @@ bool gtpu::init(std::string gtp_bind_addr_, std::string mme_addr_, srsenb::pdcp_
   
   pool          = byte_buffer_pool::get_instance();
 
-  if(0 != srslte_netsource_init(&src, gtp_bind_addr.c_str(), GTPU_PORT, SRSLTE_NETSOURCE_UDP)) {
-    gtpu_log->error("Failed to create source socket on %s:%d", gtp_bind_addr.c_str(), GTPU_PORT);
+  // Set up sink socket
+  snk_fd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (snk_fd < 0) {
+    gtpu_log->error("Failed to create sink socket\n");
     return false;
   }
-  if(0 != srslte_netsink_init(&snk, mme_addr.c_str(), GTPU_PORT, SRSLTE_NETSINK_UDP)) {
-    gtpu_log->error("Failed to create sink socket on %s:%d", mme_addr.c_str(), GTPU_PORT);
+  if (fcntl(snk_fd, F_SETFL, O_NONBLOCK)) {
+    gtpu_log->error("Failed to set non-blocking sink socket\n");
+    return false;
+  }
+  int enable = 1;
+#if defined (SO_REUSEADDR)
+  if (setsockopt(snk_fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int)) < 0)
+      gtpu_log->error("setsockopt(SO_REUSEADDR) failed\n");
+#endif
+#if defined (SO_REUSEPORT)
+  if (setsockopt(snk_fd, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(int)) < 0)
+      gtpu_log->error("setsockopt(SO_REUSEPORT) failed\n");
+#endif
+
+
+  // Set up source socket
+  src_fd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (src_fd < 0) {
+    gtpu_log->error("Failed to create source socket\n");
+    return false;
+  }
+#if defined (SO_REUSEADDR)
+  if (setsockopt(src_fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int)) < 0)
+      gtpu_log->error("setsockopt(SO_REUSEADDR) failed\n");
+#endif
+#if defined (SO_REUSEPORT)
+  if (setsockopt(src_fd, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(int)) < 0)
+      gtpu_log->error("setsockopt(SO_REUSEPORT) failed\n");
+#endif
+
+  struct sockaddr_in bindaddr;
+  bindaddr.sin_family      = AF_INET;
+  bindaddr.sin_addr.s_addr = inet_addr(gtp_bind_addr.c_str());
+  bindaddr.sin_port        = htons(GTPU_PORT);
+
+  if (bind(src_fd, (struct sockaddr *)&bindaddr, sizeof(struct sockaddr_in))) {
+    gtpu_log->error("Failed to bind on address %s, port %d\n", gtp_bind_addr, GTPU_PORT);
     return false;
   }
-  
-  srslte_netsink_set_nonblocking(&snk);
 
   // Setup a thread to receive packets from the src socket
   start(THREAD_PRIO);
@@ -61,7 +99,7 @@ bool gtpu::init(std::string gtp_bind_addr_, std::string mme_addr_, srsenb::pdcp_
 
 void gtpu::stop()
 {
-  if(run_enable) {
+  if (run_enable) {
     run_enable = false;
     // Wait thread to exit gracefully otherwise might leave a mutex locked
     int cnt=0;
@@ -75,8 +113,12 @@ void gtpu::stop()
     wait_thread_finish();
   }
 
-  srslte_netsink_free(&snk);
-  srslte_netsource_free(&src);
+  if (snk_fd) {
+    close(snk_fd);
+  }
+  if (src_fd) {
+    close(src_fd);
+  }
 }
 
 // gtpu_interface_pdcp
@@ -89,28 +131,35 @@ void gtpu::write_pdu(uint16_t rnti, uint32_t lcid, srslte::byte_buffer_t* pdu)
   header.length       = pdu->N_bytes;
   header.teid         = rnti_bearers[rnti].teids_out[lcid];
 
+  struct sockaddr_in servaddr;
+  servaddr.sin_family      = AF_INET;
+  servaddr.sin_addr.s_addr = htonl(rnti_bearers[rnti].spgw_addrs[lcid]);
+  servaddr.sin_port        = htons(GTPU_PORT);
+
   gtpu_write_header(&header, pdu);
-  srslte_netsink_write(&snk, pdu->msg, pdu->N_bytes);
+  sendto(snk_fd, pdu->msg, pdu->N_bytes, MSG_EOR, (struct sockaddr*)&servaddr, sizeof(struct sockaddr_in));
   pool->deallocate(pdu);
 }
 
 // gtpu_interface_rrc
-void gtpu::add_bearer(uint16_t rnti, uint32_t lcid, uint32_t teid_out, uint32_t *teid_in)
+void gtpu::add_bearer(uint16_t rnti, uint32_t lcid, uint32_t addr, uint32_t teid_out, uint32_t *teid_in)
 {
   // Allocate a TEID for the incoming tunnel
   rntilcid_to_teidin(rnti, lcid, teid_in);
-  gtpu_log->info("Adding bearer for rnti: 0x%x, lcid: %d, teid_out: 0x%x, teid_in: 0x%x\n", rnti, lcid, teid_out, *teid_in);
+  gtpu_log->info("Adding bearer for rnti: 0x%x, lcid: %d, addr: 0x%x, teid_out: 0x%x, teid_in: 0x%x\n", rnti, lcid, addr, teid_out, *teid_in);
 
   // Initialize maps if it's a new RNTI
   if(rnti_bearers.count(rnti) == 0) {
     for(int i=0;i<SRSENB_N_RADIO_BEARERS;i++) {
       rnti_bearers[rnti].teids_in[i]  = 0;
       rnti_bearers[rnti].teids_out[i] = 0;
+      rnti_bearers[rnti].spgw_addrs[i] = 0;
     }
   }
 
   rnti_bearers[rnti].teids_in[lcid]  = *teid_in;
   rnti_bearers[rnti].teids_out[lcid] = teid_out;
+  rnti_bearers[rnti].spgw_addrs[lcid] = addr;
 }
 
 void gtpu::rem_bearer(uint16_t rnti, uint32_t lcid)
@@ -146,10 +195,16 @@ void gtpu::run_thread()
 
   running=true; 
   while(run_enable) {
+
     pdu->reset();
     gtpu_log->debug("Waiting for read...\n");
-    pdu->N_bytes = srslte_netsource_read(&src, pdu->msg, SRSENB_MAX_BUFFER_SIZE_BYTES - SRSENB_BUFFER_HEADER_OFFSET);
+    do{
+      pdu->N_bytes = recv(src_fd, pdu->msg, SRSENB_MAX_BUFFER_SIZE_BYTES - SRSENB_BUFFER_HEADER_OFFSET, 0);
+    }while (pdu->N_bytes == -1 && errno == EAGAIN);
 
+    if (pdu->N_bytes == -1) {
+        gtpu_log->error("Failed to read from socket\n");
+    }
     
     gtpu_header_t header;
     gtpu_read_header(pdu, &header);
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index 89b38dd36..b1280cf55 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -946,20 +946,16 @@ bool rrc::ue::setup_erabs(LIBLTE_S1AP_E_RABTOBESETUPLISTCTXTSUREQ_STRUCT *e)
     if(erab->iE_Extensions_present) {
       parent->rrc_log->warning("Not handling LIBLTE_S1AP_E_RABTOBESETUPITEMCTXTSUREQ_STRUCT extensions\n");
     }
-
-    uint8_t id = erab->e_RAB_ID.E_RAB_ID;
-    erabs[id].id = id;
-    memcpy(&erabs[id].qos_params, &erab->e_RABlevelQoSParameters, sizeof(LIBLTE_S1AP_E_RABLEVELQOSPARAMETERS_STRUCT));
-    memcpy(&erabs[id].address, &erab->transportLayerAddress, sizeof(LIBLTE_S1AP_TRANSPORTLAYERADDRESS_STRUCT));
-    uint8_to_uint32(erab->gTP_TEID.buffer, &erabs[id].teid_out);
-
-    uint8_t lcid = id - 2;  // Map e.g. E-RAB 5 to LCID 3 (==DRB1)
-    parent->gtpu->add_bearer(rnti, lcid, erabs[id].teid_out, &(erabs[id].teid_in));
-
-    if(erab->nAS_PDU_present) {
-      memcpy(parent->erab_info.msg, erab->nAS_PDU.buffer, erab->nAS_PDU.n_octets);
-      parent->erab_info.N_bytes = erab->nAS_PDU.n_octets;
+    if(erab->transportLayerAddress.n_bits > 32) {
+      parent->rrc_log->error("IPv6 addresses not currently supported\n");
+      return false;
     }
+
+    uint32_t teid_out;
+    uint8_to_uint32(erab->gTP_TEID.buffer, &teid_out);
+    LIBLTE_S1AP_NAS_PDU_STRUCT *nas_pdu = erab->nAS_PDU_present ? &erab->nAS_PDU : NULL;
+    setup_erab(erab->e_RAB_ID.E_RAB_ID, &erab->e_RABlevelQoSParameters,
+               &erab->transportLayerAddress, teid_out, nas_pdu);
   }
   return true;
 }
@@ -974,25 +970,43 @@ bool rrc::ue::setup_erabs(LIBLTE_S1AP_E_RABTOBESETUPLISTBEARERSUREQ_STRUCT *e)
     if(erab->iE_Extensions_present) {
       parent->rrc_log->warning("Not handling LIBLTE_S1AP_E_RABTOBESETUPITEMCTXTSUREQ_STRUCT extensions\n");
     }
+    if(erab->transportLayerAddress.n_bits > 32) {
+      parent->rrc_log->error("IPv6 addresses not currently supported\n");
+      return false;
+    }
 
-    uint8_t id = erab->e_RAB_ID.E_RAB_ID;
-    erabs[id].id = id;
-    memcpy(&erabs[id].qos_params, &erab->e_RABlevelQoSParameters, sizeof(LIBLTE_S1AP_E_RABLEVELQOSPARAMETERS_STRUCT));
-    memcpy(&erabs[id].address, &erab->transportLayerAddress, sizeof(LIBLTE_S1AP_TRANSPORTLAYERADDRESS_STRUCT));
-    uint8_to_uint32(erab->gTP_TEID.buffer, &erabs[id].teid_out);
-
-    uint8_t lcid = id - 2;  // Map e.g. E-RAB 5 to LCID 3 (==DRB1)
-    parent->gtpu->add_bearer(rnti, lcid, erabs[id].teid_out, &(erabs[id].teid_in));
-
-    memcpy(parent->erab_info.msg, erab->nAS_PDU.buffer, erab->nAS_PDU.n_octets);
-    parent->erab_info.N_bytes = erab->nAS_PDU.n_octets;
+    uint32_t teid_out;
+    uint8_to_uint32(erab->gTP_TEID.buffer, &teid_out);
+    setup_erab(erab->e_RAB_ID.E_RAB_ID, &erab->e_RABlevelQoSParameters,
+               &erab->transportLayerAddress, teid_out, &erab->nAS_PDU);
   }
+
   // Work in progress
   notify_s1ap_ue_erab_setup_response(e);
   send_connection_reconf_new_bearer(e);
   return true;
 }
 
+void rrc::ue::setup_erab(uint8_t id, LIBLTE_S1AP_E_RABLEVELQOSPARAMETERS_STRUCT *qos,
+                         LIBLTE_S1AP_TRANSPORTLAYERADDRESS_STRUCT *addr, uint32_t teid_out,
+                         LIBLTE_S1AP_NAS_PDU_STRUCT *nas_pdu)
+{
+  erabs[id].id = id;
+  memcpy(&erabs[id].qos_params, qos, sizeof(LIBLTE_S1AP_E_RABLEVELQOSPARAMETERS_STRUCT));
+  memcpy(&erabs[id].address, addr, sizeof(LIBLTE_S1AP_TRANSPORTLAYERADDRESS_STRUCT));
+  erabs[id].teid_out = teid_out;
+
+  uint8_t* bit_ptr = addr->buffer;
+  uint32_t addr_ = liblte_bits_2_value(&bit_ptr, addr->n_bits);
+  uint8_t lcid  = id - 2;  // Map e.g. E-RAB 5 to LCID 3 (==DRB1)
+  parent->gtpu->add_bearer(rnti, lcid, addr_, erabs[id].teid_out, &(erabs[id].teid_in));
+
+  if(nas_pdu) {
+    memcpy(parent->erab_info.msg, nas_pdu->buffer, nas_pdu->n_octets);
+    parent->erab_info.N_bytes = nas_pdu->n_octets;
+  }
+}
+
 bool rrc::ue::release_erabs()
 {
   typedef std::map<uint8_t, erab_t>::iterator it_t;

From 48186cd4fc8fbd0f71cc5345f44f8cc440d3adb4 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Mon, 2 Oct 2017 18:06:39 +0100
Subject: [PATCH 243/269] fixed warning

---
 srsenb/src/upper/gtpu.cc | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/srsenb/src/upper/gtpu.cc b/srsenb/src/upper/gtpu.cc
index ddd695ffe..e01be1ad0 100644
--- a/srsenb/src/upper/gtpu.cc
+++ b/srsenb/src/upper/gtpu.cc
@@ -198,13 +198,16 @@ void gtpu::run_thread()
 
     pdu->reset();
     gtpu_log->debug("Waiting for read...\n");
+    int n = 0;
     do{
-      pdu->N_bytes = recv(src_fd, pdu->msg, SRSENB_MAX_BUFFER_SIZE_BYTES - SRSENB_BUFFER_HEADER_OFFSET, 0);
-    }while (pdu->N_bytes == -1 && errno == EAGAIN);
+      n = recv(src_fd, pdu->msg, SRSENB_MAX_BUFFER_SIZE_BYTES - SRSENB_BUFFER_HEADER_OFFSET, 0);
+    } while (n == -1 && errno == EAGAIN);
 
-    if (pdu->N_bytes == -1) {
+    if (n < 0) {
         gtpu_log->error("Failed to read from socket\n");
     }
+
+    pdu->N_bytes = (uint32_t) n;
     
     gtpu_header_t header;
     gtpu_read_header(pdu, &header);

From b1cbcec74289cbb5d6be3b3b6df1ed60629fbe47 Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Tue, 3 Oct 2017 14:49:19 +0100
Subject: [PATCH 244/269] adding pmch tests and fixing pdsch_enb

---
 CMakeLists.txt                           |  2 +-
 lib/examples/pdsch_enodeb.c              |  4 ++--
 lib/src/phy/ch_estimation/chest_dl.c     |  2 ++
 lib/src/phy/ch_estimation/refsignal_dl.c |  3 ++-
 lib/src/phy/phch/pmch.c                  |  4 +++-
 lib/src/phy/phch/ra.c                    |  1 +
 lib/src/phy/phch/test/CMakeLists.txt     | 19 ++++++++++++++++++-
 lib/src/phy/ue/ue_dl.c                   |  3 +--
 8 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d72bb5fef..a63a77016 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,7 +50,7 @@ configure_file(
     IMMEDIATE @ONLY)
 
 if(NOT CMAKE_BUILD_TYPE)
-   set(CMAKE_BUILD_TYPE Release)
+   set(CMAKE_BUILD_TYPE Debug)
    message(STATUS "Build type not specified: defaulting to Release.")
 endif(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "")
diff --git a/lib/examples/pdsch_enodeb.c b/lib/examples/pdsch_enodeb.c
index 844e769a7..98ad78507 100644
--- a/lib/examples/pdsch_enodeb.c
+++ b/lib/examples/pdsch_enodeb.c
@@ -320,9 +320,9 @@ void base_init() {
     exit(-1);
   }
   srslte_ofdm_set_non_mbsfn_region(&ifft_mbsfn, 2);
-  
-  srslte_ofdm_set_normalize(&ifft, true);
   srslte_ofdm_set_normalize(&ifft_mbsfn, true);
+  srslte_ofdm_set_normalize(&ifft, true);
+  
   
   if (srslte_pbch_init(&pbch)) {
     fprintf(stderr, "Error creating PBCH object\n");
diff --git a/lib/src/phy/ch_estimation/chest_dl.c b/lib/src/phy/ch_estimation/chest_dl.c
index b5107f9ab..0f8ae8074 100644
--- a/lib/src/phy/ch_estimation/chest_dl.c
+++ b/lib/src/phy/ch_estimation/chest_dl.c
@@ -467,6 +467,8 @@ int srslte_chest_dl_estimate_port_mbsfn(srslte_chest_dl_t *q, cf_t *input, cf_t
   srslte_vec_prod_conj_ccc(q->pilot_recv_signal+(2*q->cell.nof_prb), q->mbsfn_refs[mbsfn_area_id]->pilots[port_id/2][sf_idx],
                            q->pilot_estimates+(2*q->cell.nof_prb), SRSLTE_REFSIGNAL_NUM_SF_MBSFN(q->cell.nof_prb, port_id)-(2*q->cell.nof_prb));
   
+
+  
   chest_interpolate_noise_est(q, input, ce, sf_idx, port_id, rxant_id, SRSLTE_SF_MBSFN);
       
   return 0;
diff --git a/lib/src/phy/ch_estimation/refsignal_dl.c b/lib/src/phy/ch_estimation/refsignal_dl.c
index 4de00039e..9adbc6c18 100644
--- a/lib/src/phy/ch_estimation/refsignal_dl.c
+++ b/lib/src/phy/ch_estimation/refsignal_dl.c
@@ -228,7 +228,8 @@ int srslte_refsignal_cs_init(srslte_refsignal_t * q, uint32_t max_prb)
 
   if (q != NULL)
   {
-    ret = SRSLTE_ERROR; 
+    ret = SRSLTE_ERROR;
+    bzero(q, sizeof(srslte_refsignal_t));
     for (int p=0;p<2;p++) {
       for (int i=0;i<SRSLTE_NSUBFRAMES_X_FRAME;i++) {
         q->pilots[p][i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_REFSIGNAL_NUM_SF(max_prb, 2*p));
diff --git a/lib/src/phy/phch/pmch.c b/lib/src/phy/phch/pmch.c
index 4ce869b1c..99e40d50f 100644
--- a/lib/src/phy/phch/pmch.c
+++ b/lib/src/phy/phch/pmch.c
@@ -394,8 +394,10 @@ int srslte_pmch_decode_multi(srslte_pmch_t *q,
     * thus we don't need tot set it in thde LLRs normalization
     */
    
+    
+    
     srslte_demod_soft_demodulate_s(cfg->grant.mcs[0].mod, q->d, q->e, cfg->nbits[0].nof_re);
-   
+    
     /* descramble */
     srslte_scrambling_s_offset(&q->seqs[area_id]->seq[cfg->sf_idx], q->e, 0, cfg->nbits[0].nof_bits);
   
diff --git a/lib/src/phy/phch/ra.c b/lib/src/phy/phch/ra.c
index 418aa1260..be10c304c 100644
--- a/lib/src/phy/phch/ra.c
+++ b/lib/src/phy/phch/ra.c
@@ -466,6 +466,7 @@ int srslte_dl_fill_ra_mcs(srslte_ra_mcs_t *mcs, uint32_t nprb) {
     tbs = 0;
     i_tbs = 0;
   }
+  
   if (tbs == -1) {
     tbs = srslte_ra_tbs_from_idx(i_tbs, nprb);
     if (tbs >= 0) {
diff --git a/lib/src/phy/phch/test/CMakeLists.txt b/lib/src/phy/phch/test/CMakeLists.txt
index 832f18d1f..6e6b8c024 100644
--- a/lib/src/phy/phch/test/CMakeLists.txt
+++ b/lib/src/phy/phch/test/CMakeLists.txt
@@ -159,6 +159,19 @@ add_test(pdsch_test_multiplex2cw_p1_50  pdsch_test -x multiplex -a 2 -t 0 -p 1 -
 add_test(pdsch_test_multiplex2cw_p1_75  pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 75)
 add_test(pdsch_test_multiplex2cw_p1_100 pdsch_test -x multiplex -a 2 -t 0 -p 1 -n 100)
 
+########################################################################
+# PMCH TEST  
+########################################################################
+
+
+add_executable(pmch_test pmch_test.c)
+target_link_libraries(pmch_test srslte_phy)
+
+add_test(pmch_test_qpsk pmch_test -m 6 -n 50)
+add_test(pmch_test_qam16 pmch_test -m 15 -n 100)
+add_test(pmch_test_qam64 pmch_test -m 25 -n 100)
+
+
 ########################################################################
 # FILE TEST  
 ########################################################################
@@ -178,11 +191,15 @@ target_link_libraries(pdcch_file_test srslte_phy)
 add_executable(pdsch_pdcch_file_test pdsch_pdcch_file_test.c)
 target_link_libraries(pdsch_pdcch_file_test srslte_phy)
 
+add_executable(pmch_file_test pmch_file_test.c)
+target_link_libraries(pmch_file_test srslte_phy)
+
 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_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) 
+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)
+add_test(pmch_file_test pmch_file_test  -i ${CMAKE_CURRENT_SOURCE_DIR}/pmch_100prbs_MCS2_SR0.bin) 
 
 ########################################################################
 # PUSCH TEST  
diff --git a/lib/src/phy/ue/ue_dl.c b/lib/src/phy/ue/ue_dl.c
index 583368b53..c4e2d3f6c 100644
--- a/lib/src/phy/ue/ue_dl.c
+++ b/lib/src/phy/ue/ue_dl.c
@@ -631,8 +631,7 @@ int srslte_ue_dl_decode_mbsfn(srslte_ue_dl_t * q,
       fprintf(stderr, "Error calling srslte_pmch_decode()\n");
     }
   }
-printf("q->pmch_pkts_total %d \n", q->pmch_pkts_total);
-printf("qq->pmch_pkt_errors %d \n", q->pmch_pkt_errors);
+
   q->pmch_pkts_total++;
 
   if (ret == SRSLTE_SUCCESS) {

From c1ef157afd2fa9fe34cb4968d2b0ea6ad8dcc94c Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Tue, 3 Oct 2017 14:50:58 +0100
Subject: [PATCH 245/269] adding test files for pmch

---
 .../phy/phch/test/pmch_100prbs_MCS2_SR0.bin   | Bin 0 -> 184320 bytes
 lib/src/phy/phch/test/pmch_file_test.c        | 204 ++++++++
 lib/src/phy/phch/test/pmch_test.c             | 475 ++++++++++++++++++
 3 files changed, 679 insertions(+)
 create mode 100644 lib/src/phy/phch/test/pmch_100prbs_MCS2_SR0.bin
 create mode 100644 lib/src/phy/phch/test/pmch_file_test.c
 create mode 100644 lib/src/phy/phch/test/pmch_test.c

diff --git a/lib/src/phy/phch/test/pmch_100prbs_MCS2_SR0.bin b/lib/src/phy/phch/test/pmch_100prbs_MCS2_SR0.bin
new file mode 100644
index 0000000000000000000000000000000000000000..276c6ae01592534b437a2f2dc59a83485aca6a20
GIT binary patch
literal 184320
zcmeF2hd)<u`2Vx_3Y8T?NQ$Dw`@T+;qEA{>8bU)wONDl#jI3lv_6lW3@xHI4NKtm8
zA(b*pO9>HQ-`{`ndpy4X!8wm}-{*Rb=Vg;~1H3Z!V)MTYSnrdETUQ9P52_O2$ZJh@
zvDHQteA_@@y9%>uj*;*_Ll1A9dI*6bPf<uTl<3^4qOpn^n7e!<=-yL;{RdWK4gV>y
zKlFkux#5d#-~B*Y;SLse?ni#kHXMCijf*$epuArgs-B)8Z;u~^X!kOxzWx?`kH%nR
z*eRSXJp=44LeOy4LMC79YsJRMBC3`k!ybAk!NzEQ!tOgk__`?>F1s`WpIkLZcpfkg
zLD~2zqJfTYOr%G{m$2UcrZj5n0Qz5i%(d(Y#~(+fG4J~Xjh{@ScguLV{-(*$FUOCG
zP7bv1NeMaFW`$ZiI$&E&G2N({L9@;Zv20H*Du3Eaa?|I)uU8XLP(EU|a*j0Y?TN#e
z_i`Y>yPAsFKF27P?O1q9oqgT&k6fEu2PIN*kY@N7m8AtyqWlOOsq%|6-E|S@w%QWG
z`)lc7em&{SPQ%Z64P29a1spD4!oGLCgF-*Kv}X7S`BQQU9)7BXf`xq;ac?%PS)-0#
zpY^!>8;!86G?a?g{fAOXk|?Sq&YW(Yg@Pc*9M9YUk82*l@XJCdS5e1_KPN$C(R-X6
zcmkY5hv^(!KQPbR#8!NEgI5Xr+33rcV6eFaR5SuH;z<rH8oP?6vZM6e#of?j?@7ih
z<6!ZxmC%rx&Pn2xN3R(}+*)kJj|;zoYxQyX$i4&N#S<Xjw*&UY%fVc!VUP$|4xgf3
zD1X^b5LhqFjtbs`aq%*4!^CAEvjuQ>)>@j>rNX+Z$it>jfq3P_D{|&z3=Ao$gY%#z
z2`O*}WuHo18jyr*53Au$%Xj#oPmcyzQk=K(iJA3Ff11hT!kvG%4z4VBM-!=^)ZcL>
ze6{t!-kVw^MdBXJ(-2^DKU{*!j*B2x%2)pD)HhHRw<cSQHIe^K2<RFrAR|d(zC#+t
z?m)8jMingSTR|Lt+B1S$mteKrQ_x*913Zcw=;I_|G@5OQ5ATWMk9GX$nfHSeA|}ix
zIoWbo%L%ZmZv1d_mmu)V{h+m7y%?`A&n8yOv)L*V=%M%(E++e+h~*htm|Bh>hoxDi
zw;V=|d!3yB`3!c-?>0TW{}Zwv#^~sH1o+jra{}v=z(sc@SXaf9M}5;k>y9=vFGrXo
zvqqTFSm*|YKhDu^oml*Iax1)e^aU%#K7seiNLW@MfmbU{5L@T7e|x0y+x>X@U%+jc
zyz(4=WzJ$ZuDS`S+fvbca0Y(xjfNiMO}Irv0d<=c$YE<KSX7K8sWgqY{SE~F=^OAW
zcQKlZUId9Eem3QNH12Ue28<9}?$_i^gsoM;`O#dom0!r7e=E;eWNFjlRhuB@&OG=}
z!k4o#bO;L%zJrThEAZ?bSv1X3!`ZV}a=%#WU;x9*=q$NI-`LG0V=qI=c$4okKe?Z9
zaQhV^U6e~{*IVTKTuVb7!ttnJEA*r_!pWp|dcHLgx{Djjt9=r2_O%}*Bla5IXT2IP
zh&_ZqhC-ZK4dyg>jtcrcE5P`oBDhf<NrN4Q7~KU&N#tA|w(;L8%rA^a;rIz^+9C&%
z8#I}_hX(Na+-1z;z}uvJ*D+XX^@z9*T!35NU-7R+G;!ngr;<+*o-gR<thCc%bXQ%6
zyiPvWC~_s^7WfoL8_$A%vH|noSfbg$JU^NzRSKMq>#%a{KF4mp9CQD~KWclh2`+yL
zg6L)*#!mY%?U+o0>|=M(+E)qxDZMAxi6pFR)q}#iWw>!#0ZA_oz*j?0;Ktpjq@+`Y
ziJtWxKROpcOfnbAXL0m+lZD2*t7*oWaMsh=i~b1DK_{IB*mO1peAflQhS{ntkH0U8
zpU=hpN`h$IrNT&0vm{$*&R~qEbzsr(4Pt&m1)D{K(6+Y(j@jQtZ@<`z!IC~)ZM_Wl
z-nxx}F8&xK?*uu0{zS-%kojsw(BGtopXD2g>*bp;%ke(!J#7otTLPecRtg%r?SpvX
z5NfzbmEju^U>+FlgaHpZsM%4*wfZH(HZItWV_N%gsmB?#b^1;>XuTs}Zt~;wH}~-A
zqd~aBPe89|5k%-`!6?cQv6X)8*!O2}E5;qypSpvV<@@2_*-mOwtHAy)d<#a#X3#43
z9H*UJ2KA|G*gR}Qvo{Wq+il&DYQe+)>?tFA&3(}g|B$T#|6zjm54y7N4%zq29N&lB
zhTW$OnHxeWXl1$y{5-aUfZz$N@Q;J5TYP{fj^n&Kb?|vt1eZ2{C0Pr!8N0<U<ag{z
z8huQIWCU)*u1tFniTg~Gvv-1&+61V`UWcmQ8enP#IZU!X9X78b@_RqS^PJmoZ6Xl#
z%;jn8#X>Ta8x36n$&m5X807c%accYw+1jj`u={Nde%&-o_J(f8AI8#zZ=VSJYhD8A
zaq979FdyiLCg2I}P4KW%5MS#~gM`iP9PZ8@+P5N|1gGT@3vV&FVjYRSFZ1Z6w<Fpn
zs^Q%<J}NZt1P%#Gv7c`z<5iuqax=a_NOoBRKHLZ3(>jjBv>#>ayQo`r4$5xQ2Zx>~
z@GDUkRffE9%SS)l=|6`Js6N15{%(*odfo%UM?Z<~>Ix$8JCt1b;>cOJCKpyU=hK1y
zcSKr>kL|3Chvue7<m>Et;PIslWQ_$_(H0RnlJgnYSPa4O%Txc;#CPN*x0b9m$maNs
zs?(^APO#UVF>{YhL&H=acypR1f+rr}-QadO)pC>c&JClIs^`J)t{FVnv15CZ+~Jd3
zF4{>ra{oSD4eOZ>IxpCd<X$SlEfGJ#UN(xB7t69i%dXOEITzW&Mo;uOvmAU(T;SeA
z8{)mn3K?9CE5rHOUIQCC-zN!n1<%LoQeV&rO#$s{Phy+w4sUn90MP&&w5d@d27J@$
z@a!Hcd1!gL)u1VNKkrghG`T}7J(SQ(s0F^Q+X)}OIKwFSJrOMxg|8{P_%`r56TZ6+
z=JC#8c6!7^#+yf&cOVqC&Blncz9oFjNQAu)d*Rq(L8h%~1}b&@fRN6Y$R|CDR-QX4
zSy_g0XA~=TuTca2$2W1u-B}<R(?RD5CBnH+bI|fjL3y@VFc#hM#t*h`u;aKcmIa5B
z(vz+zydV|VhH2sM6+u{L@dC|$E3uW^So}A873=lUifEO|u#M$5IPd6XyeGqtdpY-E
zB$RR-8CB*%Kq!9TF@yP<Yq&jw|DkE1J*PRTmL!!IVnARJ$T#|<F%h7NopKo9@)4p!
zCb_4h?3m+sOvrnZM#z7KAaXRbqS#^@b1_4f(JA;#4g4aZ<LxxqA*zY{4Hq&7x}?H*
zL=#W)S#k$_dQsPXEyliE0Vkzp%?4iV0lUErI(+K}_aCPjU8ZMHvygCH^|gZVSk1=M
zQkkSDjt^$aDd3vnBfyvTh6Dvwq5D`6>`q*Y?l~JteDQ2%MdcXX(wh%Co3(I>%oRNK
zXf5a#>0$ZQoj$iC4rXl9U>B^7gSBC+*-_;UboUDp)}=)qM1sR{eM=Dt8&q(g?6D&c
zHLqZ^>;qaFa19fzK61*1_L9!@U>HkO=8nr<hVIfe<nEX<O`N|3VicF6*JVRIbaX2U
zv<G10y)-lH=AFd%VmA1u%>?aGS>}f2IdblbI#by@3++ASxz7qBa5TjlOV$T)0~=mK
z(`7lLaNY@IOZMT#fK<W=e4wQZg_wpv>R{t=9`Y9kLP>oY5q&pKnrbh@kKo4~^>gtY
znM*&(t6~-OmW@E0Eh)HCb}gQMpaXw$1sU<J#~AzmadK491U7|D=>)G)?DP}CHY-X3
z)+yu3PzhKTW(zia`Q)GHCVG!p!`!98xL30uISbzth4@))eW(B%J1$9L*H+W#C_)^6
zZ(%apqp1A0o!E9G9Q-P}aCxH+5cf~yZQTi)78XR8%`1m(O|wCLFbB3omyvI~s_30t
zvh?ySUvhT5jINek3`ayZWB<%h*!8La1eh*ZG<X>J3(gX46Dgu&be@EEj+4jz88B1o
zE-J~p60!cn;C029Zu5KtHFG|4tAuBgm6__C3&x_%kAnRazD(i@$z*g<TgncKFCm)`
z+cWJg0gzPI#d*lH2`7Jxux`$WOq;*hK<NfqCO<k66`LfOBM)Zc4J#R#>BGll4d2E(
zmvB-RAc!Y_PG`m6wt@dHO?Ib=6Sh2mOXoYDVB2J7!h4NaPCHi}N`Ak9q01`JK4!!!
z=RT(nY$TqI^C4P0teJun4Db#9fs6UkWU?Uv)VWr$q1T&!J5feseP83_gfIA~c`kf3
z<6%etWDu9A&sdq2PMJfssC0Qhm1ERUYFwYqE*^ra>(RI>q66w{E_0VLsl;84hkZWp
z2e{Bg@^YID;6NSOBNB@C->UJj_daNy<^pAIQjECdD1?nL%<$<MAnYN*#4hW=7wU6J
zkfkZRMp1xH@E)Y4wq8*De1w$x4*|=3qz;lZaJ4KC#6O?OcF&KaeCxMRUA6Djh9ghp
zueCt@m#-vWxE(^Xd4b)unf#r&gn9Sl;kk?qjCmbn)VxdJfy5~2eb#~EcbV9@HW>~S
zq|)IuJIJw1CC`uCrd$7of=zWWF0tIid=ozlue*hq%Ip_lzIr7DIp4$7nwi+KUzgnG
z$g+nPovZL2$R#k#j;&Ka3A^);k!*=4cwaLE-1uEEOkR^pg8+(;q=44+AliLZ4)0DR
zaV9-J5Jl&Mv`J8v31bwPRQ^OdSX&EQ2gRWC-z8Mu_X6sdKLVArYS_`|KsUdAPdc|~
zvXdGdM*a3b_;achI_m@(Pr+7{I8cK2U0j;Z?nI>rDX`G%0@Rt?;fxE<q2g^UUbLJ?
zoM&*6w|R)3^W$S>`z8o#JAs11e3JE0no-Wv1Gfvw(B(2rZWtW~@=6z5WTn96@ot!*
zZvs~bBOs0m0l~+sG4tI`Ft<y_@JW8Kk2;NtFLSAE^HvnwQ9zp70x(ih5_XBW;o5JK
zu%)w;n$|}U|0(_dkZ&upAAQhCAqrI9C*Z`}AyArBMxC)%RGQ8U>O*PtpR5CXDN<m#
zY#A&$_8U9LgFuLP4V%AV3k*Gt2UF_=NO;JhS%bnTFS>)Z`t5=m-}za?%BQqWwEz#s
z^0Uu3_#&2E#pQR`Ve--j?$By6_FeI3R9Oxv)tZP^CAUGScN(b-FXrB_9U;CK=CI4w
z1QPiP1NNDN3kq&6#V<P^;nI73_&9Dh8>})De-@{p$vjt3NU4P=2L)D<QN$k}*KzN$
zek>iC&ZsL^!C?6jY^~0KDElVVs`IC524UFk1DG|^fteu;TRCen)4H3F%@Ui(bbYP@
z)9_|&JUs*+C+?Dc&i9G#Xat04EyoP=VU+Ke1B>)otcr;eQ~j_NpRSDt_Y)DwcFBX)
zI@YwiQVYgzo}}B>FnBhiiYBxOp<LH}2<EOWTl6iPb5&Uxs)~8o!ksH%&CYJpT4RA1
z4l2T;>>jXRQpt@v1dNyZ3P!2*41QZJ%^W`}3);K<anV6VW^7{!tx>%N>FswRhB?A0
z74bp(!fdeE(}BK6jM%f$$!NDo18?xE1H=|^hd7~d`mzXz_em=FXtlvl|2(=>>lN8*
zHh^KP+h}?X4|W@G<hnhzgdJ8HL}=$-ICe#vT-aCu-Mx;;ScSvh=Z18@M-XU~a;fUQ
zFmzn#gtmjL*z`9E(8CpD6AiXND4WE|$TP&iTSmYHMbUqPVJMo@fr%A|@k^2@yP?Yx
zp9Ot1t4+~o(|_EgO$92n`MwnU=Zi42;GG9HEt-Q8xq;xP@|c@eZb1z8Ycc<>mvO@1
zm%<g_NDycoCsK#YY3tp0AT`Y5wXHR<Y-K;!TR;#lyu1vDZPvpvM_q2vo*i(kWC%~^
zjT14@rllbh)N5J;3^rDw=XfdZ<w>GXPUx|aAIQzkti%a{KXiDd7%RaEhE?aHkv@um
zmnXK;8S`F1MPV|!>F_h}Uq_c`Sh=9>nHsYGur(&sSEF)rH_4ruf%)mpC_XnGU1Wl=
zv}y<)Gwy(5zC5n@`3in(B@y<^bm$I92ivxCDsR^h77GO7+zH_5i>E^7uqfOay#m3X
zK7#EuQKBI%LPd@hqFRwOhH!6T*6FL{tinCi^s(XGG!4Zr1xYX%vIjggjmtJ>;%<pq
z_@DAo_UH^=NLlHQhpVr0qGp=lwgtDSSb+~)bi|)L{k@0nL36XgBZk<za1JERwZ~ia
z&q&Sk7VNvi#cvlv$x0_KmN-8&J02~JImN5l`YqE*wrvFprKEAfGG;U1f>ZE~?scd<
z?29E~A-H8LKfKx#Og0MNhC}@E5H;xoHbE;$Y3e0PtFF?dHx_0NB6I18?<%unOD>~V
z8wZaBe#X{2(_!b8<G5^^9IhE3r&~J|**L4E_&ki@)?z!75_f}^Ef)ma8zPW8HSZh+
z3P5_x8IYdkOOty%amLp_WOy~jexLuqU|}V-32Ws%e7c>Oo?Z?Xfj8jY>oM9H(p*t+
z-i*??U5KB8Np)NbsP5FEJ1mpnK%NN0W7uQ1^7$r6y<J8%e|Qm-J2yEud_KS;r5sp0
zpRhWgf@xj-0~EnsC@+1INoy>G5feqoKfI7TBg+sDPwBlE>;J;2d@$|`ctt%N9Y9!!
zF!#(mAk*k1^M3hSs;qYnx9_t@R^1xi<?q6Y!Pjs^z7JaK^x$3MYbvVUO@4(uz&9EN
z(4=skO7M-*^D3n{bjBSU;uf%M+y?NOmtNi>Acd#4%?6n-Cb)T%Gqi144Z}}Fm~eq~
zl2c`XT|)|Faf2N$yZ8>(gri`v;4P}y^^|w&i!wxad}=2D1}BvT&@8SB(t+v_OFUrR
zoh%T#be%|7I^i82eRxsqfSh_+`p~J8+<d8srv_~4{3GeGeK>)*$v*=B@o)5^(Hpqq
z7)izzu0gHMD5NVK!kIN~rpcv$sI%jJ;yY~_Jc2QDz@?dFzwN`Oo<Nk5y+GwP%2B}E
z0ba~iBr2l5*zjL7`Q6IPKK-l@cbq<=n$HzT?!AUR*2d`ZYa#x69g1Pysc`qU9?8&e
zf$c8?;X}V9S4~3~Lgllde7OhO)|AtjH9OF*o3N)}-!f~_v4KZRmcWU==is3l2_}o3
zI1vZ$(BjZE$Yn-paB`~YDpv(i-tL3FeJy0&$WppaT^pM5FKsTj<?h)xk48_sL|e}P
zM>-!Zg_Y+5Nt_Zt+D8eY>8hDzE&Bx>wImpUqEx!{v>RjzpM}Ex;xISEmk6xkz~lZ7
zZbyqd?*DlYi$os~8&;9M93a6MUs#Qcx;4>5=`092OrSu{PtHJb8(IHtI%|2u4!_wN
zusM^C?9Ig?Sf|;9sW)E{y*$l|XT#M*+Q0$b%GJ>QNjYtzHPo)v5>8~!AbJwpV3%JI
zS)oly@G@&M(<z5;ODm!scduaA;fuhRbPQit2{T5kUXc0UI^p{rc^LaJ1|KNm!MQ4E
zr9F#v`=r2z1g}CLuQl+%#T-tTK82F~72Fk8F*GE43{TFDq$j6W;fDPez%G3`?lzr6
z3=U=EV1N$0Sj~{r@?sBtW%Y@(W63mTb}M0fFCE3{8lvns#S{qd<Ok;`gmD82%r00+
z%|^u7t6lX_%WJ?+rYJBs3I*67cO~eWLqDkS#u~D$hT!tR6maDcX5&@)nZv(cW8|k$
z2-^6CY+u}ihLL`huQ33H+l<-%<G10|`Aul4y@+u!lR(=JSw`lv0vom23WpPNAgaa}
zN3`_d>638Km}$;}^k!<Z=PCG9_hNqSamKhsjVzs$Nk!V6;8?yg>s9fkBJhY1t3Unr
z)VZ>Pk{VmE(%s0UED~cU>Tgb+H+435YXSbpo=5QtWm<AIi%8ikK#B4q{9>)l7K#m_
z&Fhuyv~Tt}J@p-KQpvy|yEK{+4`6Y73p1H|f@y8;fX|kjVcLtEsJ+4uExx3|N2o^q
zsr#41`G6xar?AK_3ljgR(z%c4QoE7?TIu!*ZJzNm3+K2(?1wPuFjAzC^|=`RYd2Q@
zEizM?mV>pa44uaJ9e4lZ0W-;0Fs_nMU;PnhZUrr3O_*~aE~Nma5x2p1cQd@=Y{Z}w
zKgl77kGNz+9u94KP2HnKSsA~_uuc3bw$%ND0>2)(yCju+VtW&;I1xxZhLd4^rYao#
zsm^`tEDM%jZb9xuDvTaG3QK{DpQ;bBT$OB`_RyG3lb2<p8`sdJ6|&5h%0e>#tp%J4
zGl2W-C3@cT5x0533q<NfVA`T+Xj0cj8|e(%`MeITW;H_QW?3e1n*&MhQedK+_*v7Y
zLi+My80mOt0VMV|Cf-+N5)BmC-6MRARMJ9-oyy=ppZg9MW!8ep>q0mf6oOUK6eF50
z;@*inxHU<dQH{C-@n_NiXGr3TqbulHQGc{6spC#Nahf>ren)+c<uLzv4wjy(gao~L
zRG^SSYXKSX`XPaR`ps~d+XU?;#qiydANgJ9FjrIFp!K73@Stlx`n$21e^4Fxc_+x8
z>CTY2Ab_6z@R{bX$il*EOKRZm5Bpq}<HDR_Xc+#AP4<uBwviAkol`;$&dg@h?meKE
zrEe>EZ?cdq&yPOGuYp^eCi@`Y9^~gv&9n<qxP9~%8ECtXlZ)Fi<EbjV+N}d!5_{S8
zD+ajQxAfWnvYb%BvjUDK$pYW)_o$b;6b5&RvF;mc$k3k6_~!T+)#%q|-}mn13@%Y(
zf3NJOZKe5BUb_#oXDHC_&#U375f2*1RRFmV%~>*~{~i9BGjAh)k$96QoULU!=%vcA
zXWV^pZCxK{!*6kTx@8t4^eUQjsrLtbTiDBa<n@7c>~%#rWe)ncUUJN`V?itA8<=<R
zBdK*!xc7ZH{5&z0!DJkvhw~&MZ6z;zcHA3I^(&%o`eNo?cO|O5Q)D_GB%|Kf5Yi&a
z1BGI_#9s3NaqfS})j0kQ=Kp-m`7_DGJl0Z#1%iZ5FNx&@Uw6m;J(A#_#KmhyqU4O-
zVLEStBx$|+8XEO#K<sb|jFp-}#LDA%?b$eociR^fd^gEi@@Xos$y<!p3cQTc*6SqV
zV;XWC)q%-)fWet}Nr7oE2^|ZDW9tV=B{_>R!lwAAwh#a8|4Cy7j*ueDzcexF64v>c
zvSP;m_{^pk6K|-4rJpbG@i*YYE-i?Y^}{jh1Ii-8%+QT4Zbyj&49$$eF|q_cwDK`E
zQ3EjgARj7bF!ZACbavI=>D26I3A$ZLf_LgJ^l+s-xVt^Zt0EMFQ+lzhg@-A*|C|nw
zy(Q+Q7dcX&PmvijB_TIx2JCZd1>O9kF!LD~ie^thd3!Vrt$Rb>=xTGnhnceZH?wi<
zjvl+2dxic;i=lE~FOhof4643rHOOC#A}8`=>Go-}nYY{(tn|~%P~XTwF*hGnnd&Nc
zIjqM1X;rxE(mkxWyA&Ua3!}`Y68dXbD-1j5lDG*k7!vk_%mQ8%kZPqqXL#^i`4f^9
zl?}#+VzG9~FNiv&&CcnP#lwq&A?rUGEZ}`chKB>-ZF>kVQTWbL)aQkYhcmEWAOI~}
z%xIHHFiupyqd!%@Ld}aH%zv_&8Tq`F7I5|e|5-=iPFmvFvM%~FzJoJu$d%M(M`EMq
zCeZB(K^{dxHs$IEC>Kw}PzM7luyQe+>7S&t9Co4LlxDcEq>k#%Sv0@Ogc`;Nlf8Tb
zSUpxsrJlRfs_LJZ<{60X*5AlA{c{-cQ6JW*B8|@q<C?2wkzDo)4!GsR@zZ_it^c0d
z%};`ngB^HiW<97pdrprUOa~KZ4eYqB0ix<*7^x@3`nW%&5<$Pve;qGLszZ9-%ap!5
ztc&KfhI8P6ADN6VfMMnZ9%<SDU%v{mPXhR`{bUMkE#8L*%n7WymW1P?{P^Z%6|H_z
zM6xuebjXESz|ok=N*8yM5BksWfjbXyy;j5Arx{RlssW1c*+RI)2w_UjaivPysearr
zNei<F{la+YUHb~uMn6G8)I(@*ctOK`uYi(KA&SPD;mDRBppX!SN^;v_XUPo?l{tVj
z@(Rh}nf`EwHx6#6WRkk05pd?O9ah>t#<1GcW@%v?QOf%+ygMSobc;QPoN#e4IHAVD
zsHv`VXarO|Iq)=0l3Da@0~{-zhbmDe(BymxdU}GP-_Rcp_O{UQL)WQ}WgusK>v0G%
zGN8*&bz}RN0lI#tEUR$Hj`M!dlk>xB8iW<UL?YS_-a=+%hrlV)v1=6*9kqj|$dr?f
z^-&~!p&NBl-^>b`9EbL+NthZ`MQy(FGp9N(;ypWERtl$~hRYDnuPf&~&{AR?9~eVi
zW)4U{z6?pD_T1RHCqYwRoLzTzl3wuCXY*!;;hb0%Q2ppjuVr+=^)qkL+a`-tt`%lA
zH~G?L&jPc0xfO7?@eqDKoef5=bvW5C4o8k{!?9nwX!kxC6V86cv3XCS*@T}IT^fSW
z<LAN0bvEu>cp5A09uxkhmx1+Ji<kHt;DAs*X09G2RQ4ZnsvClkKmC}!Ii-B#8EcF&
zUq&ax_?Y;*IZT-6Gva*y@|5<cT#uzIICEu#aNG7h#Qv)sWsT#>Df2Ci6vqIhK1eV#
zT^8Zjun34&GortD-6y}Deds+yg4^Eoqt4<yuwGtCl6(Weeb^MV4@beRzX53NA;+xs
zl%>LYpJ~_febg}G9F-|Kj}>GTHhc(zHm3&=;BlF`Tr2<wJohqMQ(ETy!fW8<x`OT+
zo$5@d{H@Z~9gtVhN%YD-aXFuXV|kB)tzL7*ka{|Zuet_bvvpv1l_xmWw!yl8`lxXH
zNQGBJD=8y<tX9bxJa@K;M)-8&hSTzp9sUA>x&ra#Mh$rNcn2Q1pnz6N&Q#FiC2UH%
z!Rc5K2qSAAfW`fp_#?NNYNe>7q(=>go|pyZwFS_uE&|fAeX!Re7j^yPA#ryEjjz5&
zq%AbzYkCaMsX0P355*Aw1&KsPQUxaeeWx7%yX0ltZYtvZi2iNsfY%xeSj8Q!^nqF~
zNvgPnZEgBY?OqcmE@>I`W#}{WSGhveD{s0_!HK)xSCnQR9H}TA58<}?Wr9uZBI=mv
z$sLVTfn(xx;i1C~bQ~$+?04?LxNEJbY1%?23$&mo{*{?rs}OU0Tn2dxo`9aLES2St
z<P6j&QP)*P;5={=VtFnx%O#xPyoM(}_nwbMDu!&X(k4#TR8Ov$sRwUoMG)iCQQ9b|
z2yKR;crIZZb9<*pxw+gJ=lWm+?fA~ePBaOz3tY7DibXUhK>a+ay_5&(5=-#R$Z}la
z^aiWG^_rPK&<8>JaBf{n9yE>);QCBKcJbORQ!{uT`)#Zd!Xgh-%aE%OyIu&nH!pFW
zBxkc0qW3YAQ-Ky?7Tg<3yXm@&@6Zr)4$Wknz)hj%KKwcb9UHHM!;0thlcyo<UA-8i
z`u0P-YaC=84Izi_@i8ZAZ7@kX0rn~xlh|ZO47vJ|7_6JYmT~>jBTXLX`b*H8D+1u3
zUM6hadDrZt<SjaT{3j~=@?eV4T=rSE9~rn=L7AU=xO7n|#L9l;j@BfSx!ZrxJTHHA
zTQ0}`6Q0MKXiegtExA~*ZxfZ?C(7K{NP^vi2RKHFi*fU^M_AKajpJLVo~=Fx{-*j;
zi)a1xt5_Db+Ij+2$7?{w^Z^`79;BIxk?3+U7j27_*jD>lxOm%lGz;&iH|9&Rc9;MX
zwj#uKqzLrR-ob7oS;Bea%X;1XkDT9Rg$5**)-NrFmJu)P^V6imVVShe*NIN9RpCm_
z5nw)gzNOz2pF>$#AyK%Qf?}CI5R~x<am^&T==2jyT(lus;}V|!WLja;7!RU)W%Ske
zc{m#H4F3(L;`SFTIuvVSYt<!IQHX<H1)^-Lb{%l5BM^H-$b>rLYH1T($aTXHo9;qy
zgb2}%4Wxy4-VnXB&ZJZG9#(z04oBBN!W%<5m^`qF6h2BvH_j4{ZpK^e+Rh8_swZhd
zP$pc8t_0VOCeUgWfS>E)z{z3>ES^7~d0%DD*xZz1Ztq`DH_17ob>wWkBaw!@t31f>
z-A|_cyb~18$R<B3v>BUuQTW!S9j1-wLE?k8=s(a*f0>D}L(i__Rx>$#ZQKRNY;M41
z<=b#~JRFWMxI`PI1sL(=iE#Mt5ZN5}kL(H0!}&Q0IAg$$Yb_BAwfjSGHU5D9lN<5B
zb7$$c34Lha`kaJ1@?)l}38&gs6RJ*4;_&^0AT$<>=`V!nnh(=)ab!6(mHOagZ7%%q
z%cS=E1L3RwToM=fkvr>B6*qX95GyI-h(C8rQEt~_RMiO~a$H@IjdH`LfvF6U=Q0SN
z<YRjmtcO?U458rO26UWm#JuGfrS2QF!AEfo*rg6sjPu38>O;9;ooNe(Uz<tafD}o%
zVvc@_m$`4GqG+N@Eiq9)M71W&p|JD<ju}-GN0W#6@wX31)Llk9W;@8jEWGk8VygT6
zb??UId<^xj1*!hqobD&VFucYIj+>X#=5>p(dQt?G=N96ZQgv9r(-{(M*3re$;gp&3
z7{M*^7;^Y>`HSMtsSK!-);w7SLoGYVX+;6NAkjh!JY3*zy&r^#FMy_RO~j|#5Zvcw
zqM^iYuKM|0=$*Q68p3wW-~TMA#P4Em)|^r77+Hmv48Nnoj#?PB@x-s*eef>wGY)SK
zh0Z1;)OGEr0iN42WW*n{g9I@`Xdc~NAj{mHEy20}%pTJI*nxM`Qv7DH9MUse$?U%i
zq50Sa%;}$ibn_SZTP_gtV%6x*P9Zp3We@xznmDKS7HGOIWmP^(LbbUlyt2Gc_8w}3
z9WuM%)O8{DMe1QR%vp^+hW#jhe9E6GEu$lvKe^R=_$YsGA$BxNo2`0l&A#}U3Z;L(
zqq;;BX}jwWt$t0MvpZ|SV&Wt)FS9|VQ3|YgorAFJ!q9p>g1{M1&Sbs|oxNzv16Iiq
zvu_==E@(=p{AxrGvuz~BKFRFFs$G!J$b%kFBsx?$;0MVg$ht+rq_+q&TPcHlH>)Op
zzU9;FRz2kR^qI^bts1Ztbi&k@QRJph;yL}N6~Cgq0PbyL(r;Sh%#v$xrbidGrvCZs
z2UM_FEDP$YH?hN>qKxm{F*Ll0P#UAjHIMP+&RYA6e9xT$`A4JZOV8!>6_bNwDV20v
zyczk%`3r5kXX9Lw40Ua>_|#-GishY$@jsvNNBey0q#cAe&*x+Rsr|6W-5661Uc$W7
z&ntp$*D!hBIS_x!gc&ploywNI&6dcg;nAdr#9lEDUA$A^=Jp3zx{wme%|uRGABjJ;
z5xwYk*zq%`!no@)9Es{O+oqOIf`-M&P@5yNYg8CLZUMGNor1ji>G<KK9NRnBg7Pvq
zaFd`Hk$iLkt0i91P%%%G4>iKS)ls1CD~~){{GfE;HgyjDf}cg)Xmz_7yDHNN*7v>S
z&?^lvP1ha1{Og9q>RMdxn*n|XZTKbd1G=tCrImk1aiy_0Jy{<NW!*tw+8x6ETD%r&
z2aZt#gB+@oJRSJ03qbgBDQc?Of}pDixT<&Jc(*vS++ZWlJNLd~=D=5?pFcq7p5F@h
zE^elNy;6+79!s{lCcxzRU~<3eDwj`TK01F0hqvYGWN`XI<TzZoUvXQEy^!NaveL82
zX1C|SNGQ-5mmb1f?MHZO<Q|-T&_!*IOT(#8n=tl!34HL9LhU7TOrwbvjF^ug>Q!)p
zBckxY>v*cAW5Fzq+X$!6yyc4h3IYEYs`%1glGZCJF;PW88nTKo)w>jUm7>_kRx2<r
zIRs^PaM8ZW2!<o>z>?^97*Kf%`d@}(V{`%M;l8WTUL*`{A1jb!P>lywHn8`OUqerJ
zD;0G9J=MuZOff3&L4z+A1wA9c|GqSPf4vl}`_M?A1^>f`HkW|UMu*MIxlHEzJmMy(
zFUKz`LX0)D5N#v_;Fzlbj`^*`!rWZ^*}I1h+~a3|yQLECgmM(r%mb}6;%KiX!qC0;
zXelvD+um8A)6uK=>F6e6_B$3b17G2HZ87F}?M7CC6=SO2$Y4U69ZgiNA**IBz|Tv@
zO&c5K8RJ>=h^mt=v)Wvm^@-qT8aGI?kA1A@$?I<_0-sJ}%Y&V`1$?z2+UY<i4thgG
za|OQ1wZ{o>OLW{i2&sp|!17=M=cv~s%szJxCASn}%?evkIOz}8f2X|g={q?8vNhZe
z)?;;yys3Ho8{BEJjrG8IbnCxPPkzzI_L2S2Da_9{@%dq7uoZi}cAQk}Y)2bcQwXap
z26rA+w0!%SQz@_!-cIE<p8K=m7tbP&%&twCA}-42Ugt&2!8NR%jt1$v+J)DdEx6lp
z7#DBw1Dn7=*v2Z8O)`8;n6U=SAN>nEUzI@*zQCt*RhUe(4QSYK9A63ab1%MBAcB7%
zz=0`^r4uVk7o8S`H(`<Bv#fv~S5{{JH~(j*^8XjZpSdcso*<D@F~$0YK>4}X829)j
zHVBoPtzG7W8reH=Mbc|LQ=5Q>MGvU3Ln?|nUxCfDgxG)M7I4qt6|UA?&TPFJGu4GH
zWg>%Clk|`Nbp4(Y;#4*V-!Ij{sKfr?E8kA`=SeY91|Gyiy>+S=@xi;7)!|goZg5(!
zhc4ecKx2nF{^KN|@R?T--S-#s#qYqsyX{2q&^@@U^O^o@uw|;AZ$?p1p^CDyNC>@T
zgdQ{1*tXPO_+fXFUFUO{{^?u}ipR!DhZ-*)-Nnl+iR5RETbel~qk8m%-#Vy#qm7Rz
z7eKwv7gDt@f|H{71h#0YGsj9#6W6aYOjmX%8K3)tEDs+civ!X*mLEk)lhzaRCz=;c
z`1?U2;06A;_!yYaz8r)3H#tX-7Q&p21Eei17;iOfz~+aaa3X60R~#uNyLr<|p@aen
zIIP0>kU6-)<`o2YmcY8HF(UAy2kNhdQ$M>(dU{(nzHVSJZpAAaE#V65?uC<<Z^n$_
z+`XJkeh~(mpAt>Y6*Ow+PwKPi4UOFR2$z+HW6HoGe8889Qm+qEojPscH@{6I-;1E9
zs~^f{yu_8fcd7N4DHi-FC0j#%K|pB;Gjj?#UNc{#)5#Z9LbM)aqzbXPf&>1Wqv*=+
zIyBv!0#~XQa7O?6z!{#0V83-Gv`>3V+l>tH<ZD3;SGkX>V)0<}fBgUd@&EtF|Nno>
z|4R<7#N}yjRAs*}awPM(%JUb&IsJHakZ7gdosWp|eP5KdzX<-@V!0RYXrcDgCOF$N
zLSn0Tz^x;)TpQz9+Ap*jjU44*(72B7jlGJSC6egWtcTd*ZJ_OC%9!f@pf&bVgnzsj
zQ(BI|`i>kdnP`EP%}H1!_krVGcaU`I3c%Ji8T6>2Ab$R<jcTgz$kU5~@a3){l*qlH
zIo&!_KK&Z@8{dWETO(9P<{(E9HF4VLPE3-M;*N*!h11n7w75DK?b9!EUmX-TJMiZv
zx7)#*8NKX-7v~j2<<UztJZa&w>5{VO>$ig(@4HKX*G=t^jP!87oI3<-ueEYXR|1WV
zk!1${-6JVMTi}X&Inir&!E*))bdgXkcr?GHNq=*pbHROX(7Au)?<aLLv!9kEY==Hd
zp8w5NTWSyX%3Gku_<!@bAo^wL11#+1!zmR`68ZeeMt3E+SlPf8ON_@I`57c9<`wbx
zSHzfq49FOE)7z=KI8nyXV}E?He2FBRS9b-6dCjrZa3^Z#3&ZKJaon<Z>xfgMHPn}f
z!nQ6+%zC*6dn7vWcKdA{9!<x}@&r=9<Sq8;Oz}9;p}2HAmqzVT#QWx3fh*QcF-96f
zR`7t^!kJ`Y$1-m2W`3%vIz+@f+lkB3SwKgl$m7-GXd`xmK9W*~Pvc?acY7*b{=R`+
z=y1ieY4&94kPrNOtpw}he^HsBd7L9xA7e8qLc`e$Q2!zi=6R-3?WU73Z!pgc!yeNJ
zqnp?+cL~k@&ZWxwL3r-wUvO}Jgr6+N&2|dJ5~BoB92W!N-<J+4W6AU@$%FTcp3%wB
z9YjsY5H|?7Qth@p(%>eF$E+)f#>`ke`f;2twR=XBuV(_>dO$j>K0s)s9jZjfa^5-?
z5!=ZaTIK!_u2*}5tm_-ft-Ann&R21wd@G)vSdB`iFNynxNHiXK2Hd_9+VGSQuDY)Q
z-m0q*RHzE6iS=~u_YtDtdm8NrWk6*{5nN|kjymsW(lF(Vn?-9#_Hk!e(tQn{oLdQt
zmpjrKI-Srh5X5QU9!PsWr_=YVGN&}Q34S}W3vM3aN9lO=3cHCObU2qy<%-O)b44A;
zVqAr&9QQ(hXC(}t-Z`~1APwAYGoa?fFxj_akfW?S9sBl0!}pve(C0fsuJ3Gzan2U_
z^rDH_kM|HgEj0}L>4KK)))EDe4fK-v3D6^m4*i>_-lZ8gkGGIOzO|6jtPI<8u0Y?N
zwNTvongi`V==#Y7`>X=-mVzw2(r_RjUo?V3%X8W)!X<oje4ujw4$z(v2t#^`(Ba*J
zE-HNB_(v6|zrO*&?bAT=gf;Fn+Keu;)x^%@K5nSYg-;IMW+NGyIO+5klCN1zG4|8p
zrTT49yD3lS7w~{4(?|kiFM@&1c2qeWjtjnqaZmMhW9y@b^!((0&XWtTvA)+9Ra3vy
zf{KGE{=o%TeqBVP{w+YeU%zlXG?06y<r(s@E9i^`k?`#rAJZcKlYA;a4gcaD(U~&`
zgJPV_vTic4eX$U(6`w{ICDzjD3_DUbE0QbKe}UX8KZ=W&-$L0gUPwynq|xFrq|nqA
z<-aG>cI#`%F;0L2jkzdwVv^n^6rcME;MWB;ob28*u!)Z1s;-+3O{>nLx6f;^5sv}S
z?SpVAWe;i3zDEQ9zQyl9^zq=r8@NyZIA%Icb;q?kad^D3!tcQ`Tp4kVcFhmOa2W<I
z>g!E;jkw%j8Ku-MM*yr(%*V+_Lof;IC>LQ1xSN)^18Szx>#A);Izb+^mhrKN9Y1os
z)4j+x$t?VRTZm~pzZ4z5DdMf2Mi4%K3&|^vpgs2v(P``6(V`a@v5YB&mwm%<ezF>d
zFNzV#vPbaXQ6H7ix(?bE&w#fzjYOE5q41p?7}rnW#w6VWr9F41c+N~v{_e(&;u;ah
zjR#=cQ-(cVZ$qw6W$1smU&fAIVIUjnO(y;<rR`24c&l6veymxKF&#HS@0KWB;$IIQ
zt9mF;g*iyPyNt6!{}PuE0W_7P4Reg6@myg#rOV22Lsu`gI;n%vf~&YumgVSp!vdd7
zv3MPaWWg<h4<qXniG)cpu4@XyfdfWZnmv_Mskl`nQcdXEl4Vx=s}e>!{Ggw|1d5`y
zq43mES}SsaJbV)e&FlXWEze72U%DsfkZC6l)*giy5BHdD+#CmwhjVGq=?Oa1br~j<
zIbtNol{4Ze2!B0>DJMyRlr(GttJ4iQ*X0RO7@Wl=cI{X`#Zj(u-EBJi3UJ?_a)!zh
zGdQX#g3kNbfyLEQ{CP?gqDtnmPu$`mcFq^fJiQ4IC@distJA26(leSQ9}ChI$GJ)V
zer5tw-FmHX95jRl;L6?w+`wFUSUcQ{k9p=Yslj2uEBAoBUe--wKi@Dr@qRY`(gI>O
zy$!Cqa4}dS0&-3pqVe{n;HX$f#t)x`w7qknbAKA?*$@WKLjdDPHQ<8&JaSHA7A#!c
zU14e92V~A2aA?Q}*EgY{eK8hTTOoL=bcwWj3BYvEWn|}WPqW1WJ1eX#R+7-w0?6yJ
z5splGu{_&+Qo#2Ph3yxha7_wFXm%Ld_b1T4Kf$=}W)sGnd16?XJv8#JN3p_jvL>*S
z#>|#Qk&tH`1<A|sTELVpsud#x_qu4ZoHvy<vgSU$yaR>RW#OD6A}LNK*L(&*O(Yow
z4&}j}fmHmj+85_FuA%~$0*I>GY|MTpiDy=aBlk)cHRipFAI{f8jO7-xx_gM|O>kgt
zGY_o05rkVF-sfIxFQ%_YRoN|ujYQ&N1*(o-p|79n&=7YK=GxC?xFK$;*Il!oj<l!Y
zjddaP(McO__mCwTUbuv}#&?ji(KooTzlak)VM2D&Rd{x9IEi`jp34iFB)LEU9b0cv
z_y4-EuU?Gqu*gRH+n-=@$zhCg)P@;*cVm~xG&EuqrgAiQd{dN1i)Qn&4p~#*Q{*kZ
zyStryny(58UQf8$_0F7Er55lYn4eZC{eT05#wc^s4~`yEHj8WIkSwkWH$C+t$e)X*
zn;Kq|sPPflseFrz)AOKi6+&{_ar~3I5>_wb$H);mI%ug*O-|W!di?&-uqnPLJLU&1
zev?a1$h{yB?QFo}ISbt3^;op=HOZeyfv}c&_^HzrTGkZM!fCk}T6~4nZsvtGv<NI}
zA8?}gY=)t019W?02u{oABXeHg!1^9#G<%ebg-vNtsZxp0_qW4y>8V`;2Y<87b6oN%
zfk4@#Fg)<Un``DMkI^&q@IG%Y<$s-7F_0WX<_GS>&^I#}qrxzfH9r~_M12Mop>q7U
z?=tAlY{M<H=flUAov5?k91NKr&de$P`mX*#?oSte{MNCBw)vaj%$!>CU&SV%E?;P1
z2Ew+!XkvCX88(M~hq-ELbTW7^Je=xvOp`sK>iq@UuTTieZ}R9feNpg<l0b12A$n^?
z1S!6?0@cGZP&)}=YyEkw{X4}u)io2>3yX2f;j3seUm6BVE|P-$dl2%z4Su*x!&$do
z(JERLgRKqmZGAm?w$d2ZY}^D6f6b{zo*-5PvSiv6mmsM-Og!R*FkoLSjmhDG4_SXm
zotG6nyA*DEYh?-K@BTo4jh(5;AJ&D<Rs`>SEhEo18{^q~-elmUBzN;q4zbeKU@o{E
z$6<aR=HZt*?vi_1oEh(Up<!_-cf?4^>~&HsUb!|4tcvzvjPo6;*71{@yD<{QU#>un
z4sYz0P$J1?!tC`V4Oq}MlRkcM33iAmFyXV`z$@MY`ow(}d6GY!6|1_z8O?D<8rnwq
z<=1e0{*<9u_$WE~YzCSi*++I7NQ3yhaLnJd50zWnkQ3>R1}&->n;1_$qCat5M6Pnf
z&1=Xf{}eiYOA+3*rl3}}2@338h5y>D@O_ayb^Mydk$bI(U2cV_#H=Cj<<#Kjqzug1
zZ^HCHQpK9=Vk~Vk#v~0{R`iKI$vN_YXt;!ffbnCxdukVGw}uaOoBgF?i-Ix~IlQDT
zb3(wvY9rhUF9yf$GePHFDmp&ar2j+<G0HcbC@A?7ejabm*!ipYIdLI+@eP~Zv7Nd!
zCXtYox(Yo4H_$<GX^dF>h^sO00PTF^fN|c0-WWJaReAzQ`Zs>Yab$!<z1D`;zu(c<
zV=B~r`y<+v(ME9d0h0au6z$D_gneumJZ}Aouy-L-_Fp*(muB#SvnlQmm0*Q(zTp$z
z5SlLko|AKLIw&toAoEv@5x!V8i13dA3wb+O(&Z0UZO?J|mLUmMe}G4@f&1uH9&s|c
zY?jv-3gb~RQ1<U5ytfaB1l}#29|bb_c;iMqeyoCA{Ut?X^yk7dNk3HmWQ*aeXA-k@
zS7B58McDo3Ekr&mA%C~;#|m>F7(2Wgl@}!7=(5dl??Mya5qwS(CJSMQQy!Km7~q!O
zpD|tGBkn1>K~BEf3u<a+F!?pfY;$1*%H6nzhC$N!La2;HTvGwhC3Wy@cAQy>?HTwQ
zsRpaH%u&7KD*RJb#DOipIREkjK}JlBllDCyIL9|5Y|SzAx}b^<Q@MY;Qa@zMwZO^S
zqU2!3DUvaz{dS42qY+e!p2;r<&##BEt5b*5rm_&%>KudVq4}_F!A2&q+8DMqgptKO
zJK;HLKp_P?sQNNdv4$^(>{*otMmn+}YxM*iKix&m3qchXVafPr$t+X~P9q8@YPs%(
zoABR;-<0QwJ}B-DCN?XcgL3&i$U1QuokMfU<M@XdE-V59U%TnM4ONu)X(A1*lf~VB
znphnw51P~R>1vN*l6dbJ=Yxkle00~skyt}Gs20XKY0!%=o^OGoRc&Y{?@z=I%Q33W
z@yn+2)&Iv>+kNdqrym;d?2QYo5tm~WzxQwxn(WEsi5aYoL=NYl|3XgiqEDo<!38AN
z>Y$R~H}cWJ7JuYCrRv&}tY?8I7T(>9%g=hljJ{}Sxe-hcNg$r#na9qE%EkEJZFunZ
zHPBqO7H%#XBZ=PyK_}}oSJcr9gRe~E3e;v}$<h%n?`nU#&?6Xsy;_Fr*=~G{C3L@P
z16|Pnj@x$00JC8)`fA@tp9xCP&K2f6ML@&*+n~2e5me@MVBBXNq%~7}v?~fROQs0!
zD|5K<5;f#xg%1A8Y2c`6?!`d&5HQpAg_Rvsz4LynDGd@qwLXTDwCr(kD7b)U*JzUl
z+4tCdB!d_`D$zUkjigi{4p&x2;IDaJn2;D_I;J=uzD(`)p1I(Q^#x^QZt@Tb9Q_~m
z&h)Lu_j~_MG^sR4qvj%#B(<+~MTrm@ibRG;W+EX&lV+MzNT^KFfKcsgT_HsZ8Qz9u
z&X7=sRHWbi`6GVMzE69ykB;Ns`(Eo@=lMFr!D;m{8opf>-pqCrMW@MO;=tj!eVve1
z70W_Q#!j4jL=hHUu4lfN&Bf;C6j^bj==|b~pkwC%&y=L-mF%!8?}NX^V=v!jwGEBz
zYt%;Z&4gl@xJr*HD5%iDv%(y{wwq0{J&4x#FQV36d(=(41`lfrU{$IZe$8zIk0~9Z
zMc;J=UynPx(wM<U?VCr&4~>MC6OzC`ynt8?*Tz&2IbQ$KkkR?SSXjt&I4z}2rUlfH
z%NGl<XG9D9o+brJ*RDd<(PAhLGUvV{CCHXtC(&$7XVv`I5wPjvE*ND!Of*O$5!(Bv
zRvkJVh!;NvS1q_>jHX++h)?GaflT2BVUwT8Mz?4|zv*IlxMn5FYU<EXFMTW(J%_+{
z5kx&Sg5=reEY(Q?eQzHTo%`{J^`6WIW*H@#mnk70lW78{$L}SU{5!cYcOEodk4LqW
z-dMJ;Q}6>_!i)_!#beIvp<#46o=DOrod=`Q?N1z;Dwlx=M^A>HNHwh4O2M=s74)>K
z@$SnIP>DT^i*?JPx<moIPhEzM!VM*QuM+%v`xS>e93o-=H1Jl}In-S-48G7vJP{lW
zHa=@WCL^EKK8$0DQxBo@6@t54!r6^bUs&p843v)p#V2VvbBq$E3jWE!tCUThmP_u;
z8w&sBO$DEMBgI<op={Mn5eQ>1D7>hI$#PCCy(bxWY&F9~<9;kk?jsUb!i}z88w;P$
z043)}lsl4yFKsVjaaSf<4c{of)jx-HvT9g=LW1QMP9U~!Qjp)W9FGsW3{L)yVpWg%
z_|r}YC&de_hi82lv*|eMSAM~ytpZy^>NwN)130aJoP2(m18-;O2)8w5EZiOmuT&nA
z>1&FJT8I=4TVw{uSI>gEf5O0Y_HXt?)0Qo=4Hxa*F@ebXc#*=OP{=sDfDEcpg|BkY
zNcBZ4NY`46;*_H>Y~gAwkDLihHyV&9Uvn{`I1V>_66T!D8@RR}iN*aXIH5)Y0&fV~
z$JVc+oJ3`~CgTD}tHVj3|0|-d;KTL?|0R-6e@KFeLE$Pt!MnJel)g?8`5&&s38@dT
zT`OMH%%9+c=EZQO+tE2Lq>uI97y#7N0Ulp{CVD663;qQU*-%Yyd@_8$^WDdfNbAp=
z(7f;*xE;xWwvlBl#J>dVH;%`gS*rYTZV2p|?1{BHLf*A8-Z}Xk6I~T;AY~6S#iiq%
zL8kFA6rU1y`f)Qwe<Va~>-k7rZ`MxEr^&#Wp<9^v-dBinOmMC|dj?JnpURBhNVBN2
zJUr`{jfri8A-TyH-TND%_Kzj$Hc((;QpI?>zL71^n8bC8?-J7;(P)2ii)gFn7WSyE
zqbgZD6<3X1Ok}!LiQd~Ktay_hJT5#3XRh2M?dQkPqx*k5d!JQglgAClB#Bf!;x`T2
zzotRqy$qm+KZ&nG18Zt=#+Q>mvR}3utbTza`FP_Nk+e@CTcW##jPeySAZ$7m(wAZn
ziF1(Maum%QvzS5TNxZ!|4J|F3MXtkQ;1Mx^L(3E)V5SJZj$MP>l!vp9qVYt#@e0m5
zpaw}=Q{dfVfhF<mw4h75LFVG8Sgg5Fq#ANtn5`CIw&Pi{`HC*|Jf8=e#e-<?^Lrwj
zh1W?WNfgD|uV9A#MF5XiLwov0ws}F5s4}GmHI!De=YI;tBNdc!jO$gAw5Aqz+ATn%
zO9zSdqasWy9fYB#Z`g|D5EgAZh}JEV6zk6&Ko@(S!TMq4Z2g&N7Mj$;?lyd2_ro8+
zsonpfs@GBYyeCS?D6Plzkxy~by6L1<@a3yMa)Qo3vmsZ=e`f#Ag<zj(v0ln(9Aa3H
zy45={y~7(TMxO<P3r*0_HVrm(AHgWS0KBqWf&2DR@!!7Lu;6$wn{zQi@Dko-R{y;e
zJKeMvkNy3eDOtvduWZhC-sx~aT<<#yU58iTz@<HGY3~pO%{L(7EddX94TZP`+i`^c
z0ko_e=KR25D6yV-L_8ZV!gVV<6#X6!I{q)A&*C{w7?(=w1+8hV`~Y6l6#-{1#jxzV
z8u+udk_1>S$1LrOIR5!$yir+)T@S2Ld?gK_a}-D}I|plY%Gnq@H`E@UPKw=)#ZKQU
zV2P3$#CYhSPmB$C{U~5@p^J$9rYx{4j>Sv+ZbO=VHSxV1Ll&D9!ucje9=U%ns28nf
z+WQf<kFJ4e?`SB<{lMO8zlIq{M`Mbft?;|11GBDM;Du!sux9Z?Fnq8P1{n5|YpxyQ
zZS^N`?mJu5a57=F0uRUJ>QJ1ttpWBfmU2qb1FSoBQA|rF@XNUzbzWYAb-w4=_5y^k
z(F(AnEEa9PekOKPgnY`Pbdkop^WY>tOOB}T7at6W1AEC>c1tdWm_<%>9v@Z$qpdVZ
zMPoh8=qf|M*hqYre^+emu^#>O>OpTn3{mh`$B62D(C-<6idW6a49~G(c6$&H{WTF>
zo{fVY2ehGY7sBYeEYdco73-Gtiw?gUh`W6=MW2r(3G;3pKxi5XnzkQaT5iLQW@1#f
zPsE9PUb3hC%c1JnZuAp;QOm4_cPzwX^+$P_IQly?mDXp2K6Dd#y<GIZ8xBV=qv(0=
zIou{6O+Lo=;p#<`;bZf1*!nmJ9U2tK8CgR<DdYy8Je<jvYG>ez;K`^m{tpAQ7&O>6
zh}_*X1Xrx?Vs+KQ!Y-=LoaR4<&#%fz)ObU>{aGQ=-ZH`Y#_9;j?e$`3_eG+^g(&cT
z;fYJ{Eg%u7M!N@G!hs!XMErFMW38oVl$}a$S=z$(y+<+P^JAjXXNkodM&pyp&A51K
zE*mi>747$Ys+xK+P4v#?FGjw5&Ey^bV4JlXobGxG_8&)pv|JU8l%*IY?SixRXv4)t
zJ49L@&Q;y>FF{dS4a=3Pg(pph;8!TZD(Jvh&;0Q0oHSf(T?Vl>!uP`XWYveEi-}j`
zWL9dH$)@g11etfI;mGF-vENq)60Ze~V;w+L-f(o=e2%ovbVRd{r^)mFV#xkbE;4za
z!gfeqBaf!&aZ*x3rWQo7V-K#1RLdo3|MIn9oD>YrgWX|*@;+QNY72PmRKQ1WH^6%N
zC=%SF2dc>nS*VU3{P{JVgj<=w^*4t^=FP>VYHu7gy4@wtcT}mREkj#@9ix+yh9&bC
zV#F*XW)Nh?e!jbdO{d+VpmL=1<}o92?7dLX@f^oHLPYFpcp}r+XaL!v+hP0XbD(#B
zFHXFBfb|~LfREG1g2U=sl3!y2nvu6r^G~z*W26FfFB21$#YaKER}WI?e&$j49~(4g
zHJ%G-z%*GUY+o=4#qBY0O>H%#Ou0rfcqo|?QjXh&-<lWg%9vK3ihs9Xu6onr4`W#;
z3$$CzmTzk&?R&1^iP0&r^sgg<Ms@N({eRiHQzUgz8*aLH0sMxnK=rT&Vn|HzlmAOH
z^It#7^;%2D^z0{VcfZAXIYaSlzZ|>^-wn|V6lqh8BW#ZO<UD-ObbS3g68w@gv7kE#
z)_*vP(_|Idll|RHE$%<kyVw;^ycml^hfKulwwl;{IFc+HMxfH8ojLbgqfuuTdpVZj
z;Dt||U20CDy@M~77bh^8flrCP)pi*AUoqU-SH+q>trvR_sbUj1wX%1P6%aD!APhQ^
z31RzkK$;5PMvo&HCys?f3j6WHjZ0*jqC0Dwo5C8GjK-U9G@v9W90s}E6%P>?LZlsH
zN|i1yJw1Rq<d(5bvX<;}r$5wuU4S_QI>CBf1DVt~nwYCxVST2ue0A<o7P7X&IbzSx
zsu$y8LBi`ZzE<=Wg~x?DN680+X{rRgn139Wj#T1;RU9H;*y5P_K=x&i4V$e(@O#pA
zd_KjsvZE?RJR&Ab@O`fm{dMzY2HBG6zi9|5{jZ5AuedMnF;5|-IbEbDXoSE?xyDMy
z+c3G#IE*majmDO5MNSG2@!E%7Ol#mm{CnjHtCaY}!ncRPrf(_E`r1h#ks;*y`B=zZ
zq%Lrn+@Z>LD|>l#6#Sf^3JE4b<kK8s2CA6~Ar1MW$o&Fi$~PR8#`Gb3XUrPS)}zbs
z9C6s83-HRnnypo5;>6cV%qA!Z`%5EX`OJZ+ay%J_UmAt2*G{29$aIm@;&<e5LXGnP
z%MrM<dI(Nf{Ef6Z=;J(_UdRhpVOodJk~uSWfL+%#klyPJYenzjMBPMZgMi^MGgJn?
zUTtFT0|gIi{2>fbHWf4L+idY`DXf_N2GU3A!h`YWA?kf0-1F2We->6S*#Xxe=}{6|
z=RO2|xuxXMa0?8+qXqL8%|}tgJyPDkhgp2u14n$+uxf7~sRv!r9MuW<->mQA64Ov|
zxPqbhs@6`jP(KIm-rI{3W3H2Z)xFLg>2Xzlf-e8eu97tBE+bt(<FNUsDIAC}W7XW1
zOuo-x&&CS;FgJ$$?Hr6|c8%ottw8j<Z3No$KVt2ugDma&Wbw75i=m<Csd$#X58Spe
z!!uJea9q#=*#6`O{<CT$edU7Y80f*;tS5l4%2jgv<q%kAk;)!_CaC8&5B;k($fVql
z;u1?Ga0^NT7lr94-*gtvUk<{eKm`zOF~GHj5yWnvBzm`w!J$`nkzpmzaQ37W@@987
zCQOyU&pYx_vtu*6()%9a{dY1Uqa4#3|0Di3XGzMC8DzbWBz@&>jtxdawrfTmE7RSN
zBF_Z4@mB>as1qtycZl<%dayk;2=^ZfCFkDU$HbyZFrt4UxGSc?<^{4;+Q|n(|Bb+?
z2N&_6`4@6(@<&#Cp#=WvnZUoD>FAQQ0+$^*MRdEOag|mVi4J5ePU9ZBg^U!t#?`XG
zG5d+63?<cF!{JNJ0$9CyD`~xs5O+Qbv+wL>W`m>A>hN=>_V@vg4Ur_N7r)|<Ck1Te
z&mnC43N1myJ<EJ1e`hv-$C9-><Y3>OVhE@#fwSeA<n_5a$gBB}K%X2r)29H2=EW%S
zSf4DrA6Dga5LtkC3>jQgg((Ln;RF~730s_yS-*mBUqhLW>H>HxDucD^=i!pmZs`4*
zgg$T81q{$zTsiy}^sI3dPt2VNrnf#|@S@M;@6k%6sY{{!LMDsV4;Ah}cgTqu<M3X!
z4SluXjA-Z_U2wB&7v)IYK}pNwV*O1;B)Ii8CW%Eb@5>i)?}nk6_rj7Y_wPdE@4|Bu
z^o!X2dxAUWDZ%7_IT-DeMGgcB9;ZA@`ctk(6k=Qe-TANCw~!a?_n<w{=$8l8vo{c_
zLneZMzJ&DrRpfzRjUdD#kL~&H$*%MyLQLE;*yL~%<{wqT5A(i}gz-M;IOB+@V>^fV
z(=Ulz(Ie3<t_sWcuK?4oblAxHVSi%_dv7m6@}34`z?GL|!rfRh)u<4IM;BCF{HDd1
zjXD4(rx@#)p$q#iuEEj~?{L|LV3zVp7t;=IfEP<nSDp4<3|-A<&{Lurq9tQ6wRjj?
zX)_+``n9-;8-t!?Nxmgp3i`UGu=S+{?2qvW2R#Yc`mcj2_CyievzzdI;dv<b>m&!w
z_K_a}XF>VeR9uZpbV^1U_MF=9Y@7WHzi9o!Gb>^secw1{HCY?hoGv6wz6se9mpfHW
z_8M?e;y3FX`~gOUTx4~tv@uz7IrOTxV*kokc7Ih1+uhTEqlW~;O}j#TTeA;?Zsz0l
zhY{E$X-$@CKOj@gwUJ>4CU5G-=x_coeS9i}w2YyCQVLWhLIo@z*<hh+p0loO3YO$d
z#d(uW84IXFbIlEyGkB#a`1~<^Jf)1J-x-PjRw%={fcZH0X)u_*HY3~L1UM_cWU%CO
zGS+U$g`?B<VXf9iICnNvWN535qKJRu$5EMh%6u04RhbH(az}&f_cF*+t7j8lT&oJQ
zswT&`KVxc@7PQzchxsjPCH0$^!p|p~-2G!Z^xsh9Ti*_XX5s&`xob}*xaWz?zJ}wY
z3%RUpkTZTaU&7=Tk0*PcWW(<FjqJavCxttDD3kjyAH;=mq<M`k9K1gc_Mcdcx;nvR
zcIPT)B-MrL@iriSwn#iyxb2k5FQ=bAM+jcZcsLoGhoj1C&^fw`DDSQ(Uz5T?dsYN2
zsxCzHljq3!0je1LDi7R-&V-Jp;}Gp|5HoXki?;TD#rLF>XgAzuzlO{vi+e($BYhZe
z9lZ@++9EFQh{b~RH?aI!6WWwrA`i2_iTjTCvtR2Tl4<fIK-1(P@iv_$nxyiZ1b8bz
zcvTR)qk4~&ZS=r@i@Z^1!wwj}QVkCt{|9&SQ*pvmO|a<GMGw=v;wADkA$Ug>dvkR(
z45^hsciV1qA$Tg2vZLrd?W6d0zaVIic}bif$zqe74zb;4%v-08f>pIoiDBbFI{&3H
zow{~AIl17S^RWCiSUk?0Y<T7(7GyT|-=+^@A7X|*B@c*4+BPs5`jIWVQz!Bt_ZC|N
z=ZXsVF2|MmlX1GNgJ|X|J%0LSI{6bZnUtv>W<jU_fbZS|;NR0GI<nr9hLxWY@+wZO
zvA~QOWEu-O93)%sPK9yZHQ1OV%**+ENZ-D8kl!UkLsK(Z!O*8n>HRW%0InFo(jfcJ
z7#5Iz7-sAyqH#y}fTVpK7&X;mOz|wRmC48b?*B+hV>L9d|0Z4>R)u9wB4TS2in}zN
zh`QZnaLKE}*Kn5|cmm{@>{Bx3-7@%fqK~y~S;h`qeZ}eP%^~{57q+GNApQvo$5%#W
zxYpML#8<X}QiK?WPYEE|ttL1wMiVYphl_vlKxmS@#a`W!#W_DK*qok7R6JH8j#`&Q
z>a_RZmM7it#?6k+t^UXk+|j_goKn;;Z4<BB?nNTIQs9UDajebAAV0c@Fz@Swx6L8=
zrC-c)PkFO8A30oc%MDK4nh&3RRq?y-WGoIlLFC^*VACuWsn7PqkUqYiMf%PWIEhls
zH6tD84hlr6>IM9|+fQ71HH#_tx{J<zI0OG}jYiMsYcNHFi~lS2gn4sH#N(&ik;A*%
zu+rrfT4$9(SwuH;^Qk0qn=NpF-5(s8d5X+-*v%|wJOyQINlfUS3ulGr>$tHtdKd)3
z9f@XUM^*5#nH;?trU;jJ7eLgVV$v|NSKLLfiV7X1!E$RtWu?GYtvVsY!s#zIcDey9
z8#xhve{f{;;%h~ZS1o~L=d&=Yd>O2KZ;IVl{jqt^a(q~Rl|)@Fgd*K=OxztUP9OXV
zO|G{Ri5ZHp;h&U9V%|5Ab7Q)|*iwg2#YZ4_aR8*R*ooU7UnCn7zl{I<<`wB^cY@r`
z{kWw41GX%{s*8;R$LQ4}SYa0ke#7K(V09dhGZ_a1Rv#A|=)5Km1#Z?8*=Q6+?FX&-
zbFkvD5#1Q1hJ$C0Li0!!a{pZ$#$|pa6Smo4(3A&|9U4yZKmP>z5Pkfv{+v{bdRal&
zWZ@k=2%HaHA=Q=nxXET9yhzXzjSANS#dCilg&f8(*@0yH6$v=qqexPlf03y}BGJ`g
zAL>R7;j*s%B*N1d4N(&_UQA(^zWru{i-rTgFJcSo&yuM>M)1Ra_SkgH6w@ysU{{<Q
z$;mnXIQ7Cc7GG3Jf*X!N)|UcMpQp)M)$g*fmD{oVM!I-SX&-z2{Rpwn8G_dh$5nYj
zJ{<aXkcqBofQvy3PVo#xo|KF74jQ7V%dMc`782Eet03a=Tv#%rm~__-fXsQhaDnUK
z+lvPvB%vIKO-{o-Ny%6ydxO-kw}3@I8o?6unXCFyl*w_xYf<IwtCBUOB{;yE%2<J|
zCq>GWuaX&$r$Tbp6Hu%#Kp*Wz<k{dkDEDv(2FZR0@8QbA4(S7PZYPqqV=<twYf%+?
zwg!S}8HQXN1yx~9L_yd&o5v==#A_c!=jC27-!X>dr1VbFt?^T^QYVv@K5azR-$|G%
z`;Z*lszZEjwTY=)GTa!rj;)O5kmuGf@X&|Q8^0Ocf6)Y~sxAU+$`JqNm*Gb5Mix9H
z9xKHCWcgl#MWT<ytfj9?Qt~{kJs{-LWWvazG8arJ&A`ptbz+-k3q|RdBw<3U5}539
zLx<XU0RPYA#-Ho#LvR)t#Es@sMLAVv2aH9DdS{7$=RKIXPY&D5-?P?P4tU?q8PCk}
z2i4<=xFg06Uw%J`rK44ObP2GNvtlveqAIAk3XF!UUhplah(&=u9*Q%;rXVByV<$}>
zX4=8odGV0^qmYT;FsBt;hvM&q15Bm&2is@<0|Hn4!j6avnACg(SM2+M16m)DvhG$K
zA?z&APTXY+ABN)co{hLQzncV3{>y%i%NGsZ9SA#FB~BOedKC!+P&#BF9B<msrj_1e
z>XX~WXgCAjta}8S78Ary4u?f*xp{ayszPA#-2gkBL8``Ihsqz3m^W%2+fzT9x5n=U
z&;4t0ux&Isv-3F6fKqmB(|x%4?K~K1Mv@Cb;UI6SfOXLXeqQq+(Z@aEeTFhbecmiE
zVgli>_D*d6af$HFOW?+q6qFS-W5@L+%(3w;9(-Q`5h|2Vc~k<kCQZR#D<xn|+7>vw
z^gr_N(n@mECJD~bM{H?iDj4?pk->tV-u_4*YEH+&uQ?xyTj^V<*)<R<TW+y<S4FJZ
zRgEvAm!pE@C`cS^1d4{Uq2ldS{HU5Py8gHY-^%WQv9}b#Q*XR@+6IA3VXK43$Ijt*
z_dTzQUB3j+yNto&FXv#wr2_aiY%T1+r$Za>TBB990n?N_!x}2m*|K%v<iu%pY}?Y0
z>#csE^n+4dyJ?JgWb`ChYrYYDPyA*+hs|+IeFaN4-ia-?{bZT^Bse<Df=PW2fiErR
ziEe!!cE23O6unhh>A-&caOp9Ws3qV7t&Qk0=pgju=yBO61k%hi@$dKN>~`T@<{5uW
zl;w3=yh-gEoBr{kXymp1PAy-h*`lWJ@b1JLW;4G4vh9q?_S$kXxL6YVzs`ro+*Ax-
zVoGlgdH`yf?$mr<DVUY@vsI9Q8rZ>-G_<kusu4BWU4agXPN3CpiF!SQSZ2XS2$d=#
zJwId7-NOOf=k8>a<leLCXA<Fd=OeN0&9?+c#DG%MF3`OA1MVzkqH3>bFxD6V<?IqE
z-I)!ql@DUaqd-hGh!@-6xJh>3Qlo(*$I$6Jib=rVJP6I{X3gRl{9*8en4VX`_s`_S
zJ2R|du9FNNP|9Jen(bN3^cctv+zYEzhSJ^hYst3(b*#8-8V0zl;T+=(R1EJSQ=ir2
zy^?;`T96EC4+~MM#T)KEiN#$rrFm}s8QAZq3zk{Npt)Ed26Y-@d9nn5Ch%w8-Z+V^
zecf!u3c#K*1m&!aV3GVCsOYmJ9eQ3UnVpIIwu}@%RBmI5lPrmU?oWJ};LWG^BKnVO
zz_Nu)SXPP|T*n&-KO$M0b|!im{KUC;x1oc#G`Ac0P84tIL1y1|6!z<UHrTNU-yF%t
zzi)We4*jK~UGt4#PSs2Db>R>^)U|?mUu`Dqd*-5AZ#ty^%Vb)%-yztOvw`0u$cPio
zs3W|G?IKS)YwngJA5}vzq)(MyQi#Q0vwT5j=@XE#&WC;Bx6%B?Su(s@gO51&AH+*L
z!~KPEXku}h*jfwwV|_6>)pi($=_;a1d{4!W-GZO>;BmOyphslXw!+fw>+$_$HEtXK
z8tTuUAbv{nbhleMtem)(ER9ZKZdRo*a_&FoKK&h$eJMdAw&am><5R%TCmtLN*Q3-2
zMQ1hjX}JCCkBXjG_n`R8Niy-a0u($%(5aS(dTDhC?At)}CTFp?!u}taQV-;P5b6EU
z&0=eIvt?-kq_QlYsV!ehXPb^-yN%01`>h&We~}6^&n$sZwNyx$^AzoCRzhLE2U<Qy
zke58qD&i!$7#`wum3Fr3r!D+`JA@XkdB*C}r=xUxCFXBZ#Dz0s$!LkuRYTQhVD5k%
z+`Y|$xgPOEor^kX7UhBu7gl3O+afUhPa2-6&xHT;|No!=|Ns2||Ns2|HG}Wtx%stt
zbN3wHZ7a)12WN>?e2ws5QnmQYYA;&%ss;~E8whsO!tnR$J0#j#*?CpRVESG@TJQna
zvhgpx!1ufXo?5qy?%3`JT@4e_AyUZ93v8i^jw|%nUR~P%;tV}KZyvua=R!Y?UFSUF
zs}vp3Fqjsnn&VCvJsxUl3`S=y=;G7=;8=)|$?O}&4<@eS39~zKZA=8csG?3^c^_dZ
zv7DG$kK)-j0(;QFg#LN@4Qj?(!=;c{;5;Kl@J#iin_LZ!=v_}<{CkEy=9P3<VId7r
zoWYDQuEvRrCiC;%hxn;Ik@SKvhmMMAga)rQJpTR_bnKPobw@Lx$$U5cwdNoBoc|r=
zKc~`7_Ssy)>=f11Ny1^K>iqJmm-u1P5^hqLhpVpj;1<n%qPs}}H)xC_7aSkJhgL$>
zyadL(pE11K;fD9!Z}56PNEFw#fJ^ZU;FX%nG@iKA8Ci|E>-``+<}(KxjfQe@%#q4p
zp|WuGUOLJzI1Q)1?_|zeA++qlO8#elChaeD!je<Tm?^M4W1l*5KclD2PBR4+9(@G6
ziiy0E9)(DMC*175o$slzr)M_~L-O${N|sz_?^PQyP;LX4pX@<xeGV~`f5#ayQ=!)u
zjODSz3~9hwX|9ksf=*elg&wX+SY=c}9mntHHixUIkD~-3iqiDl<xDEIbq?JaV@4x|
z*|zDSaED5ZLF)xh{8McOqSk)Cz+wp>cu|o{ua<;m`aS3;++vrJP<}#EgR4)DBFf4G
zaP#jXFpRj#s_Ne1>c8nEOUMR@q>N~eVKe5kJX9-G;Iktokvse(uGV{L({mFZL-vsX
z->LkKNQeLXWlUGLdeia0BA{zuG@W&GB_AGto(-w8#<qF)VAjMsa(DPFnp5S$#kY>L
z$$t_+J|>;%Pdh-1i=UAn?Q*=tW)VIwv*#;gb8yYQ<#cAmJZ`1sN~Z+x;V!eDLU;QN
zC{WCWUw-xUp2(foPrgReytVnQ8*5<TmzlKi=O9c`l%fW80Z_ZknK!#FhNMaEe7bBN
zld~Pl+Xv6#e-b|nJfmKWs&<65Ig(W0(OvXt;Xd-LH4LY_e_%sz*y60${_wbaB;Wk5
z4=;arCGOJ$xr9dyePa5y>bZ3rn5!95k7362%=Qv8#37H0llQ~Tr{ih!`gPQ5X*Eb@
zUSMmRchewAcb@RLlGZwv;2rfU*s4|~?CN&7c>5r3HvSGQ3=HS-Zhfet=*r8@e0a^L
z8Q^Md%6}};#R&ac9svftth*g`EzaSUWE+|xXtx=YJaH#g11d8RpI3ZjlRJ{pnl9lb
z`(NN>w{3iH(Q4X$a}EB8o=WqRElHh8HlA--&ZXwZh-%ah^ZG3X!p^J1hrd3;wpvKD
zo92G_IV2nEj5Q%trU`R?52oXEIxxU<EZs4B3H7?9LQj+`(WeU}Xo##TFS;yCUrU6e
zXLL0jn7Inm1nsy}A()$#jRT8wGx)VOJ?zbXhMlu+z`IA+aEqKW7)Nx&_gN2d>_s~|
zD)bmBm*0Rh<4@qhK}DhshlX$`<2(}mCLg3XM_}$I1N!)|JstJjmTvYhqRwN*@J%nB
zZqJBE$)f_pexL^wIgjMKA9~RBlU7o=;7K#GYGK%p0C2rmiciK|LABfx+U2z$Osb9P
z`Y8g7@%>XgQ!<&Q{ZpqawzOgD$IbBU^<bQxbsl4bT0j<3$uB=Hns;Ux^=o>;x)!8?
zb;V-px9bF6avVg*1S!+k^A8Gqdr8PXRYqUz_u@*&FVKeq@USc=mc~wSp%$6GG<d`l
z3@=<owoKoIwfg7TuJ_Mj$`K=6ede6tOMi~a`k6dm(cU@p^G%*rWB~!4RahnPM|z$N
zhMM<(#ebU=VZCiUfBJqMKlCSvYgr!U8Wwf1J+KFT?v6wIb5;D}j%c3h?!r&LR>175
zTj+1sYzXeykK$FQNsYNR|8ujIC@en1<Z4X0w@n#-SigzC{9FijLj&=-fj-@HcQeUR
z^uV|8UIEot<uWg~LRP-u*X-PZ&h5kKv{_NqB1D=09a})pnJ_$MWXA3kU4S;hE8%f#
zHFuE<hG~C&`HTHhI51`e)i}HWGH$M;G|>;-E0yVByEg)Fp&ZoK7n9mZHG1Q}DDLuZ
z9SsvQd<Q>IrW>Y4(hAW{c3$ABsq8f3C*;0i=N(<Ts>p?Ve@~^IiQjOBOoV8b<2L^4
za|^k0{2%V<b)p4sR&><x0Gj<y8oi!uqcikFuyNo<2&>-!Tc$1$9rIGh!;`*2^<_0W
zdDC3-Ni~m%rL<7_*;@Y3TLpSYhLf8eV}a=Vz>#Cp^zQ;AC>!KQ2P<y@gCq&M<%9*E
z%#na>{~`2S+y+{AEDVAyZqnJYBZTKp8xmveX_vrxKYY3XpV2mf7v{oU3*rS`I17I&
zMe^Qxgs&bkhgj}1rOJPsz%J_mJvM(OI94W;z2#y&p*e!ixObeZUV9I-l8$0h4#U8l
z0QmCZJ*e@1Z1b7}2g>Ye4SI9eYtLxwn_RL>;7%SNe1z^vPeqVWrUmU3w)$(q!se%7
zHp-OF?h%Pr>%RlZ{Z??~z%y8!>PL2N428LlMqD)hCcClpf#}9d5zRWa5cxxUde`g}
zpK{?Wtb==OiHi;GzcY^h-7e%-WM06tA}Q<;1qkn_DEh2<DvkP<O+3rg>DGtC`I!rg
zMZ4bRU{r)Y-+-zxWr{hq^_)qaf1ILujWV>xtqM&42wtwRv&^-|nXX4SRMebFhh6a@
z)mc^4V_~|$fmuWC?4>aO#a(m~UdA?wGPJ#AB8?l^NoEwk6gSJX@S@-d-V<WM2P!K%
zADVR%BIbtEb(PC;440s5j(U;uuD9ZfGApLGW(Z$rHxk!xwBV+q8kqI@8Oje+qZh^`
z@W#z6;7a2%-uupucNyq$(>tFrEK8Rg3tnFHfJroT@CW7<_XT!5X@*ume;(!Zp4=Wd
z2v_!<=lbi%v+%A47#d!RUelM;!>IzxuRn+P4lRYHPsgDp#^a&GRsy?x5TAW5fm@DB
z$FdF%R%_~D;MU_bpzts>wmHL6+Z5c^XUZMR3h?Qnc)s-LWH^+e#UD1U6zz?-L+`%X
z{K0V<zTNgY+g3l5E$GdoJD!iD!3M5$Q>G@}`p%eddvpTE?^(q^Iv;}DAsyu0&C@7V
zG@eF|OXXo-^Po~Ak|&Z%*7Hi4k2Knkjgx+2#++%a&`Xbx(Az?G?R|;1+TPs2uiV+&
z?g>9^Ylc5f^SNqiGZXbYg4#Yk+Vj+%=Ev#5)$V2??`qCRtR2lqS44}hFIC{K5_+^@
zxe*Ps84F{czNedP`m5d_A4p%k*$c0aEyw-A8^G#Z9-Z%;BkY6<G}&M&RhqSuue%yb
zKj%yXcfCU}wr?PeFmVOP3F+Lhd<<RpcRQ}Up-p8Ue1?%5j7Ui5YbdHd$Sl{+qr=}&
z)|}jl@As#n`kMiKM{Ey@Jfi5;?}hkDy&Tuw)92s)r7_^zc^;G@rn^=u@>Leycyh^j
z8mqVl_a7cbRvz@Ezs61E;f3*B_Fo(qZ528z!X&vtn=g(08bR~i*YN+|)WU9l3Y??n
zviUKd;IjJ+Jz!YOtNab<pou@QRPge|NxAZl4|XtQpCx}ZK$AJmo=wM(-%Dq;9A>^r
zQQ)|1C12|%=ATXCz~*rvR4Y5s#3)Ppu5um9tK7tG?1VV+<!8~E#nE)ztCM&tzy{ZM
zbX6W&tirEcnnXQX0_lIH72@|Zb@6(h4^GXP$erB6S>v$Zq`x&ABcgryq^TbG>-a97
z8@C$z8nOi@xF?+~u-Vp*@`uDdhBW`{I-G5)MLnI}#rEF~U|zoqKSnfY!q)_mcghy}
zq%exwO`kyhV|Jmmz%VIFu*c|)aX2xu488=%)7R?;a<>6Tojqjpkh$Gsk-57BJXtuM
z7b~PlBojm>6YXJA`w$+X{}B3<ucJ|3C1@NNjeC-J;M#+>K<g^_4&zwdDDjhj@sb4<
z<5Vsg=tj#As^BSud*qjN7Ha1Oz=|;r{L2U_s%2)vW=>1Q;oBF2?wZ$l{>fPka><2-
zKT@F9t%itW_tAr%5q!zNf6U_PUBqA8_#ABp$HG?=+x!c->0Fjr?}0vU-lN7lwg?%N
zLVJ9>Z50m^d@1gyUO?w~p?APsk%sPgh{;vSr1kF?+~|7``lJ7VecM%hG$Dbi439(o
z`dL(?d<py*!%>;d;sI)-_^J!JyxcyKwuMjNH_aWO;hPzlFHYdyga5(RW&I$PS<hVb
zbhykg;qS_u`NAnDQ0Mk}I?mgO|D3EyleGQlsd!g7d#R0m{F6h=FW=$ejRl}-=ZBG#
zB&cWm6_{{bj!s-)N4EVi!T8^Qm}TYzNSwHU%Wa5&f1B;m_pkw;zZVI~3MR1CX)~YY
z<wVC8>F|?J4pXn?r$AMC1?<5XmS0+iHhV{-MWrH~Zn(@Wr@8QltK_l#L>w=>J_cP2
zzlgU-*>lH-g`#nC>b$!-2E*dyNubUv(j##IY}j(R_acU_Jv0nrC(fV+hEd>ZIUUs!
zO!)gxH(}P-BChz3z||GHc<_xgf5pC$o3+ZcW6eaKr(Q-i{+&isVWvw-Nfvnc^Y~Wx
z58|W6iRkpt9_GFuPrnbF!M6za#wGK7_}T*l_@+ybiFH~F#LY3leq4of@2!FLJ5+e{
zmt{15b_^-qt45c2HQ)qcZ!}#kFOpf~h%{FfPhA?z<(|ypk*B&~mt+eJ)%d`Qe>{M>
z$Mm?&sU+4EcLT0{{Q@ej#(YLk5+2yOkV_;{<}q&_?B04Ed@|RQ&3q7_^X&sj53}PE
zr+2XF`b&AzejQ#tXaczyHHr!ud-^2R1FVu2h~K#(SRL%hO_EjUg^i>5zL_N~b)f{m
z)ue&DU3=lfXG6TaItmmTZo%Xm@5!XtOqzGPgbe+m%Z*yHamAgH+;8-G7&VB)6Ag-X
zy-9p(uMu9@TfvY06w&N1b$<1KIhi>ZLGIlq(7&%ua}Vi(vD<oneu^8f7-7eUyj(+b
z)uKgBqjyo&E+>AdT@{qp&%<4}yP=BO!>bL`z&cKgIhHS@?w@rabfgn(xy!|i6zB3{
zA)Av^C}`O&TD<Ps2@?HOk8Qs-jlZ(L2IHPDhVClC3roZ3i(byi&3I~1(I?#P2|tC2
zG+;{*pR*v5ZnhTDofl*A?TKx;;&GeMr6J2%fIYQ6Vn^$|7IBDpKu4BoLACe^eP-SO
z<x1{CH^~TY8}W_gZ8yQos6Zoz+H;@i4}wl>$Tuy20aJscX<(@oNGyw`YuBE@wF@r5
zjbI}xcpJIUAtYp<5Aivt-=nRcn~)t<pc56(2pQA@s&07`<wx!$TF=v9p5held&z>m
zs#4>zK1cDE$3fiWtI4+vA5Dfm{7o{leSm{1{XXL$Nwo>*y63*Y_^3V{b-e&8H|4<U
zu3;GRX9Djpya;K9qxst!W13RahVO^%ru(KnhVyy@sOS9WFu^{N&km^Mhy31S>!Y_=
z=!}G~6uMfIcXJ(edu|xHh|5kagGF7V_|8CQ>b5=rYd`jYVv*1vbbB+MH~b)uUU3jl
ztvAH6@7D1eA1$cp2ONHF7bYFe!@>5QBJsIX=w0y~`h=Vzd|yasuaV-<oE@PvAcyO1
zPUkNilySh=4;U8J3C_+xVOf|vdXLV6mc=hbh0lAS=hjI<o3N*~7jKErwU4IJDveZD
zawXMxWJiC_Q>N*6bwrn<_TjSrIQql50t=@m&_@rY`Q?3awC$lepOTr$yonxF)5+zU
z%JKB#j}!1(e=G@eR-$X{9C@SjSl;Vq%jMPmaaw*BBt{j$umowSOxMB5dKbx!7i}ov
z{fN#L?g3kc{AK^O0d)AM6x@G43)Otx>4S$-7<p(jNEa`rPxHsX>B>YpeM}mJ344=K
zZ4177WJ~+551>!me0fKI7ia_>7JTLD_%z@*X&M^GJa!!A^P~uu-*B8q3v7Q!Em!`l
zPJ$Xq+VG_=o$%glJrsGy!1aq(7&C4tKI#wQ-hxkLuVWgfHVJJ0d#dco?6dHzX*}c%
zaKn5(fuSYLUiI->+-<)aPQ2hR3h0%mS_z8ORbGoe*jxkOQJUQ_l@oPoe!$3I6L9dz
z8ALT#i*}c8hUJ3(f3HNA?pv1yZ`$v`IvzodOV;rzxvTlG#|N<SP#yO-NrFB-;l{7f
zhKqYQ!>AdvXn)8(((+rGPLTi2zURmBh}pTg{PZCHxT6P7-gzZ(vE{hd+=FcE#XN4m
z!<`0Y4n~{MN_gDag55)!$(EX#T)Fu;`F%VH`a@i(r*9Z7`rRn#9j>%$;Bu<=pWqYI
zvqUH3s~BJ-$F~m4!CN!;5G5|`KmyD9+o6lFPe&0$<0s&{b6R}<$+2{**BLxmZ$f=H
zb;0Uf39dhMAZBG>X5C7u0+aUz=nPe&9f!DZQy}#2U|Z^a)q;mr%G26X6LMwrWPG=>
z6U_4t;N^Rc^i0%z;;i!-ayA-)MBY4Zv2ZX26%B4xPz#1xkHoPDj}hB*W9fS(XZln2
z5)B$rh2`SGbcxgq9wAyq+Fzy9!R7*IMosXK1zS_~6Q#J}+f!;QlR+Qlxxti^pTHqZ
z6YH<u#IshDL|Pi1Xo4Bs_ecn<$REOE{+^|(k$2F^Y7pr3pF(q&D}1GH5OuwzEOY|}
z(7!+KlCWo+>A0o=G^kjIj$$R?A-{<GSj~eY<J0+(5zf>>!=BeV`r<i1Q~oDJjUURp
z%q<Jf!KdIze&UBTRV`csi(;NZq1-{<xuuU3->Bn@$J~d#kyb1x@eSnvli-q(CD^k5
z1KxeAMTh!C!K4j)sP8Iu3<#{l(hK*Q%=*#XP`E|Uh**y&?<??gFKd95CgVqm4)~Dz
z9Iko2A(|Jov7}IrE?kqrf4h%?1ac88!Oq$DxiXwqJWfT+w&OzYlk`XV1G3_z&?_$Z
zNK(R)d)~3*;^kNH^~tq(dC5vzyJr*}-C~Sq)~~^d%ZGvFS~u?KFr5CqrpNL!Wq8t(
zTCn<}P8aR(smcnrqXPoFh-rBe=N@Zus75;4yscqBclcqrQ6X4$Q7%1lD)x^4MrOM$
z!s(f&{EWX1H|$@I1=BS73-3y5R<2H0CMeO;5L4E%PzfJ;WeQ!8vbbYPJ6ZAeG)fD-
z>&jbZ(DSi>*`gudRNJwF3{BGDXXabcnx(7p9^F7q<~z`quX5b$f4L9N#Mfmsz;yT_
zy8iGIu()Ij#rvLMM|J^Dm>4L$GoO<mX_2%pP#W!B8&RiFjXOOH=T9HZ!*AV<c;vw`
zu&H;$Rr(XjhWQetd65LH(>RSzspC<R|Ad#zIhy(A((XTjw9Q}W-(09m<Hyb9-|}-X
z#i13K-Z>4s4jtv+?tg{{j!I~-!G%i9)}k@5J?Kcy;V6G53u89d;-?9bsH*=O$5b5!
zrz1(kPWC!^;WMaeXDi1I^M7Jpnj-%zoe5_(kD-su9@?Pnh+f*1UhH%Q<!E!BD^mwM
zuOH`mmwu98pU3gcJz4NHIEzP2Yy#g$mCj}(4uV?1C%Sdp&&r289H_-!L(H%<=BM22
zp!uK)Hyc$iD&0xxoOSo`%G0BC$p}9<u=fP+J@X6Hf@3l3XaarZ^%gyzt)Vl#_wu&C
zg7>5AJ=rW|V-mep_^3ax!B@wMd$nvpBmF5fB~5{K-RdMx?;YV*#2=h5<ba$E9Jz98
z0VeEqB<Vq8aodapn)RRthDO@+t2;$BzikAcl0KbIxEDqjwrN4&*H@y(8&gninig03
zyNL|gvj$#YPUpwEbLo}jSA5gR)ihwdF;5+tENHR+@Yh;Hy6sXkKHKU@|L#0VI_4?T
zyAxtW-_Fl~>8mb~$=6G{#LfsZ@PiC*%CHmrE!+r8u6!1oUR}j!R?b9C!&sbew-N97
zC4-N!ORq9TcvHi~dSk+2yV7{7HTyPdxA=hU0R<YMb3w>zNkZ(Z@2uc;3}0D(g75gH
z0tJ}_MjXioZ|RvdT$sP4&luo~Yx}TQ?E$n-ub^W_9>zn1-%+V=`uvC65T1C{fv#EQ
z&qIZtH?!1nbmlvu-{IRf!DDq5Us*_E;BY^_^x8$S?5QDC{boBZa{mghY8SCyti&hH
zH~?$kjHVIeT7mh+f**Kc|MXNW|Er4$v3;OB=PTCydrm^>asDxMDBT__k6OKfRP?!3
z*f$$sjk^fiG?vqcpUdI+vNTx!?E!f(FoDQjTT55erBU1Wjwt&#jT*H{S07tqMvuA-
zg>@kX=y^eriU+1axbPi2<tptGvt}gq(WxiRhLWfn_khc68p~y*F0+Nk+1PM%F!+Ah
z&o}2h#u3iic&U`Yl_9lY)H#r5zPXKTX(h9GCIhvyt~}YJp6y!gim3;(ga*I8csuSQ
z-}B6hub!&FA1#?my-&!%h^F(zH#P=cN(*^uK^3fMNTy#iZ0OJ5bLfU~f!J4ag1fGb
zz&_`*bhfV*zirY&T|JF>cHn$&k`e=ZhAM(v@dGS(E39&eUc=XZbm0daI`E|5UOGKt
zGY&v=2vU75vesS2zZsnc?I8>lLkCgEzdK>v%ZqqS?*+8?{1Y^SKM)ofN25&5xx>jD
zpitU@{vxr^ML!jX&D=zb$8~~diWPi56-{sT%;lqm-_#*05wO+13XAtN!0y+jFf6E*
zO?TTsbd6N;s_qQBW%_I$=C48B*QN4f2VM&-kSeH^*@M4s9H)sY6_{>y0RAd2#94oZ
z`&8lta7llLfov=37`uQm%?Y$wZ9d(6q=Xu;(udYDrqm-ZgUptfMYFHbG;7p3=B0ET
zy5er4!y#9opGNX2*=^8Wq(MWX|KNrM1-ky$Wt{k*A9v8V1-k)Nz<y}5kF!qFL!OVA
z$Sn^JSQLSbz2IBc980n39@zhHej6!8XGE{U6+idW*6&udI^BuBy*3uU%^E--g}xwu
z<0;FCn@q=LNAXmjO6s!z4#q6GN;7_sAY0O{xJQK#KHRmR8$FweW7Lm;X3GsUEGxn-
z$!fGdz?40||B}}9tfRHB57KstEikbmmqbOKLiYj#zG0dHm%e0!OJCg*)t$Qr4+Or!
z|NM<$xR%#!t%n#RhPx*jl20X@At`SZFCHVq8ot&FeM4(VpYK*Yz&o(>dJoM1?u@&o
zcToF?vG{1iRW^6u3BLEV82`SM<fjjoVM(zml@P!#=l1APm)ZLCZ1Q7>s#2n+f=_Jr
zpKkU!)Sd2?pThgBZh__NHGEm^FH*f+iON_u!XzC+^CEgM`EfIDx9P)eH|O)F+s6E9
z!(P7e=?nfm_FC0{tr<*vs}B8jGM9d`yUvb{+(#l>9O&$7DVP&0OM4XWFg>qCes%Re
ze)O9MzbW(~Y#*46;lDTI<7dzCtgi?6)J`71PDX<d_^3yvJ`dx8hgZ^Z-@|#U!FTxP
zr@>b(MhHr?#F?l{Z_kp3k5iXoz`e=LL;T&j!8(gCzjv0tEg<|+lP6tOI}*ArPqJkK
zyWgjHD2;MX;kwP*lrL5Q|3FoGKi~>{Jy1zyl$7ZviSO{-$(WjI2=}DZy3~x>)5~(F
zsbo?B95sDMzgB%0rOu0``x;EB*5eYiT%Ey(UR^*8X8z!n(~GG1_+c!ZBn`dWR74}n
z<ap@0k8H%{C3Kyg0hM<eNP9nwqoPyh{D9&Mylaw<W-kMTyVNv#RW_b@igM`7^G2{i
z?wUw$bT}kL>vN|~M=5)9fCN>)fe0lLlNl`RKOZ8XerytdImm@diGk-S4&?V<c=D3e
z9bA2b1b=lW8=aPI!}^9TOjL6ePu%|i>Fx4(BWwc1mE0x0LyAQAJMMy;t<Ygp_62yM
zJ^z&{&)@9YC}ea!cvxHmylMImkFUt(x0vub=HA?QcO8~L?E;I%ljwwz+Q1Y5?`8c(
z_k;_0+jSdXI(#Or(JqGWFH`C82%%@ZX*ya|B=et-<oT6`SU7#ToV`uoOhfI$#lBw$
zqSc#+5TIS^bijTceOqhI&nxA@#ua(aQYT)L6_*@no1XBj+==9qR9})n{}H@#+e@J{
zErs!s_PE7aowvCB5MPlk1Vxjd(D-RE+*vq;2RUi*H8X6u!4jdj{`@5@S@=<W_@5@f
z{(2di>o0Wnv@Yf{mZij1;SgWb@Ca-6Dsd@IdAdm0@a^LV(dvL+vMM?czi;a!8IMkZ
zwNnI)ZO@<~-Lt6wfFc+tRVd1`K7}tl`|y~HEUc_r4tw>QAg$4k-i%X%_Tz4J&CpGJ
zdGS*E>uM~&Xtj<`6gcW2S1y*%8N`?21U@nMAM^aCh88D>(B4m{V8Y>TxY|MqVrF~L
zgq*jSF6}{8H2WCo^y67`Iw7n67re=@!w&IsYWn3o-Yr++Gp^RLFtHK8HL@GkJa1#w
z6<2C<G#uyp1HIiG$SbU5`2DSWdHjD7xJS6p6}yhcAX9sOK*kb=y$L7M|9yf%9bLq5
zV7th(#g)&i`b!Km_EPcFOuU;sm*&n7XS-Q3_G|rs-3fU#Y`#0UoaRq2b%gMRUJFs~
ztpdtV`i9r<3oMh{>RfWG7{9Cwp$?OZAtnC;EX+)#^S{pI!7Hx8)~kB-Rj3x9EPWhs
zWD9w6b{lq%GUU-`PXY-USiSnU(2+Q7JRj(vNG~YcQVSP#I(wKD-Fmf_`Rx<lgM&Yk
zC34@%>FAUEX89$Qu}bBEmP;sAX@!A(S3v&gVUlVviR&&4CyO?=fjINDn98c+yA_{N
zeNH$(WOD+(c6!pPB6CpQXicwQX83jECYUhfFhp!WOP^R>5;Viv)ML~GtgZASx@+aR
z>E6-Yu04*adJ(SFuo{y)wo$qE&+NrPUAiH_ffj8V$&dJb6TdeYfs=YZV)di-5b_|F
z<_(<<f0m@uyTAV*duRSv)BpW#kxC?u8Z;@IlMIQo*9w`5LXt=sLntB<&4orP8VE&X
zhz6pw*CL@ZlY~U(@tt|faPRNmaesd5ryh?^=bXLwYpv(=y1LV{>P~d)<r#U1=P>?N
ze-CCjb!L+rZ)k*CH&kv2k-S!`(Ob3-7dg9&)0%dQlgbmgQENVHe%MCSous_~vv^!l
zlLsY3bH$rG)OdNqXzr_jkq$Htf~ohMAZOh$&TuOLrQXimeo99ekTnxWr>~<eC$2--
ztqMqYSwbfQ-@v_&I$}b<4a6f)!K#7d=t+tfG#)qQN*fbSFbwA1W(VQw@LR(|P7KFq
z#ikIL-9h{wQ7VQE(Zbr{cH;D)1iF619@fWL^80aPuw$zwhL;*rm-xvv!TT7GS&zaL
zU0t5vp#yp*E)?EYdvnzD4YcicAm-$qr?4C)*KPf8iE;YxV9#R#20JW;fUCK1;_@G{
z>g8mpy7r6aT6AIVGmIvt&1B7OW%Q{}ckX||jgIH%3p1)B`NUyAJQI`7g~RU9;O^gH
zOJHxl{t{%iZr5<-uCDC;Q4>u9%`vvfnJdmUgW0W2SoduMMeft(a?=9I**K1SSpS0!
zH~+!IZR^SOPy+;K7zyjguO>0V5oxy(heu`6sRTb#Z9GqTm5b?+!as-(xj^-<yM*u`
zgP@z)c{uJ}C<Goe!SqE1Slh2Jg%<zBTLIG9-`E)f`v&8l&WnVF!3%^{W!kJdz*Gzm
zt>NUms*>YjItBjf$?<g}6|ee3p{@$BrIQ@oo<-vW`&0}c^^7{*S_ab>-T=p)H^@+T
z4Ssm8grhfnkj?;?;Be!1Shsncm~@~Ig<aObjq7STW1b*%Ik}0?AFigvk&&bsS`HPX
zH=xz@E3)cNDcI%GNU_|lp58nwf?tM>)X^e}(tGR|RxFH!Tbxb#w@$!MTNxi)Ys61t
z3aIyv4%koneoXyO6CqI%9V-%9{!f)A-zlOgpTjA|{RrI4mUhkfn8yCtf`?J0ZqoNc
zJz=MCy-ON7cQk;QPR>-8cStmA)<D_d8?b!#Z6V6#4$!bRdYe?iMi1wUU&@nt=|ve#
zfBpz=g~f}#rrPqP6oLNC-^v4m21sllZJ4=dJ@m|0!}OYKAl#b@i4B%8)pQn3Tv7%H
z$4G3fgC~VGlY*hNPXj&bB=V*OhWu`PCV%(8NqFgp7##QlR1Y?b%O^LB`olIOyz0xf
zoo-|4`VM%1WCd({)0eK7@1fBqr|79q09BpI$DbEh(=)5n!h)k#RArdM1Lg$5=e1uc
zTW2H&I>u7eHx*dDzJ``Qp2CIwW^&)_2L)^U_Y}Lg2_9+wfc=AzpDatFsRyh{-@Jv+
zYZ{}0y*7RbSO?oujPXN4HPklth0qn{G-|_Gh}06O?YIN>k2{IqdT)VK9fsqqi{~Y8
zy7WH!Jtc?7PhgF`JDa`_=Zn=XVkc=nwdZ#S;o6~8Dt^+1n@23>l<%wY!k>8ImG>~g
zW5YRc+1BaoxU<XAG5$QAxwr(UWOT&&q08BFQYEx?^AK%H74hrsU*I!RhnM7U<pzzx
zq<ux1M-~M^(9Q9*dlA$6PEqhC;sIPel!L{JyI@4A2?uppMJN3B@q(o-pnRzX))+)0
zcXL5!%}OlRRY%KS7MR;;fwPv(<PI%A<jNcJxT>`$>*d6-kEsfl@4rKKYof3|;V7hK
z59g-U2H4j#jUv?q;o95P_{c|kFXlRMn}*cK3v=MuQ87@FZj6VOXW{QIkEK0_KCWJ|
zhuzy<S?h}y-=(e?DZfm9>VouqI+Ja)JD*LDyF~geC#mgP4A_({!dokudX@fw!rbkA
z#K8(RN?F>|ZKGf99z0?94Op;qp-><_TV=@l@S@s>u<GYCI_VgM84*h%GwLyX(K<l`
zFPBr)UUk^hV-MVk>k8AhofJCF9nLe&b>QnWCF*#+0M@Saft=j}C?~I{3*UFcKCKcO
zRxyY7Ipqs(<%d~(=Peu#OQjd@HuCa{1E4VCo8;XHMcuuX@O8h4wIq30{!7Pa%Hz;#
z{5?`_KaWc5j`LV?7Tzm+ND|`=U8;7_V_!Y|Co2+LeV0;MrWxLt!+cUUfgavz=1Eo(
zCu4r4fW{l}zvEUQA2We+rM+G2UvJSre+^kJZ-=GV)8vgog<xLlK}icNaQ$IZK5|hF
zQ_XYPJm>~a^VMLD6b)DJy)!}0a4?P9<^n;#$5X_YEwpFEGqgEznVM7n$<{qkh6U1f
z|Lf8u@!n8xKDFnK_{FD;ZRU-~d%e1ej($G`-M@t0e|hp8snfhQ%m#{QPNbG%LkiK@
zE=b2v{!coGjQAz7U!Mn3`|oGaxFZgBwGO7suUF!TNhMS;Bmv_imvZRCZo;?5?Ih0K
zMr%AbVMa`ln2;liY?vkZw0C6taAnqwUqQ)3X0Ug}P!65A2z)y|mpyt`DxRFGC7WNV
zO~);|@n0H(eq+pJ-=wTq-)?_|0@o|lJ1hg{C2vOUpk~_rR1uZ-Me=rib=>k-0Wa1z
z(Z{r<xc*rsx89O`9S@B$QtLnJIVcjR*39HVMW@Kd|Ae%kZ<L+7u^IN3rhr?41*wlL
z6t9i&5_gR0gd1@U>-={Z(=KFFZ~x6WW#DVNvv>j;%-hE6;?&Vu-2w;gZl%wO@sz7`
zl0tp=@a!F*#Zu$naPVh1U5t!{g_m?uDQFLzPU<M!HjRY6OFz^0<xO<KM9NJb+Jn`X
z_QBGVw%E0?6W+g{#wWfn2d_3$+Wd3}KeU}DTpW?XhbLU4RgpvZRmK!LUOouSl4?ay
zi>n|nae|+AGx+T>5j>xp$j=<tMi;|@u<B^@us3t_IH~V%+8WY_jk{~W=fy6(NOFt4
zyd~q41Kz=rGb3QDau+PhisP#buhYuwUTnKmm4m-6!tQl9=+_Y?p=HC`VY*x8l&*LM
zl=f>%=gb{qkH_EO(!3i`Fx3(+UiN^<Y!zPjwI4q@Hi9eeS_rK_mx0ctuheU;I?NqZ
z#QP^V3a5JKQkmmoOwLe(=Yut*ozV>#HzS#I{@H<{l>uLqneYW+3U3@^3M=n+<FP9h
zF?6)VU%&DleofsV9CnYw?<K8}UVK)5<<SvX5un2T-mZt$J}u&Lc`|l%3T2H2=BRDd
zl_&P!E!2B07Sa!z;mye@oLOTE{Z*B?xzRyh*ghOyEwM*)r3JjmT>+N+O@Qn7deU{5
zqgd-;2=Pj`92qv4R~2OPMiXfs)3rDLwC{sm_icwtv%6GiVaHV(F8sr=T0C}mGVPXT
z&5xx(U7rc#_<iURxI1(xCROgjFAH4ptV^J{;zvhLX+9$5FUPTRr6$;&{Xp|;7xDx+
z0A1Hzh17?g@L{tWPFh_p>w9>JORVVwF@2#YCcO)zb(<Zak76WrbdICCMKi%7cZaZR
zLk|{=2J(1y1yZdU3Tp<%5LfR*7oRA~&aaUeg;6m2-f5a+vxPg})xzgDL%C@GZK$iP
z5LF}c!DXo{&x?ExU2gY4`?@apcYiIOo;(zH-EM}>%eR70-7Hf0xP#7ny3nckD)Kut
z92;XLW{IxU6)W8<6s<|+o$>1+K7NK+;A(<qR%Wns<at<r{f{8O+llq1xlLilI9~Bq
z;IR)aIQw!j9m>_l($F7J;wa4#UP^gXvsZL}@N#%}<+)g?tAVa@@i^~jFAkM<1*Y>J
z(8O~G<U>ck6Y7<9@Z7Jh)Nf818SRny7FxEbxr0D!OBFs|5y>|^mcgoHzga(^ANRU>
zgCeXQ_>{o_u6(kN-h?~ynw{ztZqW<Jzp8=o7LiozenQkUb9}3Cj+{rI#3S*J`1wl!
z&v@s83n#uL#V!?`cEXSh6-ToB`~KXtE(<328-ZmmHNqE(y+ySn@SmbB#cT9Jn;%&q
zOxg_(A}u&JVHQ;%xdT?}yIHIA5>&Qvq<cGV3kS*qq02-cTwrL<sZ0Ff$X|U}FI^*2
zg2%$44pZ>0rJPQG%EbAXHVUrWbnv~jQ*o&SZmDX5YxjHc`J5%RrC&G9d@MQV#SsK5
z8W>sWMcpk<iATd`(v2nG#TuvM?31?<%WNAd<8laP*WVYa?|-L^G##FCMUBE=K9#B6
zImz4ok|?--I$u8TLW57{&<k|M=E<%cQK`V+<1PzhDwO!2@=>wreOKJ}vXUn1TJwin
zPvl2uwn@CeM#|q7hC|116^{Ss$1ek&AY$McHdwQYW|^6z(e0<w^T$Z~y&#Ks2VNn)
z>%YVf-Y>*`cXomPFb&>0DwXY=>nM6bAbs`C0GX>1`qXU3yQLA3Wu6H$zp2XldLQNo
zD`xV|Y-|47BU3nhX`sZD>yA_UFBArWBWq3_jN68c#(+9=G;C?XGw~uN3<!l@*(Th+
zex>BHlM7lctq`#{3f^vyC+FllRM4adCI#AfWAakmp<aZ3+V;qcWa2-!OQ5yK0e-2)
zL+?YA(98HHohc!Cq~1~5Rl1ypkJFHCWQn65S|AR(wwl~Kl+%3s7UA{e(`@y5Eu08%
zp}!%Wu-@1YM^1Ic%5PiQ+U_X+@eihgnYNgl*N9&n7xH!Ir!qyAU9c}tRT%%=2mO!#
zko#twr;(Fm_+;w|!8FJMLnMwx?8I^SV&o$3sB6YQ?e&DksRA$PFGUw*O|)<4Vh9ob
zz{N;aYSv7EkZ0d%=6~-=JnxAsk9LD4s@Gw#>OLVu2oe8Fx=SG;Ik;@=DBic_IZV2~
zim${v@%DB_zG6}?W;-4xd6_?YRXiK!nox+swbv5o*$$^K8Ho{xV_>Lp8+eqYS1sy0
zo*&EBileESeA}m^TDvGtvF(dqAFMdI;ijzamM3;!FY(a={K4-sf^Pj;p42^5oTHZx
zft%yR@Wx-T>w%qceESO8my`|3FCFpr+{c3IvkqXNln({F<e)P@8@z8DV~6Xir2lII
zhfzAz{k%#uLbPZ}Wf^U;bK^Cm`tX#sE1~OV6*Oyn1M~ADNnhcqaMjV9Wupd2dEicX
zqdf-B`;Ns)f)THLcpA0}Q9@{#z<$fS!YcQCPVZ{S8OQIFao}?KX7_AXUS&;a9M5s*
z4cSD>7%U$+9?#Aylzk1j2j-a_aGld2E}NK#dj>p%quy_5dz<w0^cxa_XHZ;tB5soX
zm5o&nLH)Yl^y#M=U72kz4l|D8uO+=vZLl(DrW~P_3onANVKXInI}UDXPr%EoFRjsa
z!IO_%IP}>GI8%LrVgq#ei*6@r4$_P38#7o{^RTp=>CH|b?I^o+AlA>h$wS`Mi>*rO
z96f6dJ$Ckk6-#7zw7OEX`7%W8yitu}M}4FlC-hKj!*E!(%U_tAn@5EM49Vr;b(zm9
zdu(|fjegYwD9NR}I76wHlkSe?k3IGIf$3MV=&T6;3}tZ7DuPU;UTNm)BeM3ZzWn%i
zn&3Swjm~=d^NXkdL7?w)@){n=g_4^>@y$4XSM_9Q=36;99#ciDoQ=F*>7ckk)tbkC
zF68_(0T|L^!!~_RiF5sq)5Mx~ia07|N4u0j!=7#IJ?u6N8T^lK>=}q7^<|`)Fcc&5
zZTa}i?{vjJnGM^I^N$f-xs>C0*6@e0SK$@iQFp+hn$oPt<vnPJcnIJ1{)0)Qx8SOO
z2Iym8i)GUO??j+6f2rAyX8jh?{KvI0e1bUzehj0`yg)vbq0H`!R#EUZDF@dxjXvdm
zhvE5m$fS8OUp-oYE~5)+<f<GrYF0(v3kmdUq#8d8mGR?$!SK*NhCizr;)TNb5-%x>
zoQJ66siq;gcV!S+ZPKL<<>s`ts+Dzbd*WN^Ue~^&BRV`Y;+RoEq<?ZOcQ||peDse)
zg#AivD0>Kh|Me1{pOWqmmZSJ(`3medww%wW*>KUC1TM(ZqYJnII=S7!s}t|ir>V~P
z;Mzj=vEIP<rH)O&!4g<D+n$S(pP;_G6S!+c(*xa$;8a$~XT}`oh+Xf1TzAl*;VXp^
z^XKD~*IwM-U4`4e7~s9Xc2qR*B-pR?!Jhhl+@*gnajSBe@W#S}Egu&{8jWI$D@W<I
zW1To)@fI=CWf@MtSw}|-wqr=q5Q&`}g(_!1!K=^FxWXU<m*1X-)4!?WveC;?_gyVC
ze?<KAy&BxLv|!+v-L$6E2nP(!72LN);_1E%QG56~_}yMCeEi%_Jp-e~kFr|MyJduf
zwZ^b4>$N!YmoI88TS5CveaPa}enEFu2A|!m#OJR~qJr7~iDwlL%M-oLNbYuyN)@(B
zKEybhSzb!t%yos#nvtSo%~?3I;WBxcjK)~WLG7V=4VKNf!{RnC{-E$qyqVpB*Yxd#
z3JvdtzY0Z=2~m9PbZ@S*lCi!?yI5#kH%vQVG48qU&V4^`!yJ{jRJW@m&NXC+JN}kd
zM2&}>8|P`4<S=>Fq<|hlo@kh}RbCXa2+LPzvq`s~;*Zh&NwLEp8voQEP1DV=kJR<r
zKW!}j9HYjYFBXW?f87u<_bq)@P^Y{l!=Y1QEEpN&;=#fkQgj=Smap8n=+bM!d}25p
zAFB$kRR;ux)-zOQeVc~-sUY73U0R`JNuS^Llf1tdsmH-4i2RTXD?2}dh=Oy_d#J>D
zEuKaHR_zj0rO(O?gTqkM{W5fG%Mg2YTu41LhH;tA9Ws`0;Ew7Ve5JS-t=<<Y*FUk5
zo;&XY)5%&~(q)~n*FhJ%balp0vocW_&?LB0G8#27KWhlUS1%gq`s*M{TE8AgyUpRD
zQ322;Sc?OT--zoQHN^=kQZBewVwjX#VZff-WO`1@+fMwC)C2F+KK%}`*|`q(#yqE6
z)1Jbb<bE8kwpGf#l!A_~s+5Dd3hRdZfLh1dWUk}N*Qt;^r$+Pd_-Z=pkWTII6KKMY
zJK`mobdP+SPJ@QMp=&K~h80L>eWO5S8j|BJZ$8|Q5+_#k--B;yaR(-I>tsGW)g9iK
zU6!xz5+m~Z5G?nPqzeWn_(9s=I;wtvj7z&|+N0{K>A&W2b#o&`YE0q+vpD>1z8)``
z8N<SP(Xvaj&oHN<h@IW0V_&Cof@O6Sm{TN|bFRGO-Aq~3n^@epYb^+pr~GR`rgVmg
z7amS>0|k#ku(kU;xy8g06gRFA!Uj2$@}xeLtzv@u8%=oV!`H%zDObe<c!xgM3jCsN
z5t_H#a>?a%+!@^rlH^HtX>=5XDd=;jFv(r<z>1gT6_d}RV-WD+4)nj_jQTk>7`~(y
z<fGT&BfGh1-)kT~wHPnBiI(^-a}__@b%Gj=O+=MY>02HXCN#CS!kFlKIyBvi<|$nR
zv3oYRmW;zUJ$rKVwJBhweVl%btfow>lhU=_oCn^CrI4}tG~dz^<7UPR!4C$rb;swj
z_sa)yd#gPheAJ0&JSvCjkv%B9S|Z%NQ^d)=(xK|sR-QBJ@~|&fX579~a=|}q!PDN|
zanzQnICV`PmmHMbtC~tYA!aa+44jAFs_Iauc2~R>><*osj|-#Ur=r4<u~?+xg+7m}
z#k3b~!lQFaJnvUN#H-qqpzMabl67&=n$J)=Dj&YxzaqZ5`IW9cHxQy9{f1sD|0D--
z2g%(z8YYk2C~jM+&H?SCaQBjlTx^w3t)9vd_yD-iqEy^H@i@-*y#-#|&O>kaX;2fl
zhePJ1V07_QDBNv@;ZN!zdqXr#PD$hgWs^ZS=AKx!MqL)(EQ704`boX5Sk`-_iWO70
zaTg7L2(1X9J<7L)Ra1JS<=+)7&HpKS;&@&gv5&2zRe8&?z2d(I{dwlR0CErRCYJU{
z<_+$LNoJzUTF1Nd@H^_fW_T!v_BsHaw1^KZUyW5~a_H}gO{C^9SMs$RLias}a_5eY
zII=>@PWycn)2f@Ppz9a;Ui)RFS+ZV!#XEqe<qoESOH_DrNfI=sPC=_VSr8Os%O5&@
zCtvkgZn(viyebsmjm$v9cTH4M^AUcI-bvB!rTo-mJ9=u&BBf0S>1=c!4&A2D7tssP
zsP2GUj}Oo~MF(;Flbsx&v4*zay-wpNI!Zkr$?x_u4)s$X&{=1dp<X=+5*D=yXAT|^
z8!FvFH+dMF+s<~mm}pONw@YzBf}fPJSdWD<-|3&>QBrBP;OM`dIWa^oZVcWc?Nf)7
zzh#J!-v6zb@0)`uz2fjzo+osZ_>R+h{~;lI7LWMq%&P<Q@z|>@S`;i`d7BFaFPO&Z
zF?mA3zMicA^p&{XcQh%Tn##W?z5t!^)}pxfyZHObMr?`8;#`vw@oft7p#ypJcvuHs
zu<o|dbBB!T=FS$n%u9x_9=&nHGI!{azXJCQcCv~^f2DqBAjhnapeLpN9K8L!>^?cc
ztf^<gX3lQ<Rr-i__8!51)fA!rml{{*3wSEsTFS&Mpg}!P%Nv`#c;mHwkoDM`^8?lB
z+;mT9tzS;xj%7n>?`Bc&NGkVANQT7s-8kB~BY7y=;@KvVvZE*ApSfA+aKjwzNC_*S
zzM(qTIQ}_N0rk4-VGoCTIN7Ze+Fu*YA$wM{#h47y%y_f7VSazyzph#=FZIJCV_SuB
zs?jieWjt!{zeLtW@8M$JYbsnj5C^;~<!8kYz%Xenzuev?Kh)@mJKh!J*{@F}KIcm5
zKHm#R{W%FgD(}kmZ1qvodNwclZ#+FQXoD-WAHo390Iig6LDmXS&TpusaJyvQls5%$
z-7F`wp-%W%Hi#BGl?YG1ofdZ%$%VuLtMOE!3g>S>B=G#J!)(+8`Dcd&vF(8j->ewN
zr9umMEt*c_kNzYYo<KS^`=$9@p2Q;BK_R(gVQY>pI||?7^M_Q~{%Ox3R$J;O>`~-G
zJ9nJ#nMYPv&QWOjE2tm!A9a$ai&=~RLAzHEF#4m*4$j74By}EkN|~@Z-)6GYYke&I
zT+iV*=i<yys%#tR08i8rFN_-~_F27`##X6g?sH8}tG)|eB@SlNhz8OeYA^48HA_}J
zE}r5x#8Ob?D(dmqkI&uRB%Hk>qmqqM)?BL(`=1ZQvY)Huw_qJj*4&3L#!crrC!UeZ
z`!0M>Pm`O>on=|`SK$a(MeblS03C9_L8bH@Grgc4&lJuRQxd|2htJa4=!`nA@3j-|
z3=D)#)lTRx^;QR7eox7+ey}v&koKG@C6A9*pfIzZf4L^oSNmPUKxYT|x}ZC*8WI6H
z66f;lVUY|PLAdfn4LZ)Mfu+(8$s}(q_UZRSh&{WLO1h__n5e{~YeFD+k|F9O0Uu4g
z3cC+DqVCl0*l^ecXUF-V{;(eukfng*HraAikO@pJ`c0qrbr)BUwZ-7LpWu}?8}067
z3ZF-73xO95#nfdnLa$FMaDEWUe>$e{!<V+O!#7G8bygG7cm9G+b)NL8WG8nYE@kSk
zS>mL%I%u<M6&|hmDWu)AWYvEK)M1ty-dI)4cb|L0i_p{bCayaloe+*^eY;@l_!@Yo
zdK&g0dQqjjz>GuQDRI-m?Nq)#jia=8;hMf${Nm>?FjsmEw^s%8TH{67+G8GE4QYnE
zUcUq{2kFgDJRpu$bHtO=;&GM{iV-n-V$b~zU~?gg=g;jX&3ln86RgQ-=MOPN`kQ{o
zDB;kIv)uTzj?(X}q{Fq}g+bfBaI;|{I!4%XOi6>VIM0aZ3|9C*w(<Y+|NqPX|1bal
zzx@CI^8f$K|Nk%l|G)hI|MLI;%m4o`|Ns9<{$J|;im?)d>CJ)fvgHb6#J2c2Fu3fG
zyV{YnuCB+lj!mpI{TQ8^5X!ykEcmf$sFXQP6xnjTY`Q#JTr7Dr#!C*2mfj|O!=?}}
z&64q{Gafj&{V6>hY=+S~Zd7-=So}VAgy?i^Ice!2nZElhn?LBjnDJE^mvAoCFLcL7
zM^nDw7!0wCf6Keqj3SGZ;go;hhQ01N^5gRg+}=?YmUx}QaIb7CS$r1fX4|mIR!#mQ
z9D%h*L!}+!VR26_@b`!k8kl;SVvWYhcRcKjDVf8-X0RzmKYWOHijT8OL^S@GRKh9_
zd2Bar2klJ#O@&T=IQ+gPY0tBSu5ah_<NnC?5n+7z-X?LvoOpITZh?vkNy2mK+?Ral
zo^1TFf$-l;I}W^YiV|mrV)(`7xHtVSv}s4;_k9QX<9<v0P;h~!+w8_F%N^8ts2@ye
zvSWkON+2OKsn4HcobA4Wvt~&=qT^b8eL)AVx$#Eg##qSiv~Gs`2Z{xuZ3y*`JqR(O
z0aWrD(Z?wrHt3C!W&n=`c2~u_zw>Cq${3ierG^KBD}?8R0x9Ey5B>4F!oK%kQMsgb
z=yJW1LelK<MyF&sj`HIxS@&h3LDpbDDMPH#=!b#J3~@&H`&6qv88w<`h)?_2<L1Om
zvTL$@G?u(&wkNA$-iBZBBtU^<JqOaD)!oG}N&RuLy9<9g+zeN9r2qd@rL)yqX`nfM
z4F~>8=7!S(_EnbnR*3}^BI}HU%}HD^=`Zx#`#@Ow@H@p+`g2y=7dodonEwr0j6F<O
z2)Ey)-~t0RDF?HeZO5kz10z@S5~<hikeH1NcdP;Xh?$t<EA1e9D1+X(-{3!J4Agy$
z!aK?`PCru&7c^t2pmGUp_H@Ixz8`7cRDG6RJul?9m2;x<09+F&@#CB}32p}sp>2vY
z&U-Q)=PcYRygS(r%XV($Dpwuyxj2-1`hBOn#n-4;NEpBB`%gB<*ozL{O~ms>V=4d6
ze=sp+Ha{GGNa!w`0No2;h(-^>=!3~{y14f#ZCW;nd(JLIJ;zPFMm0y67;23k*-}^P
zxjRhv(12?V8+pJC6K<>%;kqR8x*c>u_~kA2I$VeGOFcsl%5$a~BO@&M*%5XOT1&l+
zE=c@&3)ZTzr@sF#N-nxgxH>yrI5$29l@*;pZ)P*i_!AF)lTzsV5f9c<sez`K5;x)6
zRq9js9l{5?h!OL&aJi2%*&XQ3hAxRh(2ztSVtxfq>QaVdVl2^F@)6~|7{}YEYVZ`d
zTjW-9QSz@Cg7=_eN_t?(Ico^UEz#vaJrY1B?Ou%QtNB3s23g9gJPQ2XD4LTAMwI=8
z@vGhFkh>j6|F;x>udt!^@KV&jZGaa+iMmD@ao?3Gq~hC!CJffWb;i~hn4yOkefNWJ
znm<}E9|*>GKY(U}GK>k?%d4#|P(x`iwYXl8Wfc}<s(~LDrx%L7{QApP+_%xGgIDO2
zrxm`uzmIbdc7c(-pNZ99d%>BOAJBYvFzI`~rLZOkzN4edy<s<Xj86rf1sBQu%U)so
zF%KcN+(SwZl#9-eop6Y?)YUd9hp02zxPZ0^-=bXL*q~C_sprLY+Y*H#ht6X2$`zCu
zuoS<pm*#x!F?=n^1W)cgOa3l3czMS?x@J=WgTfSf*P`7VK6^Jj+?~iO7yDyXPAi<-
z6$>*)sN>I53)thd8-MMl15K?V;<?h7xXoi1wD<f6n$`KdVq-@RmOdM1arUe;+6y<n
zn#<V-4?yIt37B=W3y11$NAqslvUxq{NZhw%tX?~nzq&mKOWX6n<BwB_^<(<A;)Ag1
z(=0aqH2_AnHGoQwOYr+r1(`3{0=Ca9d6IrEWyh|A2%0R+x84hLCBApa$}wEk+=0in
zdII&*hSnc;Q1{ydR%pAi$7=u&Y3AJXp%302A_}T63*oEj53wkD6MJ8+;3H!<2*-UA
zx&O^P+}d+GoK8(4@$W7=k=;nA_9{}Jzm}jg<v4WC9nH&yYQb;md+B|v6P|kK(%{j^
zE8S1gi4!kqvZ5Dk8L?k@Hd)}OlQVc?CR2Q_jx_I-gWc3j;ZiR@cDMc}J4uh}<k>=W
z7}Ouuf6#=3FRO)Q^-+8;SsQH{y5I!+w<L=n!56YMS?f_P$2YwM>*1EH5vK`bl$NrN
z!v}cWsX(;+^Af^)e->2B+K8qC-wny8U-gbCd^-$(UL|0psSHnkye<~aRwLUbd*P^0
z65F_T!Ee@mz|Ghg2WlqZVwG`tJ8qnifAYAP9<hMq?{)^eTN*qz*$rzGTEND?hoEXQ
z7%XFF^N(3(d^xR}*Sb%mrsI}8T+IN6R%)Vo{&mV)okOH)%5S^9qyy{PKrO$&;QXfm
zcaGC!ujK7QKc5b~JEDOqG$t_Fx?=Fl!Td?P3TUh$#_FFT^O|(|)cY}l(_w)pn|f2m
zka^VP9!e^GODVg+hlYnm!@>Ir>`&X_!O@=FsyK)5v^)}5x2DqzMMaJoT@S<OeU<C(
zx<hZ&Hc`OWj-0sS8wJnnj=dGTVz<}p@U49?WHnutDIUmSGb=j|3%$UdK1$A<oo{I1
zJY(EADT!K_HNe-l(L69~5WE_E3zqgsf;*Z&C}pt?pB{IYeHSH4YJozW*u@z4Y3AXr
z;Ajl+j^=BRkBHay*NXdIH$&3@_5N>V;oLdB`P*-OEVJ*2Q%rTxv_?+ZXU1Yh*iZ5?
z+9CheumegoQs8cUQkAblCQhx|O6~WwFhW?!E{axF4ZfXtt)Yx3`nJO;m1sVpxfZI#
zFgjP#oui*;^50RByyp6T{_t%Tec!kqhV5P~I5}*9`F^J<Y~M$?wCy&W{re0qs}{qb
zHT$4I-<a((p24uatweomc|QLKH}`4soagINOaB6zSB!x*@^QR#<QMt={#LAN<OgO|
z%OUBU0Gpgsd4Jq-+;`6cH+cD=PVQ#>L?_Oij{Hq6OR6xy<1QHg8ivzE1&lfELV4qr
z;m)KU@ORM?ym5LI|7}ylw=Fa2Ngp5n=AB8ibGGv?&AI%}`zJN%grolUe9r!sfMaGR
z^ZM)3eqgOTZZ*usIxT7M(KDayCVrz;z0SacYy%dT-G|nw-_%^}&b0x0*ls7H#V9NE
z(TvB{E#uJb%rSNxa1|S_?-Sx?TcMqXL6zJ4D!RDem?vzo;qoPY@OA7(%9Pw#%R0@5
zPa~=!?Z7wTON=hIkvdLVu$4y+s{(}^QcqXqkSOyyjh|8$;Fz%^c|+wWJdpQ~h7FB|
zvF*bVn@@=gZ$5^ipkgX>IR-^xVc4nYj_m8O1em?<yfEoiAg`P4fm4SJ!Boe&*fI@Z
zab7Se8Q<sAiP~J^5y7`cbs{Aj6X@XXkB)vZvi5sVB&L!XPd*SSAL`~N9$I_^G}rCI
zpkq^@)OtO>xjT^jjySVxx;{)YzQ?mn>LBNYE;+h8gS+D>bg-}iDXGgn!^X4e-EFjT
zLN>>lUV-f49nf)mwe0@dk;p|#!m8niVboPe>hZ-8d#d%I#V7mlN#Tm1_d1j17Diy5
zV#G3ceOzHF<xTH&p?A^BvOkGIeE3BNN-o<Z=vu87mQGj3J-uq6ac>wnJ~=|`?sY{A
zvy<Z9s$OV!doKm)-WMjX)8n=CW5v7oqvU^0lu@VEpMSazNBio@{C$L?Fm16bXPViH
z`wum7cM~0)EqT~h)XoCWs0Z+~_zq1<KLvdPyK+;vBDA<Sm@lpxi2vQpqf<+N!cz}l
zx}jVKeZOk4nyL~1UkA86W(ZGtxem-)!Z|eQBxU}Y3k&JBFktyAd}ba^^E`TSV5jbE
z@0KW(J?#kRF3rP1c`v12n*+9KYT(FjQ5aAeDXvLS-~$csDR0kA-n}P-O2S(O|H@5p
zXly;{IJRNMpG*i@8-N;7DUf>KpVjrGZ1lw+aCLg6nC)E)Iu+J9F!q}4L%}kr5u$M7
z%zm=(4#sD6ADD^(qe5tW=}@89sFy;-mr-!P<)+y6pa<p`&Bl-S4q>s}6?S|bgFD4Q
zR+(K)Dw`82w|5jfepyUe8r_(#W>Lmb3(==kmwIGcLuEvpFm7f(j5&S|%5LdVgHtwz
zd)0%F<WPz-+m06+_Cx)}n?lDKwy1pK3C%7o!Vr^2*=A1{3>sW7^=o3u<<$*1HT9@?
zc+elRn5{<wo}q)=7KqH+cxg}u8f|<e_V>Ss5h(_|>em*0`*Z<$rA<K9N=>%fpHKhV
zHP||;jvX5Pg#-;-{%dAK$CNkHqGAbo^kK8yMPklws@;islMce|uw2RtybdbfN7&WA
zGggk3;o8^tt5OE8#9h_P*s!lH9coOcfZKNj-$7>L#{eVYvDYYeJa5S%oeE?yyC=C!
zSVgDx)9~QBOPsza4+l)rl6p;>@$%?p6#2*iV?!TOs*NMdHwL3xvKvRfJpj$M4jeQ!
zj2Dei77a2Y#TgGotXS!eXP@4LjBg{cF1L;rF0p{F;mTlhJ|4z)T)>G29WY_&TYBED
z4_`_72s)m7X>qT8WF~ozMt&Pket*~V8|${Jx@~``;Jq5}Ov|87D<^Wn>@@ay=*Gc*
zsqk^EK)Via#s3~`0Nd?)+;-rVV72BV&q=r-n;CV5l#jlKCI>TobHiL>TlK`x=9<Dy
z*FZet|5r%K*$-MS))Fu0C~Z~#P0k*xPys9?e%~0ZFYCzdNndD*TMwSs<q&yI4jk6e
z+=gSO@1{P(U0EY?Ji4c^g&|jK(ERK>@K(Ag#Prjkz`*5TwWX`*=3j;bcH3jomIRFH
zIhriD*@MTwSk_-|1xc^6#3!E5;LYG6V*0-~;wc+jT47dA!}g3q1A`Xv-m$|laoH01
zAWq?ipK^(9>OpT_ETVT+yRopLGrn236x$6>p#KV0X~*g#^YvPa!9OionDdX;4^iP&
zYvxHAs||E@N-2h|x4_w+6~Z6o3R!IPe&{_<ojqD}@j#b1^5;i-i?+L;(U%Vvxc8$o
zKdjt<_eC8TGs6}(U+RS0&g_HRJwDTf=T6*3^*vQQ)@1h>Wqe!ffVWj=@JFw1(4z5|
z&z-2DV-Uhi&Bri*`baPPEu*7dTEyFZHsYdL`(e`Z0y@(_1BxmW*o?!(+vh9cpxsCw
zF6B!HDs`ctKebe>+6e#FS+ZhL1m*t;;K1flurcSX@S@HY>#`@{)B2b2YML@1JQWEq
zUwtCwc0KTUw*XJ4i~wclhG7kU>iBWlH~R0~9xS`sRd!{=VEp=P343S$qzHdS9=&@g
z;lR$|z3hN!*gS+=i|^5$xpB0q>@>N}cNG0Y&eGeTYV3P$x2S$5jzS-OCflI?+~dI@
zww&t2`ZKHPhHGz9y7q{srJBM32Z<vPmoBC(>x(5O3cPsBGco+T3~wKam-y`22q_No
zx1rIzvOt}GzY5^KqnDshaW?#EY^HT3eYj?3Pfif~a*tERusQ!Ab>8eE#((U_Iw@DM
zEG?5}D3{{%gm9ka7fGQ#N_e(LES@z=rQdHxO3C+kQ1V-qU;S#4J^T>~O}&(P%=Cpq
zx29*1aBBc(b@amJ#Vuet#DV8e*n)=&?KyPrRe8F)D!m9&6r4i3bHRv*@bpYyxb|xt
zA1sf94ed*~-;hFDF<}<1QL)AEkGBgymYZUbS2W(rc451%%dmIM3{t9J0N?jaM-{U(
zP^7yKl+@?bql25^$XR6J{x%$_q0Mb6+XdfJTh6N*!)k&G<UJS<3f2d4$M8;Kk!A+0
zt{z6Z-YZ#uj0_ePo&uW_UHEbbTRc^i1sftA`1*#mxMI?C3j3<hqtmX6SFLT>$w~{u
zO|u|o!fvF@cd)686Ia=OhBS#k(dlib=<OGd7JXA_*0T(W3o3QY`U^C}vNLq-HUiK5
zISuJU7V)R*=i-M04xE0&pANlt#4i(!#P_dj$@8AX{MxgH4V^sL{|Hf;r83SRGLnqz
z&1hc#Od2h9sAl}B6^k6rNV&yV=-BZv^vUl5YCi1(*-V2aOOl`_fkj^*C$#mL%x)vJ
z@X6^FY(2IQeO=oDPIofmL;Ws7jMED$ere0gwGN7{A>Fv2Ll!tYC{fzUbHFL{SYt;W
zH0=uJDtiMQ@h^~;s%FFd-EH7wYDx#v2cyf3R`JF4M`HHFI>C6ehp6Bxz<X~uJa=>#
ztxu7GTUZMjSe_S?8j^*^`gL&lTW_i=2&SpplFxhL2$)b?ipQ1@!h7nQ@Qv7oSEqMF
zwZ&fir0NnJy{^r*@k?Qk<xL)4Hvp=hE3#w8cG~2>7ZVqFgJsbZQW-A8bN;z(S^fzU
zx_Ux#s4w=BJbo%)e9^~$GW9Kv#nZ<I;0cMZ#rw)Y{w#vs^Bdv2-3-v(<%&Er4tq95
zLw!5Z8c@dp{{HYd#|ajkD2KpfSuCWr3janfr6I3(U~0%kNRMq1VjA=L=#>EeZ5Gci
z<#li+phEmI?<7{IYE#7c!&newFYy{P;J>TpIAw6Q=(9Kkj!g5CXTR!?*AnCSP@g%(
z9nWz6=oE3`QC$wyj>aM1ZVL~!Bvz#V3JShBfUedw(Z<s8*h%{#{k=Vz4BgJNP`84`
z=i69yO&)a*H7AQ^#QYJDsjFT$Jb7#hcFr-Q<Q)=Esc{2tsh+@Prz24DK}X&m5-!(#
zF`IpjFM)@*1NYv$bf`_hP)t<#1HI?$p=sfJ@xTAtNUP^%`c{wts+mJ1HpPB1%GHhs
z?K8l^XZ=~<FHanD)Clz?58K<z-TB!0Y#cv#7ms|YgGc@1v2fv7{%?vDEBbI!uB^Y9
ztG6D8XO&M8`+SDRv^SvHPwJQ1ZD+HQmndj=fUt30rMN9_jo5qMZU~S!kn$XBdVOFE
z*uCh=*A;)lOveuVH_?a6-;Utilwqvhzn+b?LrHZ?3MZQPB3=J$F#d!m72Y?K^0M1u
z*V$<Lx)lYzbrPp%VLGkpQGz>mNL|Vm;qsWy^SIq%2KP5GVA;49`r`DJ%59~G0o%>g
zwEHuCtd0=VXKUl@w23^i{W0qMRM5MjqhxvU_k>5E)^oI!UCcK-C!aVXgj+T|qqp((
zf^v6P&M6Ki<#9Xco#-m%?q12)r|v?VmR@LU^N)<%jtbgM8~N_XI9m2|3q6nZK=~nG
zaK2#;qk<1Zp5*Vm{$`B0=Rr9*#J+(?TlL`lg;iKyAaG>tKbdLB2{zR2$iKtyaR01r
z^wjqREjQ>Q)J(9!+apWid1pP$+^L8ongKt1=s}mAI^x&uo7p0MG@s1y;L~>>3bx&z
ziQ<dNcxrS7Q)?|WcCq1WmnI8dI;Lp)V-9*O^5JPkerTn9h2kiXALrkK!`V4pa(*XG
z>hh8%?eN9O{#k;eQ=G)~w!_iGp2+vaNqI_rZLy+C;<>p#r*nlNxV1G)FrsYIm#)z*
zf0TGZ!%h0z|2w3z4tqWML$`W;5+3E$iz|bb@$T_!@MYKl%#k00>TL(Ath+BmTji~+
zYa2-&_6%p^-^2Lz!xT)Y2qll`i+C|!PTF&u;F-~1Sa@Tk@X4YK#KD&(R<(k}mi|C*
zGzzg+%>v@P9wzMz-JowkE46Kl5e9zp$HAVPsk8SO{OuV<$K(<dtGyKNO;99*s3F6`
z{?&>eOLJwb$MlrdZN4F03sU*nY+d|a`k4MSIbno!{m~qoNwxNy!L6S@=qw(HHkBFp
zVz4#dy6aB~x7E;SU<{8B$|u`~(~MKs^ZXx&_+~<|Sl7!O&-9c=9|KaMqRxz6qulw@
zHOZCQZ3fIas>Pq<n!rUPi#_zcg(KHbkk1EG>iT#n##>a<f=L5#;r`R4^l1bxQ`4bk
z*Z1RZjKhVW6CrZ0I~uCJ2Z}AFTJJ(kdioJUG?3;SUxvw>lEp|{Pr9xB1!8_gz|@kv
z;^X(m{NbMymN$g3dxZ&B&yN<5<%q)h&Y#2=xwXRQ4_S1%YofR%Oqa)Y?u`{mTGAZK
zQPz+X3O=5vX!zv*kZn2uj}JRXR>v3f+wH}4z2P-)2>&d+t34xFeC~|)ZE>(QLM}XL
zm-^U?wd57M24P4_6sAl~#@pU&@#lxnWbb{JbYAQekGJ;a#X0kEu-{71Q*WW$6`DA&
z|5!@B{t7DR_|tHI4LmV@2ZzHjC^)Z!Lf8uG`a=<u%5Dp3_w86m=d+lYz5@!cJRzro
zcwzFs383vUjdb5{7K9;6T)O@dwH^%PexLV&k<vcle5y50alA%%QnrhUdgED3`aR?P
z(^4Pf0eQS0Ef#5u<hgqZmZitT#J)!$pv4-$?(W4A%NEmm9~CT#2qD|A6XC>v3Ya=l
zg?{UMa;Cik=BIDrh{-{?`pQ&1Km7<=OjN{kp$Ft8Y8vd3`;JcST1fV1q@H5FJq_zw
zOzO@vxJ1DSuZNuBtSBu!{afl#ub9UbxsTxK=!Gyo7<rRQDq8+fWwS$Lv6E6Y@a!${
z>E9svE{k}2b#K3L@}(#!g`3KDx?do9tDUGYMCvgbFNSg25%^%SiLifp19XmGjIZ3J
z+_*{*dRlcymzoLu-QhRwmlT;dHX8B=?U%AAzS7L+$YZM763jg&O0&x+C+SJ;5fX-V
z=1*2z1fLxa6nVD{${O}TiO)pbJ8uQG>=MKk-NwKeo8A0UISNO=2ohT#`9j{C9LaaP
zm;)bV@~a;2skk<o)1*$}?Vi9pPyYuh?b^JzdJsn!DM8;pM(jP!9NO$hq3i9{P?cAO
zhO&j?xtjyAy!#cZi18$^myvkv;SSpUb}S}Y?}XBNsY7iSg3B(13A+Zk;qQ*;aQcE8
z;Zedp=zmp9;N&FQH_Cusy?p?s*|WJHbifPKWt3(%mh|3DMnfYDh?~@#YG3Y!4lA5E
zrt~)jYz~x#Og;uSKQcg1Iy0miCqq(-3e%Nzn$;F1G{0pUq;nJA9{5JFTc*;At}i%u
zkpfLH2xp7W^`zD<RcKXt3|4I`d70x8DA>0e`ave&o0f|5VuqtZ-K$J}70`3^WVl#y
zjn}M?7uyqKc>m>GYO}K9u=D?ywLVV=m-Uy~)Lem(*CTMH?-RQ8Zak_hoACC51$6Sa
zAKNwR@#S^KyyMm^POuq=y{j_mQCboC_lhOu4-P!j=_Pe^I}4Ad?ZhX!kEzmp0&2f#
z6&JQAi4!uv!_ZJYyz=iM#P*y*Ig*DW(y1J_?n}h2tG1$3gCj|mG}&MGtJ2xM7OHPZ
zJEgOd|HL<l600wZHIhHvFS8mV%2?d{>@<xzvPP_O{R)c*{-dQOJ8|+1GdAo!jV?Rx
zfylvyG_&V$l<ilbmo8^0=vy*P?T|sUz3x(e;Uu)9`?PB23D{Jwi1#JRz%<)qVpEJO
zZdovZH~Q|PNs&q5JJW=8)t%_V9#i;SJCNpmj1x6&0p%x~z~;+mc%N4VZjpZQ#kHf{
zGisEuKsJZAAJn1C`Nr_2Ryqqt#NegSuQca+M{KPS#2OQ+t9&DxgI1i9ctNq;XR8TE
z_cE(GP6y=&>!<M0notzx_2H#thXWT{34zn<=^iWcnWK9k`${65H!sH?bEMft_e}A9
zUv-RVPZjIueiDr)`l7FTKj_)0!Oa0%rQUfCoWIvu>hvX}-sx%L-S{A${8$zB4poX&
zK16<FVjAxMdln??kZhvkBbawFUR1U^1XFV_5SdtEQK!Wm>RBP}ehyQ_nR0S|^_QN+
z^hf7Q#k4U>6&D}(#UqXZQXgq`)fKI)<Xd`$*S0t@&5<&&&MM+Lmm8w#<qMGcREH<u
z{UyC5BW$_j$Fj4QnDEn&M@uf|hfkA8_WTr_c-BCBN2x*M>_~i(yHawjTt%%urg$Oz
zC<V>^M8!#agoT5{S!v4w%wAng23rJb=x>DYUcVyy%_C9&Kr#xW&1s}j90pH+1Otwm
zqWi<^*f(q;E)9vocTZFik0r_-Yj(h?o<rqj9ye*Nr#_B~UBkbxDWR|7Q$hcS#7Il6
zrQQZRFga;An>JaZ+P?&G<<P$DT_U*_A3E~KXC3*qPrB&RBT8%@wg`^xG~$5wGw^`q
zxmO!`6$`IB<Gr|5I6>mJ8)zGl-6b7Xe_c-vlimrFnv^&){g;r|Ux_>C*HG$iIT_oj
zv)>gHu=|iF4)=<J1J)se*8n$Wi79hZt3PAg0`xFVqw@1dh0bp!4uDN}{A~S%JeQq<
z`SYCkZ-;COnpaHz6V*U5uowN-o=2*d$I1NZe5yCwg1&8%-#IH;C^|g=Kdn#ToxNw^
ztBpIQE|i(n88Sq(x8tGRr7zRWENan7r9oRvSs^x`r*|o#sJLgeU_(7Ewo!-rGIiee
zw+o+t=**h7y7<Ici=Dq|<NdttP#ClWex6Q}op6%+eLCe7+f+yTOV+^aiuct0sKiXb
zjj%oDBkil~hZoJih%eV~2Q`mr@}nO8SswO+R6f@UKg0U)q`_%mAlHFIE7Zw%sy0rj
zT#8TLd*PF!4G^Fejd5$|@x-T*V0!i*MP0oM9rM=H#eznOQ4|X*cQ=Y7_edU}@<=Rs
zGMsmRKLyWgoY}YkQLbIS9P7sPW$AGpj}BLbqSATfUp5tHY3}AbxyOZB26{ZJv4!@a
zu{bbHn)haQgAm$WRkn66xsQ1v4t};%;sR}fzzeFJXs?AUhZb^E(PgL~c8Q|(M&R1r
zCtyPOJFt}I<}V)GvTh$URP<;Ahw4;v+UkHxC(}4M=>Tukcc)5|V}il*8-iF|iq~=^
z*HfG1Z`d^L|GId%-t2~8G{%l)2j`>ys>R%`q7V8CXN7-0Zo-H=i@1YP8&K~wI<-ZS
zpE_R?JFHA%pV+x9Z&SqcMnRaHY6dsAO-23vyZFvO6)HHGMdd#S;rqYW1n<gfc=t6I
zX4~cqk2@d6%7B-``KVvwl~;YBZO<D1uSxQjjF-G$H=>|<iu5`ZepL;%YaxdwZN8rq
z3}q8cCB|SbT|RpnK3m)ne%sv;iyY!HS(-Cidnm}Cte?bJwrTLPj#ZpCM=qY}tH6)`
zjpV(32h#qb61Qhlp$KD@uu<lMp>Il|@qQXCy5F6h%>TiWy&LGl-bL7<CQ;a_lntwQ
z0=T5A;nsfxC8lR*Tru5+F{q6G8(;%AjeYpwo!QvUJ6Jfr)EdY3-Gq9B*Wp6xTn_6K
z&{QvqzG;@gph@|VsHDd8vtsa{!UY<iln%~|#-V4RE5}aGK;>-@Xl_OmtQey$Ub1rF
z=A2Prx#%{9gxccjDgJn^-3ZNE_kz-c{@6OTkrW1}z>Q@S<iU&P!-0PBG)}Wr;w&qR
zckHWS`a~1FrIg8Ey6Zr4$Dg#ku8N$c`+wru%KyjSc{tSke}6npsVLHp1`S1|gzozs
zrASGMP*NEg5m{NGl$4Zq8OctFqC(yGIkt?l_sR$vQMQcV`}=qNKL0|Odtc|A=kxJ^
z%)jgDNN_x^XW$n)xxyW5fqb9%gV)cyAimszpAxh<^m7)i@)^!*A<;agz?FCDt;WWA
zdffa>Vu_8q4!^x_P}&a!mw_r=p56w5iYBCQ?v9<zw8Y>vGs+4eXq<c&-ln=?*v!r_
z?~4M-YS)uic|4n*zXe8h?YLlmKU(fqEX)te6~e98Qrovfbi}6}PwkS)OB)*CM)x-G
zFq7QODxc`mupIuCwv`)CXi}g~8m5i-0r%RQ;PLQooD_0eDDAY4ZlX0hsbvX!KQ&Tf
zZYO>d^HF#l63^|sDx%SM=_ass2KOjC3Z^x7Jf!szEgd=;DmoO<&!m2MDn1DvR<Few
z&*R`o&SSyo-W*UE{EBR){eA0S5!^g`gO9FEX1(n)Ox?AY3ga7T+;SI;_&SrFew~HN
ze`9g8<1-l9`w_HzvJXGM9mujx7X**JN#gk%nnGpSPpLmM4d<Llrt58avb*&i*d<bh
zgYOveZR;E!G<b`Ao{s`w+2Mir?0e&Zm53LA`SH>h3UGJd1A4Q;7d9BE;mF5NAk=<1
zf3elTH%7&>&jCf)T9W`{%L~!V^cJ1lx|9B<C*s|Y7ujR*SbW!zO5d7WMT5}A*c#^m
z#;XVOZ@=+yN6L5E)!&1de|eboP)n@%=8A{=o6{O!9c(v#4;VOR3ztetDcj$YC!K8v
z3M)5C_sm3G6EGWxZvO=aYWBQsgA%$-ET{8N=3`ce1kUnK#I?(2bFZvk==9DO%>s|m
zp|v|;!-EL^moZt4Tj|G_?pxyqPj9iuRTE4yNPvTl3Os1Wcd`l0;xdW(s~%Ma^IO;8
z_u7LP)IC>B8F5e$nh$_Njy1;`?SLQ>MT?sYIp=L(410B2$d!Cr{(c|e@0kU%1ltxe
zSbSgJ!Oja6!quonz8gPTye2c<4KVL_o7|;!0Qveyl27juJY3}*-43m!nfZ#iKH443
z=U2ldGY|2wmm*(lh{ic>)A>^42iP5&#`V?VtlLYMR!1pg*17=nxVjpZuk4jAU8F!S
z0wtDaNR(i*^cpzpsbKT1-dGnu0JjxA1LHwiaG;<8oT4hJ>ZKvNWn>Fh3L@Nz{t4Mo
zC3r_%2VHl`Q!`;ARW-SR;pQGJ>dl1eGYq}2ZNPwgJ@I3r4ODD?Ob_SoW&eReyjIbf
z#2F`PifRa^DfC3sm3JU=%PQ3UK0}yhVufq9?@{QXiFo<sTS_?Qj~0EM`Qcbyw0Uxr
zYfCa9x6fW#T%a<m-CcvlUN!X5`yn;tu7;uWnFft*6-O1#fMZ#fd{|YTv$jfI(ZxeZ
z;era^PQC;)X1B^_DQ^)CR=yH0{JBr5_e^+?mmwV}N#@XjJ0LYDMD~jdY0ur^=wG{&
z#+wAg{q!u%$&z-Tk*8smQ#)L94tZvw4v+nmj{E(FkxP6d>OcPne_L{>?c`M|Q;+4}
zrB>X>DjsGO+QNjZSzw$#5T@E3Aw{dc!mxdBpe4iv`uA&uzV!xdK1Ycat}o-Y=2k-f
zm_%B<@&+uQypKYy*2CDy(L7gq1V#qglI{(}f(g^nKGF=+w=c(3kFoG7vH>nQF5$~3
zEOAAu0Z-O^1Qz4$(EXePRT;ZrLoZYOs*(b2QLfNqgR9smb&2nVZltSU)Op)gZ4Rz<
zV9!f-yvU&|HEta(DzB=?mY7i3{lOJKjESHl8=J)^BPW5sw{(kqnU40GQn-Va8;)?w
zp_urKlvG$oil+>?sKtbbIO+?#X5R(#qkHK}#vppQ&xRYycFOmS?1;&epMlN7qp-92
zJ?U>B#)W}}P_!^#*kSubSn)%f>_W?F;RPjrW|)Z|ytA>zu{{-CiHC-&dwjsggB7GX
zsH}S&+L>A7nsFAOtk4%8pFP0u*ZJ_&gASbBe?Ps87|yScxZ-5*)1+llTs^dJpzDhc
z0_T_5u*^oATQ5$MiwdVfmiruv*9=AH;2T1|U$JPe_7KjlFvQYB+SphrL*qSLVX)!@
zdg@(Eqg_hnt!qcJX=yNR?Q2VeOwQA!nuAbcG8pQAyn?ANwPfQ^4X@3v$R8ZA#Bzz@
zus75SYYjSy2ZNtd{*FA6Jm!n5B7C@Mb7vUev4{?QKE|;Ue<>_h;OaPC`dd>5v(GdM
z$GA}(05+1BtQ|ccq0ZO)^k%m;8`->LgiK-7Oq#tR0dLCu;knom>J0YMRR2L}=UD+e
zqRi0!MKUTpY@_GT4Z$Q<1NW=VqjIN8dB*#6%9s+vmB~%Q&kd2Z;aL>EPTfJ1*6gVH
z`0@rUNsx9BZJjYask6kQQYL3FXa3qX0a7QQrTRkx=Pp|$e|vWxCrG{XVg*gX`iv2d
zIn{$tz1c>dU8_k|RTKAnJrqLw&PFjMj?~ZQ!M7)!*stjuD4QK*ivd3HH%x^!A34(B
z>!IQ@&jtK>{4}1{;?C<jl*yO9PQr7K-hg{ZJlrj3=vLv&9ox+mQX88g!qbVnx=TC%
zPiFA$&QbjF`5@U!zyHye34C(jWOi5X2unH)g2%(UaY;!hA>h+p;<{acJL|yXn#2aY
z;lpP~bmZ;7&WU{#$IvTp1L^%SiFQxU5yr<<(2^zZDEzZCjz9K<ui*rUoYsx+1{@Y+
z{&baGbL&W}YmwODV=<TcWI{*j&TO5(kM>!9g_lilX=RNLSg73=H)Nls;gk0Ay8ad1
z<-LWhL&A1mHo*`B|C}OBNauldrQ#s}80Z_-D(h<DkK?U+vfjkC?Aa}f?O_T>K2YJ9
zuR8Li5wGd<nL_HnMgiN*PmqCDAzrIcK+DzpU}^m?h&tp%N0e3I-by1-j`zT^${!#g
zIzeo?n1<({&*!(i6IFW8fnGT`U~%MLSXJoj`g>n5es$4A;);Hx!N=_&HnAO^aK9ov
zR+j$V1h1iS(y;wLF@Zw;a;d%13DOA|$m@d*IX|j$&~Ck3p!3HEEUcozs$dPj+-b*}
zRpTM9Mju-xb{%{`oNuv!x3ygq=I-o<MS*Upcs&4T`=5bIGUqEzVNj*F2KeJh7<pQO
z9GwjL!o=zP=~fn03~8lXN+x)y_daUf>B3JcRzdX0ejIGrhhl4+gqwT%b9&Yu_+Gc2
zGCoxZZjW0ia7h>8*@r{$XYga{d}bszIL+cJBWX7`cOfi!?kF^OI3nzR*Bj1Oy6~>A
zpXkb^dQhW$(i&Gz>Z=T3#Z3ox3hc&r=9OUX*IXW9G6Svm?-DMIaiORcC6M9~$N;XQ
z*X#;@XefD(R@jocgAe33C5qqI?1idlb7WKg6hOe+Fq}GRAz!cW$SxhG!T!^8d6%gH
z_K&pX?uI4Qpg5J|4+Wv6QXY=7dMnFqO~j<9J2-mt7~VNx0J$dVioKUhtU2SIkoLV5
zI=Wwh{QV(9LVib_<u{opRus_+7ky#Th*;2m+6L2)e}EG`w6M#LQMA^wH|{uj0cvmS
zkcxR8&HL#sewcWY3xmyRV0;tpsSDs&6-{tReY3c^VktQUJ^<?&$ww^tLPwANEuOBt
z1rZjVAY_M>m#++v7$@uTgiaYHm97`F%^!+>>k|2>;{%v`&<WC>b-<ukIhYhxEjc$k
zkX6fYaeqGxR$WyHdAIjtsE+~e9#xA5#;<5>_(plym7NSnPRoYRnGI#%CrNMQVR+=-
z9oq6^GhOo;jmCrbQq%&1p1sDCxmqDjZY_ab^;&qsWFdNWO@=+c{Ajz`JbBK5KwkWJ
z8>rd!;=}!AZ2o*5>jr1=`v_y+czToUzx@Ay`Tzg&|NrIx|I7dXm;e7S|Nme9|G)hI
zfBFCa^8f#T^8eTRY~-vl@luX&33UC_3GZ|UOvqk{7Y8keof{{^f?+xEX}X!9xnMla
zJfXp6Ghc#nrYhGq_lFU;d*bAvELN@^Nt;(D6JHOfT9t34-_0G_YClH}=`7f}@1to6
zOKD_?Gv7{G$$fVp<feNaxok=jHFms)rxToDOw?8!b67=8-Zc<kFAfkkecTMoRXdVL
z$81^@K1_D!P%&=vb;rh9U0ju21>&>)lw<aZJZ}|XV9Xc6U8fgr@i!C_oF4%l8^wXy
z9nj&_Uom@C67~F72ugFKMUy8FX<PTVbZ}-owVg<Z#ro>#C<u6_RTHCn2psU}K74)W
z54#uNhBH-eSRc@n3};qDYP)8-G29v#y|_iUji#{P;PxC%Z4_~Jf@|fszOdo?2G^tZ
z#5R6|IILWYl~<pqSBaNpe@{nBci{IFekEP}e6lCzKC@s8^990pXNf<)Cy8!0_Q1m-
z?eTJQZ~T=>q}Hbg9}fIUe`+^T_!cD`+wB95+o**7)=Xyo>0MFLr2rOPO2Fpz4tVvq
z2AM~lhL=S<_*frPocbvi(kAAE+1NZdRp!cvV{3U-<ThTpZz5m%(nNjsKN6I_$mQ^L
z4qdKKC%vWJVgJD{cwheq{QEEh!ORR)@7nW~A3eqPAA-5OYztVKor3f$vms`*E)Fbq
zr*(DL#raRy3y<Dg%9b9OE^IOV0KL=&=#=nEV97mdqL&U1ui`+-G6P#p=2MY|F-BM<
zv)RxcWU1MQJFQBFy4f}`-XjqmB0BJxn;w|3`=Ds~xToOm=KxuQUQ(RD54Ytm7UHMs
z@LDMszWQHhp4OBjHm>;!Zs}{;<yi&RNtv#D=a8DE4DFP)9pQHD9dc`4gfoh2;fdZZ
zxZJ#pF8;DYebqRA{L4W2acl~R>77{rk%Mru!jAksjX=NKE?Qd`k8d;2(3Hg8r2A?f
z&obXG1pDo!_<}6F6={c`r4HVv?asX7=^}DIX2QBs4`@)8Dt2m%W0%GL@GkEVSueE2
zvEfqpJEaKx3ulT0EIhdKTnW#sv4E@HhV#BzUKm+555Jcwv9&@lW_NbrM!zg*-q{Z~
z4j3;i`|=a~ug>9%`OdU+`~~P}w;4XnIgJ{pyF>WBQ(~0FoyC*RaQ$^H^;>ijE6yi?
zp;EELRa}KBUXLKCZU;>~T}IzWub`Fta^Q`I8z%Y=NApozX}?PvSUW5hubE#J++Q0~
zncY!g?b|%wQ9c31@8RNl%MqAzVKO;N`$_lkMeKF>3-N9@L8E>>4jiTl^Y>gw^9ViR
zYV%I8)zE|Ft}P^dx=xdJ%)?ui5u6!rjDsfIbJ>x;tZX|&toVCQ{;Fs(drUCEVO=YE
zUQiN_n^7w>Nwmfz?H|f^$DXEN6SX*1a<}(%mU;?dj@WMG0G@L5DD)U^Pv>4ll6?O^
z(Q?Wdt~GikDmh${)lA5T(X0GGr@9wCU8>8MoJwF8NAt!)E1YoR6k9t=9$(=j86Dg~
zw_aa?&$C(uw@6jq?LB~ZSuPQ#edx$5FAx-2u9N$G87}#Cm~>aW;$^*V{JAayT`wM_
zvptW&V7DT8?j+?Jhk4@YZ=G?O!(lM_b^=B!nUKnTK`b`=PTdFRQb>j$2j?e==0oH(
z^V=7)jd~{Yi1lJMk2@gt-%IvSX5wZ2!8HC=AblG$m&Oj1`ln@%Vz1aheB^LmQ1n=Y
zZcjt$Wn2hpPa4m+Q@_wZ6-$N^Q`9O>rn9~YSpGnp+fMJM@DA3l4)Wi$q+a)gom~T1
zJj`L;dNcGHG>-xv9)t5WFW~6WyY#C09sRwe#;KN@;qdLjWd8jG44ZXGbQ-*dPASfV
z1D4ik^<k8-@AWR(;o7Uv%TARQx~PEufjbbpVmx2HRYET9l`*b$Jcr#r49n(T7N5NI
z<F;{*aNk@Ted>B(kFMKcRev4S>#|>XyX~VGb=M1x?rwl*YqeSBpD~7<(_n*1-|2|W
zZonIX+#I`4h|gOF*?mKV8I?Jp{Z0wyS9ZjE^`^KjjPdWFa7c-%gG*;$f@;_=dcLC^
z!?fb*rqOsZRX#`46O&~D)01J*(2Y3w*jHixHBCO2ewSXVOa52+7a=TV1qTcpNbOP%
z^SvlnY;Mg)G2;q09NmH6S{~904|mu*f1c<yTEHOtI%!v?1(BmV!x)Vs8q>RmiZb*-
zqeug0b=1e8H{-bHhIb&vy4cIenO~(`fL&MmaK)zqIBG~J3Mr8wzqkp93(|Mi&)#(8
zZ3_83+)B2AuZ8+Ild!negqwyQfP-Gv<h|dQ9u?W4LAn7%@5&?ViBfM`c9E<qH)8j4
zHypL921gZ6LYpF8P}rh}HDSBy+k!KoGg}ct%C-qxF9|qSb(Kt2t^qp7kHd+>52(*I
zU(8hp?0)nD6lZkA8Mpnxa^W2E_@yg(=K8_k5wFDA&5jUI)PYyqh2r{IJ%yq7)A7Q)
zc$m0%I6r#4QXF%5J0A?NkZm+?l7%eZjHi~w(5Z~&xO>eYE_`Lq6~C0|Y43P(>6UIR
z%jgSL2U_TWMItXcC}7^tBz}3XM!u>xnC~{15PV3aM{jdwqZ6Bj9gmb)t&<`jfBhU@
zDQ1Wt&7_-Id<oe9ly<x(zu|HJGRTnL6Hmu=;H#S5XzHoKJhF6|_%&)H6c1l0RQ8A?
zBilGOaEjxuXFIaKrw1F&b%(j8gU~+P0{dL?#pb0hXr9!Se<Xe-{n|L0(aj3cs3}z(
z<K%!Au7jv(hCcmnGR3*QtA%y{I&ko^cMxiKRSb)HChn}h0tFJQ^sapXtDTn|RL28B
zVQU&ZnmUe_E#4~ByiZ_<x}WsBXgr2$t)n#Vj{R?>^OwpvQTK;6J1*!hb#40Nnr2P3
zc(6l!da4e>ulI+Q*IQ|@!hFbylC#Ei$%y)LDo^~ngY4Aq2%aI&g!#62=pH?hZ?{e5
zaIO95CGSe*-!<{X!F=9cG9B8>npkz^D(o7ahs9~WoUG}G{asaAW9v}9@NFY*H#ETM
zrUT(*@l1MYc2G!K-JZ9X`@zr?X4vIt3fWnnq6t&piOym5;+ZgS{?dLl&Zw7iT7`${
zuH>5BQaOQ38?{8S%OW9CF$r{Rdt>ON0T^?j5AK)p`EMkb)_%{sP_3K@)qkbC*RyXz
z*o<q`lvxdfefH7j-Trjl)0-3rY=_FX_u$uFKfIxM3$&(2@~I7d@leee{<QuoHFQzL
zs>{#$`r&I3G9eL$$9iMwjee+kU2>zDtMm6)Z|JMWGZ<@LA;#p568iWqrQm%RY0SUn
zgf0#^c!|^{nf4Wa9tHI4EOFTN3Q;-13X_)R<M&zi=u*{%!{2QHvjTH=Sm?>8THUa~
z^a9<SXo9<khX^fJo)}^gN0X)7_@xI!gnxsNa8UGi3fUMfT<M=COwW9SkJa|W-hNxf
z>rYboq-7$_yCda&N~$pYY6@)DmYAZyjM@H`I!5FRV7;$W*!FBWPV5@S3RAWUFMAuX
zLjOvt$tVK%*L~33dp(SLdK?{d8>O4LJ=FZx<xlVXvGem#8k_A8eeGA{=@Z$wbXY#n
zn`M|c^Pd=^7sSC%8`14&92;#vBkD&u;LV?ze0AYrp;qEXJheSb8wSqh1+yz<#Ts?+
z{(CAfU$_^?To}#Ab}l2QOX*-!Hiz`=%E{H{yLfk0G0j<$?`mC924*pqSo7!>Eqbz6
zKH#+j-}}2C-nF=)b89s`U15rYI_;yTAw77{ybS2{X%WHXk95Q#oR`P=NsRv++_lOO
zTqORFl>ebJ4K?`m;vHQ`)!<FP%II?5N%5J~#rJ5xm&OE(q-Au0I`n@|n|oP{$It2V
zpgoH~D(118<zg)DIbG<Xwi1;zcj1&Fh8XR|v~HjVrXF}j!mCa=W=IY`$j+5@Nid?f
zI~Vf66&ZXBK8TM;bmc|i!?CTG7Rh#tV&l0s;oX_*LeCq<tl_*)2<@rJLB>ja(Mj@v
z*Lh)+;vY!Qi9~%X6FkMzyz$W(JH*CNH+6lC^ytJJ=c{0<mJRQU^nvq5Zg4dC3M6*$
z6Pl{3;iUgnDwoH@6B}ikVQ@z%|D#7VT9>=T))8tO^NFQIyYF}8&N+$nzRy+hVwDm`
zr!K)l$(=YrO9ftdB*T}UbJ#MlloZlbIoNI^c{b%!ySS<38*d3O_i5psG#MIXM`E68
z6q?j!fb$h&T%vQE-lu4ZxjUpBht#>g;PaH8u8xQ8%iP%<j?*@d5~n_BfvpNYsPa|^
ztL)ZuluH*F<G+^0U~^%Mbu!)wAInx5Kj4V(<{BTpPGH~Qh519g;o+iuGSk*&uefFK
zd3PR^_1q5{FJn<}_a16qG6X%WAHu^4Yrs-2@v+~%B%i?U`~zB{79QZ$LjzDFRN^#^
z*+nmNo!ISY0EzZJsNLwBwAe<<!3^#TN3RcIy_n<J__CS~sgH%_jb3CnW)yde4&#ca
zO|q*e61c~m(^CIrHw?Jj9xW7uaD>TFT+$YRvK9K!^G6}3+ID04!)DMJRYN0o_vP)w
zkBLs(0{Qn6RW@pTAijC@mojXX!SYfqt@l_@rP|AAwaGU@dhEcGyEdXq>K-vE)QJ`p
z{-EQA%i#C}BV2#K6XPC7_<Y?}EbUUoXQHf8t#SctynH~P=9^KjZwJX+Fq-4TV};MF
zR<q|tCt1+p{_Lcg0vdVR+->qu4)6QI)w$CKY8jUf<HBv}xzHZ3JX_0OhG?Lf*jqST
z=gONs2XVx?&Aj3MM(!?oqeA0cG1|v~NA;9?ZnYh7-MOFgvOfvfqa+{NAL=GJ|ClV)
z_P1cq^PRB!fuB^Sy;!`H{0n^l8bME8D}2#uK5kC@Dn^7%1`DMTg7u^;U?IJ66Emlf
zw(fAqim+lk`DdE@P#bCaT&}&+NQZSq!Dpl$haG=TclKxCZ0i`FcAy+uKF-C_@>MLp
zTn7`bsPh`j-F&J2dkV>lr$yb}c)?|5(X(5+@L}Hp8dDMpvB`T#?XR5vE-{gPJ(C44
z{kCHGSQ8pp_X4i=>BR234`@K&V7zZ_kK@j6hE7?I@<Iim3G#pR@|`Zu*)j<4S&tOs
z*7bybLvzIIoiEV?^Ca@OX@*yho_smU3M;;532N6B@ZcwjnLP%fLTsY-Cyvqc*k4pV
z<-0iXK(j2X?Hz2mB>hf=ora;!rSxiEXidhu-?F0-Q=obNJv?=(15W*s49h<6fm)wq
zG<VfS!8NuomQI(xW4Am83+cW3X;DA8<*o_m`hOCi9p46HFWe-TUV37qb9=hG^Ar{B
ze=isg&V~_>{=vwBVfZ3_GCmkFlN-EdWA)kd9DH&iglV6KWl=3Od|41=Z0yf%nHT8z
z&t$fJVTWHQPNv09-^6-ZJ7g)FsH;1iJ4Q?Yzxr!%my|K+<TVXjCjSMk(*4kBcso?h
zzd;@5=8>Y5F<O0UKkZmkOU^qgaKh>?qR$(~uQg|dZY$g2oI-tWX$iu=E!SwQW+ZHJ
zQw7r05(d<Eqvgd_7`t~IJvm~Dz0?diX3}c<n=0}6r99k|0X^`MOD>t2Z36|b@2)3a
z?xIyz8n~p32YC(iC64+{H+|wo?zd2K*FA-P-M&+~_jeG|$I{kGeS|wL4%j5mCQU0n
zew{A4Mk;dY`1}~bUgtCT4DP|9zLC83V>?U<u|`!DM{pQif<q-v_0ZP|JYiQitiG>A
zO3QyifX^o|OC8R>`wIBQwMc$sJdb}TDu{=-1&Hn4`{G{NO}J3jCUo3<6Gp5~p<Ne#
zP+g1?{*P}KaW4v|<<10^Y3}&Zv<NnTOoPrvYr(YXx9rK-3}L}MFZ`f-R=~Z67^0Jo
zL8;$pc;5`@8M27J_}`O!(_R$l9n7Y0s>m%ffHh|wgKsZtcv6H4Eh=vWFL*^47fS3I
zF$3bI`D0a=xe#2r01s}q#`q2&DJst!G`HEEc&Qu5_pA0o!rx=Um6|WyuU$OsFZfNb
za@xoyV-9Y(br=r+IzVOh>tW=mT%mBh<ZRM1!^%P@SpP2+`b@Yk-?6eCFW=e~Mafz6
z{O}!UM;<tGrac-?c?KFLm$7K+8?kplBtq>qq50$(4C;MUsML&w1)rzD#37cPsdids
z?A#thYue+p1r8E>e>lGF+=~aj{V4QPoQO-~kHCkjCEPW?A1^NX$p88l@LZP>sHM{+
z{BTIX>z=iwoBR-tClr$NvJ~9bL0fp+VhfuSB_>>x2N%eau<rX2=%J{Ho(J~e_D*e-
z&~G!4eGpW1_TZvuXYTiO4Gk@oJcUnt!Ru9X!R%K@j?IduN5QsiRs0xMWNd*`I}}Lk
zs}ZR5@Zx_{GB`4_tDra`ls`}XOY`lHf|i~wi`{)GeZ+4%_c4}zrC#{y4JA}FVx55F
z4cYhgA~E>RGR_)zNPJLN3C%+r1^-{Fc&SC;Ta86E%HGo7@xn+{nz)xb-Wi8!ZTWO{
zx`E_C4a2z)#}SUG5*4zh(ww1pc-W@ys504>`V4SEzpFXeF+P~wHOGPRRey5oT?N1U
zbw!utYOI(w9QSwyRhM=4gy8PE{CR(rFkqXJ*zV9SGTnHZQX-?cuhurYb5j-Tb$8?0
zUWs_ete4~h7{!L#-n1fr4~0#>39EXY$0`#KoU}U*?>v!mI2GosJwP9$#apyVdYcT1
zQ$qDo0sKbd4D4I!&6-OdP{-c=!6~>4xH+C8ed}X1b;nwAv2qg+IQ7M?`4%*{eKB0Q
z5A5NQgEO@!Ll?Vs_%=CCDDJ7v+qO>Qw445PCdQNfyW8=VM|xtSj=<BUnXq?+7Isr@
zCcTZWY&9#EKU>=J-a1np_GLC^Dpw1WmyD%NV=hp)E<?G?oxe~#@gU9podPz!Ou4(g
z0p`E1p$<WQu%o&?mbXcKi=-NC4f4VE5qS`?JsykNUF5ed>a6k4hu7cU1}0)VnCjdP
z4F_eDdg)5;die)^T+~EOr?yjGuomTS(jtir$>wEYw11f?na$9Um$Xj9#PQ0Q;qC{$
z-p|02U-6>8ses?M<b&qG6qprs0>0<2MzudvNOxuknoO>zut8I)rSlQE5I+WoMKr)A
ziQ}a=Yc{m?G3Bv?HqnNK$x^;6m3MyHh0!nEX#FyOIRAMmeqMMPUVd;x^J_;*3fHkh
zb3d-t-p^;>tMTQ0YjV+Fz%OR@V0GCM@n>EkFX^8IN7g-p$0J5@)B`1nSK!E=2JO-4
zwzlXwx(-)N+eRrP3Tff@IsDMvltzRVa>?<{ptZt<gA2#;@Q`jb>+g&Ot*A~sO!VY_
z8wz>EER~aK1AFt>t}kS+($3_>{`s)(*$h~^IhLoL(*YrE1a}GkC=XJ&FYeIW1{9kj
zOl%FH7wSsXpcxN8f97G`@?GfOZzG1<D6r|CwURv8m-EIsbLV&+JmxVR>YAfPtw}HC
zGxm4GJFAtchy60Fk^Hj1J4Q(T(Km7j+YhiSWH?^`c^nEFjbTBpx_IO2X}Ucn2Kzm=
z7h*Fnz@+p6_|v<Fbfp=5QoIq5xqOqa7bf!Z9SQJVRA*`bkI#nfrS#Y*Q1Q-GRy@ud
zUmA5q>u%rS{*!@JyDy8pj9ycmiJTI@Nj{LzW#Uk+J+LrNO=j<-jDt5ZS>*_LzcdCO
zKK6nPduz$@t|xjbe4-bZmC<)*0-4OWqEi#p(D%?{j+NXJsgb&JgBj`UZ2C$(JfoXX
zpwvP+Q`(_ogA;06D)Z7LQ_NfA!pm~jfR9pVv>V?9+Xr}J=-o3EHTNR*(YZ@sGD@kl
z`!C_gn5UpV^fv5mZ4xz_yiqStg=^D(h&fYB#3ToQF~I9i_42p`bS((Mj)nt-bCfN0
zBV(X_h#gy{ufPrS{MmTt5{`BIDf6k?&I!x<aLS@svYDa<yO(?OP0M8TcQhp3UHO7b
zc^XVO`VE$btFhgglkjute90BMj|1)R(7V)kbYq&e#H&E``mF@Prw__z*Vxj(*2!>a
z-%E-4qk)0t!)4uUo$3B(Z*nkJpu@iIV!!k6;l#p3csEz#OsyKs6NhF)nr;r489RW+
zgTdgd_F3>Y=!b#cGeE5<8XV7j68f%s2yge?r>LQ4Xh+<6;gil1{_<uE)_qKcuY=;S
zf9Mg|;+qC<<H}&)Zx!Afv<XkXSLC!Q31a8V?%3>fh-dv+EO~NAapA505~@gv|85Qw
z4)*y+_D{Eo6aUzVZ_fOa5Bqu=CJ0sF?wJKDKUQ$-`wc?<nhV_FVl+fA7{jK|oiSU_
zhX#AEzya|iVdlJ2I2nAEg8HdKoWx;k-?a$hLnfn6{aV^GQ+ddmn$2)3<TNH8Pk~OQ
z&V1&{82;;4!MDG(2xAo5$sYM1lnv6UhqkUS$n0vaXzSP)^KRq{`$qNPbGL%<<mfmy
zneK?LlU#A@AsN5_kU{B9#yqQj8IS0d0Pk<#q_KO|VPw!fGB0(5s?`Gp%kF)+dX=^O
zgY$BpU{DDOO?Ft=vH%nNY2uAJnQ+MPuXrx;0eA%WL4|G#=<{Pg{0%uRrn#-Ap)r~`
z>vRTexfqJ?PPpScizmX!K>?_I(vLPfsR@3DX@a(_jyg#U4X5FMK(A!0>lUp*jPSca
zZJ#d)v9Efu-^6!xY<H1ZH9c7jwb}~vL+vqffwVU}bOE|bGy98%9P!gT88zJe4&xTy
ztEn$tj%UL<&_LBDxIessE{`8fsw1|-65pX<F7Zy1r?$s~t9#MSi~9KdTThy}q7V;U
zo&vveO|ajzDj1(CCqn}ZOc;C)#_dy~C*@&an0;Ef`!<udd^?YZ9q+>C8>`Xfdr#M<
zSwDn~OO<3%+zJzOG9{MDR;b&mjfpGI!QJUn=YEt7UwP#5cIQHQ&iXjsrK-<opUg&e
zr%t@^{2<uZ=M_bK|3=GZpMfRiSwc~9Bt8sF<ooljF>tXduTlC5%I@aa>tDKX(W6Y<
zbFDLZ*(JceInw?&*9?PBi;&u>J%1W|T2QIgVyE0qbg12K(fsIcdBpi&vbL2SVOQ5i
z$Z1O;=Zd|OyKIx#{n}$ePnuN{!{>?W=?Yx1M4y*Wy$K5%r0=*hnN(1?m#p=Mu))QC
zJamCF-B_Il1#G}CcO8Qj7t=`AH<7J>-vEVSowyl{1x+1ev{+_GpL_2SZj80!KL`4=
z?b>8&-G7h9jmgKD{&sZk-Yc-XGM<}dnS5i28(Szli$|8u5@=fr4uN8_F_U(nRhDcS
za)~0t@`RvsjODkazKFdE_*ZA*?xkjo-hNPJvks%wrJGAjPk^}{;Zgs|Z1H>uSnrfN
zs==G!uyP8XQ@$-|cPodO52_ftL=a1AUt_rUKjG7dJJ_=NB)Bc}!2@{(u*6w}1xsH*
zw1t&eJbW1T-ny1Xd~7eO+TNwKh>bj4Zo!3vPm9AQj;3edU2*Bh5Nv24MJ^3Ba8flv
zwm8>@hnN|lwEu@Z10B5o(4L&K?Qy=@W`3W0l+O1n7e>uoOpo7eqVFY<=)L|LeV+dX
zf&-`H-l{j?QIZI~B**&&%^>KN6#}0MABu0RPKay$b=lkRir8aQ0EYF>#kzBvXgMbi
zBO0H=^)@}8a8ibMDkf6&jnDL2=a?XCxK3fOl1U=%@d2X>3TQpfiMKOcQF9cfEbogZ
zy|?h1Eni6?{xq$T|D?ZHe?n7r0><QzWLeC3nDSE@<sAphX{tT8ZXZK2w`)YZ=b_R~
zOCL5kcgL*yTppA76I6O;k(H|C1b(v|o8MZZHg?8}<PbF7DfO=gRAR7wq!6I*g~z^{
z<DcSmdZ51+A6|_FlaLf%azBg)OTEjDKI`BN$S7TM>9m<faK<h<)lRz${S^KR5s5K$
zs7wX3E+)~D32VfHmJF2N`6k<={tN~S@2FW_1Jn98lA2~Jez~`fMh^cDUr%=Az+S`U
z0k?ipa~uus+1QsaCNa#NnJxxL{E_G1P=!(5E1+KL>l`{!gz_9I>wWlxcq?HME|KT+
ztdu?QF6A%%>opHEKWKotMKYEdJrd?xtfo%-5*O%XA5N>Aid*(2lTts4Rqb9&mN&1_
z_@(x6GN}sv3r6Go&1!H>n*aZng!7^&^SNaEE&5P@8q)W?<?AzEz?APLeBUe*whcT7
z0d6m3nh)<o(i3amyeb~sss^#azj$;pngj##CGO^^Ow_(p%xs(|4(Z;LdkmP({4GvA
zKl)gW_JAd_z>PoY=i(UH?sZpYni9)1{o-(X_!%DRJ(!0?m}B>i!Mt%}45}Tr!J?MF
zLVkEx^sIO$oc7G5*xowQ-$0wMJh&>Z546JQtTYUJ5($?|z6tGa7x0~#yVy$Ft38sZ
zad4IhxBYfe&-q(;lujDueT(F~GyBk=SPLqbW-CvW7!t_^(5{E2a4=07OAk8ohv<6f
z|I%9WYzm}iC*4j=2IJWsjx_UNIclZszy(`EU`FOA;eHXYmEr(4A8k*m*L2XjaUhP_
zSq|m-JHckWJq|da$c@%VaFc?!w4*vIrfKe`3MVfPbMK6II@M6_mpsU+p3TeGg~O}q
zk_&smUbN4A41InTVO91)dDUM{;ibHap80L(U+tH&mU{ueam?kPd(wooaoVmMI31jQ
zLOAHc2ly3rhew=p!Uttnh5ARIVA6(PVC}RE_h*E_jhtl|T&p1tyKcb-Z{$3%R+&ex
z3}EduD?q#`F&qDe58iOn2@~oMlDWPrws#E`Sb8&WcgmybCkuJ_)dZGHcg_Wkl~nO$
z4$S->&b6;R&~;HMH>eoV{rW-(_!G|y|Eh@X^Qz(0!L=0A^Cj4>$iRXH--MnEqTxrz
zdQ9JXkE%u~f%_6OIyi4Ee|sB8JtudBcxzL9TofjG+V6pUsK6`xq|y{UPch3eg<8j(
z2n!8FDxbKSH1~x<YojIh{rH{Mu5!lcQ%PLh?;!n=*jN|aYH3364%qjwBOlV;&IOHQ
z_(@w4ug6ss@KqmnCz)`%<zdYBH7Aqz$+T$sL5V+efnR4iaX`mHD5<_8s4on}K})xy
zpXxf;P~Q%+b!I_#-5Ydp^8pgxB~wU$U%GY8g}hEXP?EZX`1p{gJh;6Qm+i2_aq6Ws
zs@(_{2JUn{JN78g>8`;(K^@Sv?<<*;;}q=JD;q7wt^~_kDUV>3AX}SkM@D@V#4C4q
zP+iz;EOpVQfc);*A<iE?eyg*p%S`;x>#10@?l|=*OQ4O$YsH&u?YZ+qYYcV2B5od?
zDZJm)on04p=DK4^9MSt2AMAFNh7Q<D$6qvxo3=c{_(EfGq;mwE$r>$KtK6auBM*RS
zydmb;cg80x#_`NhsgoqlEj^q23mdbYFhcr_qz*96UmA!{?pyJs;WgstuSRS+&V<#I
zHi-wCj)@;KU7<~S3kE?xCn>#>?x#;EYlts(+gX5%m-R-M9x3qY!c^*!6)I{EoXn#;
zE8>Tq^Qqsmd!R%b=o()JhoV=bc8><pLth6!d)A2oLKgLTzMc+m^x)<-#cVpZBl~ah
z6JJ~ogl9?Dag@6Vhq5hEHCh#_>a>unA|W(yAZp)G<LA?TndPr}U#Yq{Wn?kSzUJWA
zre4(kUofnhsl@9Ptl-?Ocxsk!qG6{tpm=RH7j#~WyT)6I^?Ek2`pXhHG2;TohfRg~
zJp<VD`Be(9S;UR<EY8V~<UvlB;9DI?bFCgy)lUuVdAyX9LX||lr#?6+YAh!!p2OM(
zWA+Mml|Hvt>HFs#{n9@mxSP3<@%4D>^64*X$)-Z$Ghd<MNSt`(^IBE}bADmiZOEoZ
zM@Si(f?(u<#g9e4TVjYZKX-au(?K@W<`zZkFQY1Z2UauO3BDEuWM*SQ;=pp6G(uuQ
z_n!&sPaeUf<?3*|izf~`cn|VYcS35y8!(jEnZuubp$`f}`MH)rgJRV2-h%yfd04Tm
zafd#?EbB+778Z+Pr6V!iLzxeI`~WkxM(X=RLA>5Io*Lel(2C?ARJ}7({HPcKWh;!O
zH{nF6-z0HUh74zyDfh^K=wzO=ek!(|(Zk6JgE2Md2;MsRSUhp03*kX;o;@*@9QudS
z$IyW|Tx$Q+E;>L1*W1fIT@?A{3dBiM`=Zm_9&p5H04n(*KOcPDHLCkZ!Qjn3`T5CJ
z5-VvSPB~U09yWEtW8)*K)xHAe_OcUh^){unLyF*WbO~$3mWnprN!;AI10JOz!piEc
z&~o81y*_YWd=<0|fBmZxx8!=t=4+?1*P|r<8S?`4rdGhS9*RR=U(sZ(xEh$N@{b&k
zm63DZR(yLugKOLDarl9GJX7}_)c(qWZzY!SB7Os3yxX5@*V{o)ohQ)UfHCg39+|EW
z=D8Brrl*s)(AZ>y$D-A7SIcBp=rNBYMkNU|(oAqwixzfw&P1mdD)j#C2)HA0BR91!
zBa@I8*mE_Lv<G~l`rp=A?9u}<LJp<T5*Nbh5%hWO%fB`W`1NR4E^DvB@#meXU{b2E
zuDdDKoP7;*9$X~Lr^b9<*O*?{HHs%z8pw{#d{J|Lfy8oHV+&J*On6_`0N6At85Zx4
zm)`9fgN#PcqL?x7#cS`3aG=v{%!`~QSD0r(Z?^y(X|Cg5d6R{Mf6aLDT5Vb}p*Kg{
z8~~#}dxYbMMnbre){rR02I6J`lQ#6>%L~8JIoSa*^H`%8P+$pmzc#aybYEI=%z`_w
z56AaUW(w<!Z=Be^az9nJdrNn0^>FI}iDkXi3f!vqKwGO1`7PAuaI2v>F+v{)th*|H
z{80zO<%w)Cgy_@#KU82<2I2Z2#EcWwuvOwk8U8k-x{Ouuw}ZKOW9J3ZHT0r|I;ZJ?
z_iw3BISB+CGj=k#3$M5Tp)UIeO1(indEmBmRw%Zw@r%{tQ&XQ%$V_Qos_M;`4Ls3%
zmk;&v>54kB2cQ=;NUXCPRQYQrU-$IlJteDndFda*U328W6;wGh_o(pI{ud1YZpS{W
z$D?CEZPKwh4ZcUJ$moKA=kBHR*(-I_W8Gx#-)SGu_fDfN69)2u$`;{!?|EGCD4u28
zRr&7L5`L-slLl_u2u~Ze`OchB7<s%ArZ*?aoJ%)wisZqpx3A?TmQzLF?T)x@cr(mb
z>m#v6wqwRyFK)bC3QsNV@WkZnLXZ2tm@?)SxP0G=x2H}9mrjlt);fn0LZv(NS5tnX
znn3eRvS8S{9NDu+3o&PQ1}_|Bz?QQzdD+c5eEa-PnzZU0D6Z0>`72O(*l)L}EA95S
z#Eii4>bB6ie>Tor_n4M$T>^od75H(>6Z(C1A~w}mkdLd>!GDG_?{8Bf*zX5Cee_h8
z+II?GkXZaj`}D?5zdMR?mkh-Rhvtf2Z6);jpeCmXm#OsQS~mMzOl?Q{gN?QW-Mp%R
z)iM{}(jlIUze;bQ^=>?|dLv$Z*a===x`r#xw#S5>9(dAbDaLHyBaRrjTX4`*<7GGE
z$T#*pZC4Jb4U5i-H;1LO?$!ZfR>~_%NLdZ%x_%XhNxyS-*(<oLI2CN4cjpXgj!}&?
z;!>mQkiONCe{6EXkkebC?LjWi3|NbkP6lFkU3Uz;q07q`sYAW>5RT1TgA#(A+^4+6
zohviBy_*l-*glI_-L%0Uzw;q$>~A=}&_LLqI+zQauhW6Fp*&l^O+0MgO+36NiKnS)
zh)Soc`BxQ-X)pWGwjU=^b=GrwFaL!-3P<yqx>$C4z6Z|lE@SpGppf>RabIvCcOSKm
zHcL+T3Ag=t$6JA|3%3c@3d^W&d^&V}R0pTO&xZUF#gJK5A(Um0z`8$nXkIo8Jlb!@
zd`&fW`s{>;`kwMxi?)Ni*<RYXVjImL7tS{du29oDb=DUj!8w^BZK-_*w!43euCreY
zUX9B+bBsFLG;d=aSs`A^9!v7RPIx=U5kBrbPtBjZU{uyyF@J0X*59w9<8$Mw+mlj$
z7kY~R9bU-oXMF)3HziQYnJBE@8-cxkUlFeFH-_WZt}u4(OuqDJBrp6i5ZurGq1Ah(
zz2Ye&VNuN_(yca;dYdHz?g+w1jkV&HfOcr+znkV~M{}{iI%-KfV3#q*f@8=L2&%4Q
z|DGZo7@Et;e=J#NWFw?k%;k$NYoS$4g^54*g0=Eo8n3YtZx7tWb$wN+=14Y;ULC{}
zG@W^GXbvsvxQ8!I{@+dM98FEm1apU3Y~%bKzAo9!NxfBAT;WX~(#>aoU^^jpQW2C?
zc7t>227E_n2ep}2iaj>!u=(L3w0K=7grO=t#&Ir0`JR`Y09xXTP2KrhFMafWc}=)E
z#F|@X1<>jq(*ED{4Uc=V9zR!JrDxBKp+mnNc=!8JnTdL(XjXJbzH`eSx>`AtA6;L8
z)mvnA+0_nv9DG0r`jpdyU2EA^xqyBzuNJQzR1ng)ZIWf^-6YMYOiGSz^etJ&@eRiX
zLsuIvKdvG(Y@H3Qqm{tVI#=$LE$z5n>&Y4GDS1kn*svWj$#*rxotc9{kCkvmUM_2v
zKNNIO;(@AKV&mBpkaYhKL{1jyRo5eI5#WYT=Y+9k+7B2O@{1<yN#<SIYx(08Z(Qbm
z7vdg2l31}7qR+W$!jqbbJfxrlF3mg(BesP=gYO=?5ns&rPQIdl)9;Ie9!~<}ssdPc
zV>X}t@sK<d$FYms3$c8c20pBq&M)#Rv9GQotLl!xDSC-`wb+!^q&rD!aHg;#Q;iHW
z-QZTmF46l#7XL|ZqCZ((+0bjAxZ?RF8h_px#!FnjtE1k)lC7!`J}HP2{iCV-{GF_2
z-+_HwLV#9xh7w&l4CthSIDHt;nk%B(=pVuw{{~dI>q{noR|*G9I`Q|x)|_!nTikhN
zoVaA(GIpujOxhU>I8=KvE8BPEmOG>PY-B2L)2tHHBu-4h*G@Do;0DY(vy3t-$K$My
z<8hUllxdZADK~DdqvuWz{7V>4-Jfd8&iB6$OBD%18xo-V{5KT+Ly;YS7SL@)4KDuv
zULHO!0%xcfQtzjc_-Mptc*>1rbEF=tP5X%%m)B$8CwjPO%v|~8{tiMNF2N3cb>T_#
z54gEZ4J*&h;fRS@$B&hpL#M(PIxN2r#Y2?Xw$>PCfAXRc(N1_%#|d}-y#{^B7H_|i
z;dp%wpl&2o@46<<{{Q9w|I7dXm;e7S|Nme9|G)hIfBFCa^8f$k|NqPX|Cj&&|C0Zg
z`2QU8$BnzxZiGooyW;(pevmKmkJop(PZhC8VZ*2v>a(gH8)j?bx5o!5B6Be6%=ANt
za;dYaDeVK=rePqK@}>(>s3cejm%eAxu2VL+uh(UoW@t-3<OH3SpHpb=a9C`tB%eOA
zllb|;M)-H$3|1W~hyGs_A!ketSU!%YpEW;0TU3<2oAMp{MmP|^nnL>3D!hKCJ|~wa
z(cFY`N}W59v&wYY<;@Q2-_wq|PSL`1&m?|ayDBnK>A(S9YU$Sf19Y8sL(l^!Z0#%c
zioZOCVu?4}Xrqj4R+)f8F9(bn*Nczdy9>|Otz-4C^KrxZj;y*Z93Bkb4YR)W<4Rpi
zSe<njN~OC{LqiiNbexI3W{wjI%t|TgoHHa>c}l*@<*eS#0ae;b*^*}#80F=_eLm>G
zwC8^%|5{gJ?7C}sW3({~o<W%3GLHBE8_AF7hw{p|N;v9>A8FqoLIXA$VXbit9dKz+
znLlTsP0&0TyCk0uXKci>H}fHNSSF2l+*Q1C>nXWE-oOUg@mSt_4b~k4d>rZrC#(O6
zx=HIfb?66jde{Sx^|TVwC#6vK05d#)dnsp4C=^s3A5lif1#ojpAHF1Rf^IvDV9j8C
z?qAwXNQpfuZeM+vt`*2(lr(>rG_Hk>1?%w9?`v??X$&q>i=<8sOJJIBN7%Ymg)6u9
zz&UvesNZ#-*zZzLjP08XURvJtETK27?5xRe)suxQht81I6erxYYAjy4KbG4Kn1yOT
z*08qeF=*Z02#dlTa6?cHHI}vq+i_EA+{JkLc4^mJ{dp2xi#{tX+HJ#5BQ&x4^b2yo
zA3}Ju6Hd5tj-DHQf?gT>rJlufI+xW;_&imKKaASSiB*YkccnGV44;6ro|g)Ck!f(%
zI~B5*z9Y+}<!D(kl?Qiv1jCj)@FOV$I%<85EMQBLFfc?8`|5K9b5AqjPwg7G)=rf@
zHfZAC*nMPZ5hi<DTP7a*G?JffETR)qXS8g<OYr`wjwi>=f;rMoyr}mcp8V566w}m6
z?fgM06YGP!zg(s(`|Nq8o<RMEdQd>=9=VrlJ#=r#Cf^d7(DUJC(3G!3qlzR9TKNtR
z?XbjxaY=MBqaP}zIiTZFW6VE&L_Y1I3w%s!r722#AuFdnAHPR%+$EJ42e!bMX~uB(
z!B+9*LS-~~ypHEI<bl0blX$|WJ-RnZpUJ@atop^6zt(-DvIZ$@JH0<rS!e8X{{+pG
zoE+m#TZBA=1@xmfTjF6}q`XUw^s~;99@t+4s(wM!Q={RR<_1^gq|;<`ryMRhj%Pj1
z3W{I+gLf4PRI}<DXQ+_i7PX#FTmPVQ?P7Sw#-a3ejv__IJ%LHblfZ4@HjpngmiwMb
z6_$M)Dx@Y9kaOO0ROk@Jj;p4lQRXy$WaP>L*S#S2STr9WJWA>f2hx#ww(v$PNGR;B
z#*cp}i%XB~p^xvv=<2lJ&?EFLsK?BJj};oQe!mB+C7cueB;I`g%^KM9r9U~HUdmt7
z?$ZtP9XvNQjL-B@MD>VcJi6;niXOLy)~c`J?|V{EXY3kL`SBmXZy({=<%=+^_$CZ|
zDu-cLL!sySGOElB#l=<~IMu;G%ruz9Hu`&caZw%}kvNXOKTX1}&sD)cUCNjBc}!z%
zyK}dMOk96yG}Sj`!VU)?dHzcmmTL~9>*M^O$ud#?G^RmZr`P~WQ=X8YZ3Pr<@D#T@
zU!a#BPswarGFN;slXk_HoKvcS?B4}Xst>{q(OQ^3|1W*qo-M4~=K)|bSX|if0Rjg`
z@ZJzlsz^_!`olRm$n7{boJ~TvoZc8=XND<z)g@=!MQp#sPS)OYsNm8m0w072@bzao
zC|}tojyhnC-@1BHO#fN%Th<M~Dm?}BKS7Z2Y&rP8Fy#U%tI$z39Ax8)xXa%(SnTtE
z?41ceR{s~S6_Gg=g+eK!Xdp_@UaLqnAQfrQq(MbeB#nwP6e?7Hh7byc3Kh>@OLG)W
znrF?aNTpGoea_oB|99{V`?L4{u65tnRY`eMI%7>!AiUn60x4td1cSO2yp2x@)4o0u
zi?6GpRlNp^`t$LZf-;@odVzL+=p&rH@tuPlEWt_2TOVpUL&LNzu-hnUmt<KDu<f0A
zp^Ly4NjtbHMw)jQoaaT-9^?1B5)f|hmo07?g#SJn@|R|9YJdG%o)l6o$R0cpJIE)p
z#Yicm^mil6k~X5O`vmaqIf{A>bby?qedPB(5^~Q=`R0sPD0v~{4%`KdTW-q7zhL+@
z;3!{wGC-K!JB6DiAH=o;ZE$wy&dQ|N;}DctDLVd(fEm8Y98;A_(Glvbe6N}MnvbL1
z((W+(MLL^|xks(%Bk5HB6RJ3ng!OMTh4t?{;VPT&q_b-uJ(GG*m6x|sako6o|Kf<*
z`!|a}(gmE<>#s28kSV{Znhr;guA_WRpfLGOXfy3jvuZoS+@(64+x$YFd904M1lw@X
zh#t6q_E~C}W{Tyd&iE_iBW(28j6oCTpks6+bnj$~RcjMT*+`i!=WKv}UAl06q5)mS
z1^CCNl=V##q2p~2At20|7yebl*iGF~(>#D2Ml3_U+p|F!(Fc=1_2tE0geQA0=gn3v
zaPvfOFxobdJNO@i&Sm?BXtSfDnUopWZ4$&4Q>WuvJtHx3O#l@<v%v+*$^6&<qgb7o
z1)r?;qO$uCG+VwNmoBP-7N_6jWi=L0Uu*+ylN{=HX1CZ$X(wMy%%q3`A*?&w1XnD$
zCEl8nL)1AAf3MI6wf<4?;aLxyn&k;Iy2nwIbpIOk#RNXRHs|HS{i@!Rr$B~R7g*Hd
zg}SP-=zMM_?|PUBBTkLrHAkP(pI?<?l6Jh1JpDNAnj8x&_Ss`g&L~nFtwW__Z^*i)
z_NJPrL*V&918y8Uix-(@aN9;5UeK>Xw%`09_09Y%oY08lwYMu^um33+b^bWU^xA;`
zI<JHQC+$dge5*J`@~{0D7YA9NU%>YLtNHKNcJag6c5?0CO2da);ejh>;a;;h<sQn!
z&Q_z@J7ozKb(;>;eG17qtOmC{(7;V;P1vivk(9ekhJMNwFd?W5AJANfb%qAG|7HqJ
zo7DqTbPmbv_si&o&TZjTxzsQC<3d#jBrnsQF+|-m@YuhNT<bj=?I-P(!eZO$@@+-b
zcUjAp(GlX|VcmGnxrgve=_|Z7T7bGQ0`P&K6>j^jjanOeqmeus9?sIBY4&eL!_Yyz
zy~P^Tgah=$QHJ{2r{uMj7wMUnB5x?S<%=6rq#T;$P0GGa*3pCU&QpC5Kkub=d-t$H
znhoE}_v2492J`Ai2k`85b@{X|$>{X-psduy1G?UvK`-BT!xxdWz$_>UJXwP#xP;Ng
zGm`%?w3=@J)#dHl-FdyVy&urQS$@+a3j+W7(3H-JG+%cNA5Om|W$w?y;zk#CKQ$ef
zAMcKyOCLhFPpX*k>?rj9k|Qih(vki3Oy%Kkwt{`(Md=!TB6d0a7z~?S`FZzfY%p_$
z1|gQpLKSgWgBK-AJHb00dZFg)1hiNq?V`>n(y?#9seW;Pe%h!+?TdG!&FNk6K|UMX
zy0}p5mwbvhwqT7bijqUEg*+wi68s$@cutiG#=C=rsVe32%%uI)Se#9k<|UAorbEwN
z95C4b8NHeJOmx20iwAV+Bj}CXM>_*`uuqv9&o(xJ&&ewMZjp>UhfaslrXyL>zrj=U
z9z6cEHu;-%!lR$fXwd6wq4Gyg)o8z!JZ*z7E4O&?((o<(b!8W>rYLaRtS!6yTn_z~
zJK>4YXfVm^iN+2BYW$gB`NB;DWA1OFKiaYQ&+~|&@Oda&UA-^tvLDWsf+oMK7!M1M
z-lmvwi4b?S6GSXgVY|9+P}m|du=k#V&1;=;%bQLZK1IewZw`QeXKx-^s0a(~?RakC
zLWr_CB0s2l9%9u8z<j4Uv|}HUm(op=9UetL<{pE|x4Uslg)=mzWRdE(e-yTQ7$&?9
z<YS4UuzYnbUHqLdbZK3U<%<i2GpTDN)=mKl7rU{zPapd~z5pYNeK2HEs;E$0Kx;B1
z$yYa<F0eJ8(@2D$n`4BKMPG%g)<R0d{=y0;ThNhM8x{6)=x}Ks51#gpuJr3Cd1|Y;
z^qVHwA5bEVjh*q?s{@c~8i!+UtY8HtL(De#1?5-b=<}PI82P8JO5@8izTx#)tX`WT
za`QPM&S)ggk2nQhCYMQeRE-1Wn%tviocK`fr&w&bS%~W~O=i2hgzfYe;GT<0{95-I
ze*U);4!4%V%6V%1-}x^>!QgV5HtYnvee|5PD|*xVl>V%0r;Q)aeW$V`x_Bgc5f9n_
zKs@|Im4{zXhjZ&6kouvi9O}D;`!03l0qvf=N@FSRI=YA#&N@%c-Z2z>REf(hHc2_p
zVOZ?<0`v=aQ+1A=@Sls1{Ao9B^jO;!Zx+PC7=KssSfZD#bfO#NG$+f~x=)~kKGD?b
z=fE%Drcr}|4t32sCsTPBCj9IWD{P!r15Z|~@rz|a_|iQ}O#iHe-A0~+rA2O3;k$vo
z3)j=YCo}mHH$kki9qn!1hz8Cge@%V|R{eLgdXbu-_+l_vZB!KwPFy2~PE%(`gDH}$
zMM;8Iq`{2vQrw}`nG$t};k?sQeyg$(CiTz9!FGxm{aq3Ll>{*Py&X!%@4~*H`eOgF
zi||ldu~2s-8>TNzBF#G*m{OvICbny-s5lz>kL!haZ6bS1=Mams6}0$s5#PE#9e-K%
zqlx)|ZzD$H>b9{E*}XzOslz|<U)%}7ymT@p^u9@l?>W-b56&1^zk+wT)S~X3lRW>6
z7Y)8{L>)q#$o^uLaLlDAFQ538T6cu=<zk7m`>0yj;Mzyp@Ab!w2PbK-QcsL*cn_b>
z^^*DZc_O+LZy{@saje*AF1+cUOC<w03PHUGNqx=<IAOYU?;8C9T%b}o^FCM@w2`PP
zDH5Zfe-vu0HPF_+4@dPf!h{}+vB)D4kJ~AXetPUPjOjW+G|th+F={=rMaO~rS1rVG
z3lq4h^I)F8ClY6c=!><xy5Ol)HC#DPkGEt@!;y+=+&=pPc<%W}_M3Zgt8o)t&<TcH
zPYA*_il|=ON;I+)DScKem=zsG9CKebeqk`3e|tn6<{Hm+3C$1~+z8IE&cKc1H{h*l
zPukh6jb)Q-NoQ^l4vs$nhl_gQp~}^;^R%isRE*-hEfaZ0eHPai9H!-2L1KpzqQ0HR
zVdRTd{I+Z&t<Q@hug4vEug+{RSfYbJ=f8sX+dXJWX0$vkZ6N=vkLE5(kHOhcCLyGL
zK=%AXvdVOj`bay488H=js_P->^>I6tbu{4NJ2!BK`C+P!x8aV1cEEtzItro#@`|n}
z#g>tq;jaETIQ2D)EDD~`?n4h~{OR}fq^1&<^!`trGVcxDf4PFz-a1P`UVULsybhV3
z`Y70hX7Uut<>}p|2YZeu;66^kO$E;4IU5!$w~m+2KKbAc`uNs)0HoeJBl?(E()VRD
zAul8wvWlJwZ7YgpK5qLRO7DJwNmG3V-|)R~I3$wOyms-{<puO=zzQ0ac3pfsxRTD_
zj+Z>&YP|OK0=#}G7cJbf@wU}%@z$^L)bFs=$NKylwq>j1ikCmd&+Fz9XU*oHE^3^A
zBbkmLjR&n-$!jHLz_tYBP-OS*`0Zg0JuR}q*j`&{hUf>e)9!%o<F2^H(-5P~?!%Mo
zYoX&+H(Gn`BHfI9MYFYSXl$$k{^WTy@%>CpyP?evI{;7g`w7KSlAC1A9|20%!Z5p)
zlH<JxzqWiukN+;9-~v;A>{Uav@72<~S!<!xU?;?vT7%=%O_V%Wkt@%%NEv`(+<T2X
zHhUZM+G8SM*Dct%bh==(zNfHvY?ILDm(B-YnsJe5F1}Ve%9E9Y#qfz$Lh9vQuDdx5
zzt7D^x#d!BoO4!Y+9{B;6Pm@<iaGK(%}=HLzAreemb?$gzCfXFXKESw4`w|#Lv`J$
z_@qyo<cL_0_ZLp2`xEuB;c*WRP8v=_OHY#G{{L9#1@MK#QZ{4BeBQX>0KELYlFnQl
zDsETTr+-RGFmYE71&^}8jk}Eb;M-){QCf<tZY1*DROQk4E04<c+(OuPO%>*>(1V<G
z0>{Zd7?y0woiBH&j-R<0th~>V(s*^$%NODEg5B^{V!&CscY<!3N2Pva9=v;+jGrHP
z37+Fl!qaEzJo!ijPHXby3!6{SpWC*e>KV-9lB@Ll-!lj(_U6Av69371v~>3PNSp7>
z0hbf*yi|H7F0T3y+tYoh)F1^~CBg2K%(J|_b{s`IJ|bOH7T49>tLpdG3Cg2ill8&Z
zR5w0{wx74-sb$&X&FShqH*%EVets-x2zg*ryF~c)UE-Z&-h?-%6L>&*cOL(00e9*-
zi*u{BaQGS<LHqAsq5a!vjP&a%oezCU>t7)ri{H*Jx``lL|AvlejKSDLC+WL>5C)CB
zD)=uN1-Ir$VRy56bm@i>)^F&_{r*_P8{eV0@%VIM)TFLhWh+qh1x=o5H&EWyP#ycV
zL{M|g7pV9!o(xW0fK}~pz+>?Q3{i8V-q|1NY=JdD--g1=c_##q5dwndA(}G2MK)w$
z5v$a6ha0C-dCBX0)VJR`GE&G9%tlQ^(}{=h-KHQ^*=;P&cgrG&-dD-qJeRIbu|bzh
z+8FV%7n&c?!GKvdIDBoBtp1tw&h;*jCYO4O`QOKbS299U>0liERvY}3R|+nfec<xj
zaiqM)1=}ZR;%B{U5LIoD1!}wK(eXgpy2xx{-Oneo=^lyTD7pKL*M^CkIxa)|+C{Wx
z-%@N(iJ(_$J+Ra{R5U4lCF+_#fS+ccg;?bX9HevrhN#ZKX2+SL-KNjt#*#@S?%Pk8
zA`#9cCrqVFDyRHu7GIxvNBNaa^5^CIY5cQ+SoJWU^!G;M!i_7%dHp6s)sqNtTK$YN
zrL$o|Jz(yOcJYu!433%~MTgaDrM%J<;mJ!&C|2sqwug#w*hN)dHK0*+lei35w|*x#
zDX(VtVl28XTP7S+ZJ;sNJaL_2U#9a;oUCJodTt7QvQ>wdZ|V=;1KjBL1UK%rdokWV
z^`c6C{|K*av%&|%qdD>CD_PrvzfgTEMf_QSJf-hWzM|_xXTw(Ul4u89`e!cgdU#M+
zw0t-Yzuya<yIG;xMJd1keh<c7w_rbM{#<!aa%P9xK}M>D;9jE%jd|AGJ}!}#e^7<q
zUF*cCSKBdm-)lOrA0|5a^rgr#Be=)HB3RmE1P<6{%=?N4U|aeqF0&oaC-=pOCN7~o
z=V~QAK5>Q4eLO*zL)zrN{ii|s;#L~J&zn->!ueDF6b@02A@$D#AzdXD2iHg8*d9x8
z-QPAThcl4BJnRKy=P%*1WKZhqZYHxFA0?*w52Lir>+z?uwqW?G2^J}@6*jIKO`qIt
z**D1z?p9mV$$6hBHGBl-x}K*^Hy!cg6&w6CVlwSNtb_+GGC5bCDRF&NIL&z<{OTPC
z*CKj~CyXBo`vWII*31#Y4b8o>KId-BZe=cJ^Wsr-ZEHNsRo_sCen0+sX(3$SdWObr
z?uI_b>U1gnD4qPJ2|n(cVspU*G1&UOVAg*z#e_<}xDcPJ70=RXym?phToTFC0=G$7
zb>g{8J8@R?VHi=DL<g7e$CdXtVXw=ska_43uD)4JUt`wcbYnHVXJCqQjjzxn$ov2K
z&Bx>>OMchk@LRSHjdCrq%TjgR-sVHy6`G(+MG7o?*i9UCrVLgOQKnO8lc?(YC2@Ib
zw&>)!nY-NCFWeQI#Uq3K;JH=>_&dLadmkpVO}i<Mi726OF$$6=VvVRbR~IUh0cKrI
zgrz>qaI4-QfyaeW`}}(OAq!jPo4bYH*@oh?h#fHfwUpQX8jMe0$Z*ZDP<T6Ko8bF6
zlUmXjfjG1Sep``)WpDI^Q?k8q+%_9r|JzHm50^t>++avnKE<6UAEBQ!uTX!jS-isC
z1kbHEV7z3`x640Md`@S|&&?vu3dy4;^^@jlo#k_9%%ShLsqE_AiEk|dnQ_b^RGl$E
zSmjZTvdcN5N=+2Lh;@P$vGv$}y)N!qe1kUR_W=9A9z1!lhUEHOk29kmQAp;0V&^Vb
zXzJw@aoub~?AW<gOi$P-q#PSSIY$<7oF(I!htH_m<vtu4)J`60`rO}Vh<G}B5?*+!
zjY;=|aAE&_v}bCuY@}BL`8?I&#smu}ol_w%@$JbzZZm0^<j?ByDTdz8NZ{jc#b_~H
zlN)R9je0!8Pi(uY#jU3n(!rV&*dbNoa6GZ)o6`m$eA&;pO`B=NfPDNAaFSo19*bh8
zE&uxR0-kM8g{M7}g<V?R;IdOXhp5+rnV`+nm6xHc<r1V7S-|a37YI14M>obBbNtvF
zRFip-mIe)xa(Cw-|J@k8F!YHyp<e(RG!%pS7YEuI<V0KH5e3(0@{ZyD9Cit?YOgXV
zbgn_8O_BIDFdO2#-j|WKIvD8w6{=@9!hussR9&QuTOaw*vv6}^?x8q*lco<*D|?H<
zCDq`wIF(+Uai_)~5wv+_6{xC~vFhI;*tz2!kae00@1|(u)^8G<rzVm|%<ID!6Go!)
zjjtl4t-yFA8Tb`E74~a%;tcBrsL;}t9lS-DY1@e{9<G;*mQ7Xm3hTs~#jmJsMLLZ<
z63H1pQvTUX8_m8p(8dm3@xNE*{3hQNcKigGA8v_jm*0Uy$t|?LIRNgh-XVTiJD6%O
zbqD36>a6e}Px=g3XoKcw`7gt%G<)AlEV<T+U6T(>uGZcx42*$Ef$hSzZog&Pfqh_5
zKroV%NY*aLaOS}4bR=L9njF#RzvFW;Z@Clyys!^9sk-s~$r59rS?b%)NBH$B6wIb=
z64$Q*c8ZumWv6$*z|=}1<mC-C57;8#S#N-Tb6=9>+1W5{M}XMF`4wpo`vze;F&ti<
zM|)l`q8ckJK2vK#1$HT7Mu+F{rDdMjDo8U*Q%!N}OiNMiL7eFCKLFhfzf)?&2U(uU
zFb<ez%<lqv@Q~zO;ji3)!YluyEuQCqJ6gcn*IpPG{|_3LM}wiWn@}R{(!x^)j+$Gh
z!Gm}1<SlP)Fmz-q8J%2)$22stCATk>)$OL4<Nu>inxW{cponFayYNBq4DfV!V~4zr
z+;gE87k0JdO?Ts{zv>zKfgY6PRV+MxJ^`*A90hUq-D!-o5?82x1g#TJFk^5p^dc*A
zQjDawfql7IZLi>RARlvAUXd&Q>5CULQn|kSdrGU?!WKRKv8iMt<a}5QzrL*km(MX`
z_B%(2?zu|Z#f9RA-F?vaUot%SGzpg!tYmAe0?4cB!cOvu^wZ>%u&Iv{=GWEAHix)V
zRKz7pv9uGs;=6Ft-z<Dxe}!(&ekqK+>mmdMTCw|rblJtb3u(lwUc5fK2>SIL39fzh
zU{b<dTs19$e@{9<gU%!ix>g<eNcd@V`Ef_qZ;S?fob(t5ci9I1s|xUW<4s{teLRLL
zoMFFg8*KNpX7jCM(QjP@EQn|UpPxJF@%>$JMl)4>Bk_4!&1&KD8ZWl>e+88hM!e<0
zYiMlB<<vGU++pR)7uq|3$M2~^L#rkUySuSn!)~ab=fw3U8DgGB70L4k2*O`mF*Pqz
zJfO2c+VeeO8;Mq7?=_j<IaZ4k1LD!_sT*%g(u0TZG(c_iX>w3f!c+aHK+2dHURvcx
zVW)TUI!_s24la}(@IFkvcT|YeEAEMg1M`_Q47iv55$+uNNt|-19=ce!iiiAf%RWq<
z!+uV?`A1%>@FVMyuu;tj3?yfFv9Su76?l=C%?ecZuAvs+7IFKR#ZpdOkK^|(z<N(d
zZ0IWk)6Qv7GkXhcOP);Um7K8ucq^Dt9wPsCxdayKTEXxcdxSTkx@hRXmcsXSzyb0;
zka{|vF3fXe)ttASu9eFL$vya^S2fh_mmKehp9=x{O|<d4J<khxLB~(Xu%%U*R%U#{
z+X-p1g-X#F^C4B{`>0&<*bd=0d1u8hms{b~^D9u_@gORWc}}nL)G;Z~mtT8sgPqk*
z9A==#-dFMkLtRUX*S`lvlm5WZQ&ptnJ0C6{oG4orlY`HW)JwbsEvzd1Mt;}Uvu5&H
zTAP?4{!7hgyMxj^S{Ogd?QCC8-j#s&OZG$7&&fPMmI-<p!@2X9qq6$1zXW%?X)tiK
zwfG{Z1zxVIlo(9|VA^&QdQ#{pKOb32JskGH?JR9<y4#sMd!Lr}P3fZHivmphI~4bv
zkA#yUnH*)Nh;Lhn?)uiz)LpO9bkY~z>0UxLITh3re}#LxRf?b9nX$s`ncUMRhx9~E
z)*mz#<NjO0?Us3Xxxfi#+uV@+$_;SmZ5B;;e@UV1N3;3YB)q7-nwqluU}lLE$!)u{
z?|UnbDGS6<6IFatz687IFT&@_k&^4U3%~H22u?Bm>4T*?92#~8^}7V2!-r~w$5X_z
zjV*%2nd6CK3%!y__r79xTu<#V-Q*VD%Ur`L?Zc3idehGzU*MhR2^9D2K*9ScdRBD6
z4~Z?X%i@ceU;MCAN5_y%uG|r0G6c3i9Y9oU!|!&Sqliz{5RjJxi%&m>SPvfzRZ->#
z3NI<9a~#_ZHsOj}9z3AFBc8pgCbm133UdN~3wHI2(9<|cs2N}bRjHcjz0!q`-hN6)
z$3BGTCx5a+=l%FJDxAN)m?QnTzlFqezs0RFxnvV&1bUxbs^(d`bE&k;xu2BGic>#|
z)~!$IT8J|D{iTGDSEzE+lx{RGdmjy%xr(p26+y$@yX4<~3Y>TlYo3;o^032{oP1Dt
zvpo}1vs<B%%sFq|d+{h$k=@b*l;PgFGP6xn?6I#imlfQgGcg4a8dOVm$Hxjq=em$n
zb_M+|HIvc>3vpA%MC{l@16<v-@b@Y^9$IpMa`UZlomI2Mzu1c@>cOy1Ynb4+a-ozf
zQDKGK&J6oY<>&ULvO^yOzM*2oA)7{VZ2le$P>I6%7dxWE(8&-~c%EKe+Qjkm|H#Tu
z#GzIHM}n$$KAMf&L<v=n+@=uCSx=_0-+@ZfksO})6c^)c14B69d{dbAWDD=<I|{z`
zy9>56x}e-D8b`kkp`8Dolb}`!7NyhS#sp<t?R5mKd*@f$HLj(Iqa#6CDG2Qg6j?te
z4a^-K_)gR*@){Y&8&}N7PfrUu^YlMh@i0Q-7ix24-Dl{cxScXLOs0R8E?9Oc2{Hyw
zL;vox*lDv0DZUQpvfM1lyD<-69UF{u^B=>U#6)_RVaunUOyJFa3RL(qgTI@&<HRdD
zoTQqEW@R0C>`xKr{iiKvG%DcjZiV6t$3%W~eK}7E?I`)LBYD#I58yK`4b)Rr<bhG&
zz~WL2yx;p(P}12;cg}3aLAg%sKWHcR^3=kX#b@bR^ey<hEu8~>THyIOGro{^5$6Ql
z6Q&lY;gr}!xbw{(OJXd9Y*TYezf?#42Q||67emNfB4)_1Cgb$Ss;C~c5@pi!H{{W9
z-l4D%7ZqQJ{?0C>cQPBr@L%wL%Uu4$VemMxgf3KshzFnU#K2~0Uw=r2Z|fzocSbcC
zEKxz%Zh^R}$8PL<Hj}PuDsso(vslmOIV5Fvh4W~{$IBMM%Y|S0ceQ(!={RL*n36_a
zwp=BbYX!XRh8NqZI`P@?%^(g&T=6`P*Oe$k?X|`HAU%83pCP*V>1s!`Tz?LJOW%8x
z@B#c*Po=;P>KHL&leA;l317eU<Me4q<S@q(uH-vGn87XbJXeEXS0>RaqjJPca%k8#
zNZ$2aFduf=MuWQU7Tp#4@GhG{nEBs)e)Dt;m%r-6(M_#nr?81<_p{>JOVqix#GYS2
z9*f(RJg_ui13vi>4XdTzokv5p<l?>weL`}jPO3UK3z^jQOaiFiE8&*#a^djQQeK`|
z38U<%@%!KxVrfE&5aANXNp|M=Dtrig1e~Tivt|Ld3}Ao#7zj!~4&@_;<8Pf2JgWUW
zB#g>oPst(rpOm2(xTPOla`zN9YdfLirTy~4Q%k8^FGXR($)4PB%ZHteN3gqeeLWti
z%0a>3;M93tdUH+_7cC2+F5+}f{oW?%1Q>G6wA&E(DG`&-XA<oBOW7;yNbP+bPnuZ{
zL$?p2gaa-xL8*Zi+H0t=%SJIpydri8&c{z156ka2CUN$p=hW@^a^6&FB^o+qk?lJ(
zq2PWD>-|cj<*R1%Wu0uOvrl4$hP6;|#FIPpIZq*Luh3-Q5ZSk+6QYA%D*ZPr8jU+`
z<Mv;OYqIOetc&EVczg^J3(vrao)$Rj+eTUR)HT>4{SbAGRT192b>k`Fds)mo2})o0
z%KOfY#x7H`aYFMh&ixqy>;GEwtbN~v#w~;%=M(Wxks?m*@e&IC7E<HFzx4CUC_Eox
zO^1UlF?n7i92;W~?so<8tXvy2e-?0SgdyHrBICpH=`y{$x>!*z^=TAisCQEl1ow&J
zk*lUrfx#@=TQPwLCZvGE>TdLJ>nO14CZN123N97w8CjF73&T>w$m*gFEB(9y`e|C+
z=eKl@lV(Gai=8>gy1T4$v?*?h8^AAnhC@a4Ga<<SC{M>(*t2mTZ!Mc6Wo@>>-@W%J
zIL-*0=UkCpQt2Re54C|?7mfJBCU**T?#oxlB*48jB~W&D06J(5!Ml5Iz-W^jkoxC4
zte2h{P4^R^`R0BK%~~d8<f$+?ZH1s;eW1bIifU7(+1?aWYPf1EK5h@hrQIZtZtEcw
zHa!tul`h0L3U;6nWeP(gj?(*+WppIt4x~LXhqjKVDf`iA8q?;=+K)md&ipuZo2@5A
z-cjLM!EPW}D)Y^EbIGu1E*7Q_$3Nxqyl>hEXc^&7K3l!vLP?FZABn-xzXxbiA3ywC
zd5PK|siQQRaQL8EOy#?BNz5D1MLmb2_SF7p7wUrxJ|)1{-dT9kNS*tyV!Sc6D~?o|
zLE0a5Fyg%z&zPl5J4#+s$|g_JEO<(7?~X&!Vteqh^2MHYF0kf`1C5^YhNgU&OW&<f
zpjVslSk_NSb?S!S{PTpH=BEVn@LbuP41Fw&swUUu&(KRPfWI_&aLJF}e0FmqmG}4~
zHl28aeI=gP@7%=__wO2{ckjjgdM+8yNTlam<E1{U)UETqCq5eZM;N@U9op{9L64(7
zP~p!|ez$oSo0qPT)s;M<aUYdQ{gx9355LC+Jxu8E`%avAtP<o7dxV%V%D70?hP0Fw
z$u7nav|?1zew(T=K$r{jF9cKXzS<Q2F@txv#lbzxDLm};BJpQ@tuWoD3%C#2$5*W{
z02FuRS(g>)!&qBf^~0DxmD#b9Nq-@$c|D(z)aV!PIP!JVu}tGL`L9g@O|)#HG4~z{
zLmWH9gz*N@=go5|-MNmuABORp)<j;+7QDzsVs<MziD%;rL5Fr%wJqtzHXC+`6TO_-
zRedC7ODx%=7ZYjmURUgGwjb6WNfTebQ$$<KY}yqcNt+_Zk-K7dDs-DCF88y*1520U
z&qPN$Ar|7tFliTl&JzO<tbj%{1+1AH24-rSJhkwk*rL{p?Tv$lAlK87=8xP-r%^QS
zFp&+57eVKyjd0>q6<k_=0Vevya!blV>UUrX#}2q6_O01Zmo{v`;10{hWSd{0vh57)
zDVa$Tze2!2$q2q23BzkE9?EVQu0hqehB$ZdRndC4Dh93cg>BEzP}Wc-vRd~W&WyZB
z`w#vU9117nBK<LZDB2SDI_N{>mTd9f*l2l~#aL4Dze3ypG)rgL@#0^rITEKf4nH3r
zEQa@ei8D8M<GRpTntF!GyX+pt=WVB#T@~>DrbxC;TfrUU)9Ju0BS6O<;`-&guvYUr
zYx*XLQ@%Bk-dA;8|7#&0YS|^csTfHgKN@jup)-a*{SU@1RK$NL){)u0Z`}WKBduIL
zi2oSuA(u-FaNdy(T&4Gx>Ml&9?YFk@#7l>v*JCDmlQerTF{koEXHkD~6s+FNI5Ei$
zBey<*?qBi+@0|X8yyFJ)+EXr0R>+W<%H5b-jCqNBFS=RK1Y61oM`H~t6#5FS?asL9
z^D@ppX^3M#By;)xZ7j6Z^JUKuJaiq4S=cCyRQd*A3vDPr%oV5h9x6Fp&f;vJFl_7E
z7300e@`H6dP`2bed0h@hja)6(ZAfC3ykG3OHjbLBJjih4T(mBmBMeU*0w0IX;CJu$
z(Lr$-p4YeFkR@}lQS1aWqg~l|^$aXov6xp*pCZrSmk9xHfKAVRmj!nBVqf_)nw-;<
zzn1jGb}uKId~Ysw{#_04O#(P!=Q`R;{xIR`b#~o*lpc=h#D~Tz(BQlJ7{6*6#^-nl
zCU4!un5za@YLiSIZzyA*8y$Gl%@T1=_w{_}p#YB8I^z0dxhz{XAKyoq;<sgz|2(1=
z#`<oeOI^3{guHoFu~Hp>5ABCj>R;lwcgk2dR2y%`YO+b=RO~j{jO~+>@mlvmqDTB;
z*tUKX1jZJEhEodoIZ3-cm7AcQKa4jVnm|8)Szz;u6+EwbD;_)BjehBvNq*!KD5{O)
zF-02epFWa0EeT+2!+VfDFhY!`NbC|a68$>`;~4v4JYh<c__g;herh#F$k+6yFGE+M
z&5|O)x#tJ?8(YOAdN&F~v%k^x)C}0>`UTdX*e(Xw9HI>wqL4S!h0Oe9<THE-j(dNR
z-bSp)bsC8{^3gYlbh5(jg&WDzZMt-Z7{d{&M*Q3C5)F-6%%>A3;2j4);hSv~b$_=M
zE&kTCn(+=!*t-pSeD>qnDSDV%yIox8ZNx_cBl)Xiq~x4<2XA7h!iC}@)NtvDC%v`E
zdnDs=>nmij>Y_M((oAyJ+(qBN)e9Ig0iSJYf!iH?B?i6*ZLQN_zjqyZ-{KPbF7YI<
z4gAkR-JQtApbR{}-Xbw^C*MbjxxJ+e{~f%L)V*`?MQekYH@!l9lNc>%IF^vBu8K%Y
zBQYq<kWam+7gc`@=Kn&C*}Z!KZH_O2m#xmCVUpyoasDMPKk<i-rr65X)hB?(*lyBZ
zr4Xtk_6kL&nG|Dmk_yjHWVHuIXtriL4at+ZWP>bGS#>5U6qQo;D_@RVk-%fZ`^r#O
z0DpG5VP=I7e(V)RgPM=h8o37#RhrK3kN&{(Z|BHdJd7*$sL=5{pTsYXmuSZKhkW-;
zIhlG&yS&w}sOOL67#R`5%Uvh&|MLI;%m4o`|Np=I|Nrv;|I7dXFaQ6){Qv**|NqPX
z|1bal|4;t^|N6gr*L4%m{H~+zQs!cCU4J}qaRT<~Zi2g$viR7+cuK9VfwitW^xiZY
zXU(ldt)g#2`@%%*6y%J*RmSkct%Ko>QnI+?)nV@a^_{r&qjV-YT}>TuDn9%ajl(s}
zq)cd(keDfTNk2%l22CCAbbmX1Z`b8_Q&aerr8$~NoLT#yk3enwPP!n*QU0=}Jioso
z#fKvf`{lqDosFR>ED0*Ki5d;Y;SPtla3Fd+E1G{6)YE!F)_YZ4_~a$V-`Pd7s~<sk
z@hCp7qK+BbuJ~FOgPq@5LdqRgve#AOwy-Gr=x@&b?i%4fr8c@5WQ+~3cSw%M9H{s&
zUcMQs#d-PpT&HrHW|wHf(=YmvW1S)YqN>O%->qfi@_MEaTP|ANfe(c~5G^AzAl4>d
z{w>Y{Zl9O<-a~s};C?Igf0ZIjm7E6s;^%SWEjbO2pUGEonDEQVo|1ma(D~_g2>YD{
zA8rnXs$Y|MU+?ci#{x_K@ZvZv={N|V-I>TCpR-}*^^J7)hb3i18S)whdmLh4Aq?Mh
z0OJ>L#VdUze(Yl#PF*YGCtD7Zuhv*<)-1#B&5Q9wv<3EwPZeg)>Is=wQUp_JW^+@2
zKlEG*9Jne{=(MU-=9v-?f<h0x(JJlM`h0+f;yUqZ+$9K+dqDfS8*oJXJT-y}A78W#
zC&$P0?O;dr`?o;2B<*j)M@!7_VW#*M!gzfH)8_d{NX7XjPkAQ&S9AS1-+C1Uew)N=
zhO7|#zbd2Nvu0CFrWP*z`jVQZ9CGXt2kv5?%8hrIWAMcWu6dn=A={4fkw*oz{mm+P
zTsH=c$}-U8vXplt1>D#x1wJ~|QdM<l)IZmQE7x!4>n_>U_xyA=_~C+MXD+JzqBS3v
zE$$65%>i7r=_;fJ#&hEZPnO3nfyYJ0Y+$E`bDCUXSxR4Ao_B?;zZ=8vmjeO%D{&1z
z6fcBtC8LeHq#{j4wh!8mjp;=&ac*xEUv9u6jdS7x%Tf{aCv)HzJ^rv!k9Vqc<KKVF
z@vPAc>Xf$wIy|z+9bu7Nog5|%>-|j3-#C*)U-ab#NBhDj<syj*C%H{DK7m_SDTjZI
zgqL<QF<R3f76n{`Gxv^DkzYrAf3#9iKlK3au3QFdhny9|=ezOy>W-k*>Ag5<)eRc!
z+e-H%Jy>gK9$)L(X;l3C|K#0=^(3R;QfB?)Lo$vW!n-0Q;MkE$;a0|L(j5C8G~!jH
zynPb-zZ*#pAI-z0zZG;uT`qPEJ5NKNs?pdd8RE~8_vyx!(cHh>m=s&2|0U}&r1nS>
zN9vxE?fjAgU;DfwS$zqaR+w|HV=K*@)Dr_^ACb$k3jFbM9QB-F&6C2Vx$*NDe%Y-r
zOQTO=LRwe;H*^4BUA2}!UyK#oQag($_YT8vfuSfwCD3l;FXFlVo}$&Jv!tW7N__Z8
z4V?RlbS<hB#>^Oo;b-^5%bBs5k=30GoTco%jVj<MbvzNf1AX*m@*Fo`4&EYN-@0nm
z!JSUXQ!P)^=VnXlXmSL9XcP;2;%D%^>qVD(xr=SzRQb|hOI#J}!*(SnX|?lKQmifz
zzn(n~(@d7)hMf=T*54(pJzp7?Wn@zBgF7(YuM9U{oW}ZhFVU(|Jt+T91<$Qf5#`4o
z(c8#zyhQ5-c-)dY-S)Y{a;ZOLAvvQ<Vh8ev;eCYnC7QVP!V0K&lHsDsO=7?N-SD<9
zPQKu6rOf)}1&L9eO>v7#AS7ZuLQAG#eP$_+>?X1ME%o{St~4P-)CTvFHw4cEIr!`O
zB5r9<7nEm)NleFgbY$vv@x!J*FhTM>WN(ipbHjbuxFLq`uKf$ECT(L`>N|07LRa2e
z{9TsS-;f{QPlnv*2HZ6GIh-}VC+zIL0?)-OiHW&6IQi)_y0PgDtk2&LUmJ!<GrKZT
zd0d=eFu#<Ilr!kH@oLW9?8nLS_mCv@2DEQVoCfn{V2fGg<?ceZbMgcQH9h`l8U|ms
zeUizX`>?&O83Zr$6^2+$M!$Kf_-wB&)_HEnl~M=)Zu4x6o2?Cc@}YS2P&c$SS%_)%
zvsv?29B!!|#-hRo2=nPg&ORT+14c8%<wkxWft%4=PfOZyY{$(ZTXE;^7(8`iJyx7m
z#O0HWP-i#OdXEKy)}#Bhuw^N&2ycYHsfFO5V=8R>6e<{&`SXYoow(Z^PdHb+45|k?
zlfqYTJUg|KXFoDP-L|8Y@4oJomiiYV&-5HUsFu-@!r4@+oq&fgl~LHXWsn*=8SW1Q
z-Z^AD{;jE}rC!F=d7&exUzC`hC0k)!+gVtXwhdQJ@WHjcdO`R*H*_udEXF%N5*NMc
z0Jog>^M^SM-s^N_I|OUtb+{7RCjX*-ifQ!sd@U`!s}GOTp3<x?jWX>UH)+Y8K-rIz
zajeb$Xff^)`>cos$T>pwhtAN+)2G0}bs1?%OxhyzUG%2-6NKrWpoUg=tZiM$mo%n`
z4V6debI&DwbK*bPTC|K!J9nW8U5t6~r(!Ap6Hl8reuE*dz46y)O+J&?3>*4Am0ZM%
zJj>%0-aGzI{<<Lohs9XqPW$^p=k*JrGS!xg%dS$2t{gTl)2355w_(t^Hn9B@15vg=
zgi&!Lu<T(+GVXi{#*do;r8P@=?l%qbk?A#>a;*;~jsGB|_txSAw`?fvRTB63wjC-C
zWswkd9=2sQ3BPp@i&2eNDC~nVzBf7oD`j19IQ2ta?=!5w#gJz|p2Us=tXVO-7TtHs
za9}_JeBUq<-TPYc!W$8M>VSZ}Du)~Ps?prckMeTc!yGo)QuNJzCF1>^V%;W7tVxwz
z8E(dSvExpdaeOL{sr*Mx%H8qzN(XVQ^A2&uY$-RgI*0c4osZoWpNoF)B|e9jH~H-h
z5Y}pHab4{dcv$60uZJeX)cw+~w^5NtT<MNM+m!fpzwUUygTz9fA)`E1xzyb|O@E_9
zxY0fnLRWUej9!{hzg-D_HO+#&?BQILvK^XJ3h3EUU9P%XOZi2jeDu2iXtJIw_TRgU
zs;^xU4sYqhX$=o4*4zQFPg}`L-pxmolV{lO;d1s|Qp|myy7F?9IdrP%7%hyFGENB{
zczQ~#@UELF{VD%P`R8iH9l3=NF3po?<|ab2S_uBxl!HedT!af@BDP9g>hi6@+||OD
z=Nb>CbEgydrTSTXB)!))e;iFCgq^IJc9C}1`q7a||77WAIyn4{<g#tkr|@wPz%)RB
zi&3ZX`PCZeb@&4vD)6M<BY^WPQlZ|(Lp*2K6T3_Ke$(tGvEj20Z%psOiDzyI^A$#M
zmce!Sd2b0%^OysU$-^b)N(3$VMe^leJcYOY@&%;}uf)0@F%aW-iJq>E#0w`?aG6Jf
z;PWvDWbRJ9>_Un-PkAnOwzv+9H-+P<dGXYq?nFCQFD8q;zc6QABUBhmJ)I7sOgGz%
z^YrgSv{xk9NL_lJo>sV9))5E%ONC*2Yv7J34gV$Ir6P6VQyq5;M>My=*E>gHm*sPK
z-P%NZ6%)h<KiBYoF5N&;>azA#>@PkCHB>SgO?@U7L&}7MRbqh=t<N&X(O&6rV_PwH
zo9iKV{NM<WXA8`O6gcx&C2fem3un%(0jDb+vFqI1aI|hbMi$=?107t^chDCxQE`%N
z^3u!DuW%xzq&i~l!h3Wu{u!>;-T~(4QgHL>3Ap%k16h<mkTp6afEEc*>(vRL1s@hi
z4=`j8$4JJ2$&5)!9K76uAHSNzhmPBb=lUIirxTjt?OrW(jMF8%Udc2;>crnW;Y%kX
z2XpxHzI;G7kXv4SrEVKvi)Ws;36E>M!9uy7dfuuN^*-IE*kd!H_3&)&pkReH1~Ykh
zV7XWjtA~|6Z8-7f9_agIIbWLKPYsUqd8v*D)*cH0*Su*sM(TuzFS5YzfsXtvbOy9}
z?8RiKG*C5(!C+g4A4TJ!x0)3#-LVr#^}R2+56To@H&qGm#|Cq!Ty?H29|-TCc=Mc0
zb!fP%#N%zkY0PT{^qFPL8RO!>r=}Z^TRDuB>x0GN14o1Mn^0al(~z6*{-YroRy?`o
zxKLF+K>RR0mM5F<g2XT)_2w+x)?=1<e*Y-+TVc&}_`YCczXQteSAg%{ht!bzk4~-6
z<OQ|!AU?Y{C+f$b&%a#Gxj#q#q|}im953yfSOsrCYO%O`u=w9)XVyEbA^dE&6+8VY
zCgqJe^!H{1eQ$B6p1oDkdWMJSb+TR9dU7`<>|RJV6TZT{g(Jypo(Z~Tb|hKHyV7^d
z!M8JvQ2q5o9F<{$UR#VXe5C<uPCQ432Qo!#R}Xd#cqCuZ{vL)YKLHHP!UeXibZhAr
z+VQZD)HiYA%||S8+}(PxejtZJDTi2iA|4&&N*rsk2S3Vd#A>^IS$Sd`_&>;m-2vS(
zd)W<P*O~|f|4^PCWPvf)PDnniI(U{HiecInFk{F-7-M{ZJ~q4(Je=KN{m;JW-5vx(
zEAGgyx8D?6pGHDq%wyF5v=@G_3c>fm$7sp>v6AQ19Df)`(9m8J;9kLSS`;#vr^kC!
z|HZp-Wnn4x^Hs<9HkPa)Y~gbA_vHVjJ1*~I%g)(H(C_s=VY%UEJXn$`R`i~UD)y&X
zXss89S3V=Z7sDxcjv1Q_>>}$o@&vyhZ~+_&>jlMrtyOt`NpNYp9`_CDhpva(VPoDw
z3|ymz@7qSg^zfB%F<X@M_J~m-QQ-4qAqOZ-qqdRud_CWpvm50YynG+W)<nwI3=3c}
zIGIy~I&$(a1DCi3h&E|BaD5KSI%MIsju*ks#sZT+<U*me6F>OoJ~`+O!-n1Uva~60
zp^wgF{C;O92G0)XnEg^-Y2PN<uhgzQXkZj}7}=j~FX!<6OGBWO_atsuvJ@(2>EMrL
zr{K+Z2i$$7KZd!w;MbaLa$K`p2s_`O<zrRwTj@2BrDl*yb{6T475P{8Tk52uf<}Y)
zVbf(Rd_M0ZSjAZLziJQRghBxwXvxPD_5q}EZzBGh981c7F34}4jG&qJ%KYR?fS6YL
zfwDH0iTma&;_uJCykMO!pRKUxbqaF6SzbhyPYNLS%{Q{kn1D9J9C?gkSI)jzLG8J{
zIq6?CUh^1^K33CEr(`X*eK`g*{oQGFgDD<S1wP@{Kz9qI-*m-Nnhc}(M5r!gws{Ke
zTLyEfCZL1OBl>k}5MFydjmzX3utD_?m5e(fXt|hTbf!7hrgbLsu{H1`F<xAMq>Wxw
ztPn$sCh+(W=^iz)Bds4GkddT_(vN&Zt*-~5-1a)?yqJi3JEZsQ^HKPGr5E3Ou>(Za
zQLrpX6V62FqtcW?s42M<v;R8=s>^h7&1XA4zIY(U<!t2zT61_;pF~g&8o;g@lQC|h
z6CAM4g4m2{;L5#mO6f}eooo-qm+RR7sVX;SE@CBBU8>n{$#$<S1=pxG;<L^>A)>t$
zeeKlo`1Y-MyE}v3^e7?tMPE4XYmKW7o(hw~K7n5E!RT`DAyrAewHl8o*$+QWzW4CA
zIJrfIT3*ej!dZ6wQ0l#CT#MyAxgMS<L$S}mvHWb0#Gg*vPf<I(IXpiEy7hl2=zOuF
z&;y%z(fv*UcAI(p0UwD)&`3Imy5roCaa<X>3Yu=*2Zs#_r0`u2Cx^!Je#d5dIm!r<
zZIdYI_B#COvzpoiztN3HUtr#zk>or=>ScU!#v`q^++x=x)Qt}0bA#7`D|zx)DYLlu
ziv+a~D57z$U8(!-c$hNJ5NE73$5lPkY53}5y1t`=LW+;k&z95FA!v|bGUvIdea(+2
z#u}jF^sU_CX*2!O*g>DFPqFvwzH}y8iQRM_3g311z^+|K#j$RC(Y88+mbxFu(DZr;
z()uPoG3bl$MruLDbW>CmCrN%oTik3C$tvHv;L2QWHWzQfr#q+7`kyl`xowC_E89qI
zc?yJ>n(`)3dnwyJl!G(t2xsOI%sfKV!-n#ef(|&$U5{g`(_lcv1=3iOBAeVMC(-0I
z<y-c{3-3)RRbXoOFhaGIoji%&3U3Rq$um#u@aj`L`LwbUcq~kzv(qPH#Kt<@_IV>7
z+3SKflaJ!w^)YzcBLoNAYvG>nuY{G-Giy-31#Np$0k<Ehv*xcGv^cPl(wA=KCu(O!
z+pjfL9omf-&uXUiQa^X+m9ccU#U1as_(HBl60Mf4!hXLj`Ocv(_;3x=n&POc<}G#d
zrGAkZK6@8l_|uu>Ap@~==|=AC=t?b%fM(k41Vw*Gep57xlOO&OT<0F*^PMX|ZS!Vf
zPMa!CSa4A|w73vEhP@<PS1Ua0+lQ5(UI4%7AP%hQj~<!(#H)4TymjOySnc5j>BkQU
zU8?%vn*uHL`s2fYh8u`IRwTocpd~zPp$iP&vqfC)|ATJ#Q^9?Pui$8}u9*M$1pIj9
zj!S-~OIfrz81urK&#yTQ?Fzlb!Y}=Kg6j_OsA!S}X4F$<s6P%UsDkWy_p1zC8lcmE
zb%Kl1KJir3Uhq`2!c*~iG|uC)Ji~5V<>A@J{55X?KPl}GKf9hI2eCJH?q7>_X418h
zJpylz+DbKD4cPufiu8_{PaYE%(Dpr<qD}Bmh*Or~g+(`^^Z7OK$m}f?>8|Gd6is-(
zGnIPa48E`B2eswi{GcEel83c||Bg{u+&&&pns{UKbOlscy&IAyZ-zf_Rd|9*A>IB~
zPtmizDQn{!%3K&h$?3gVWuG$Nue%Aa->J|~oqI55U!53q^$di}a;6KvV{wacmcB0f
zBpWwt7ah**%7s%7fklV?LeIXoTw1k9rV)OE3!coTh#%+Zy7_O?6eHQGaVJDp7xK(w
zQ8+Qp8t0|^l1=<R96$alZJ*T*pU2x_<akT;PKrR4kXXtexCK|bD)E*jhh>Yt7~rC*
zuW97>J2YSUGE5qCo}(kr)Ass2>b0*UyKgoT{5cbyO;=IIc|)u%NQMACC%(2R4|H}|
za^<0N(I`ri8V*+Ag~{2xU_n<tdf)|YDhCqZFQB*KPS`Nlf$h&eM9*3usx}w_2bav_
zgBh23(zD;RbHi2&nORFWltxQznNkj`DHh6~y&yv=E9iY_yLhtf1pOTwFR!sm!?~Xd
zapbcVIAo*%yWLVa_4pG0-QI`))<_J%=cahKM+ZzkP)g<lwt|PjL^kfIjw^GX!=tk4
z9IbDGTfc8+A9;hg{c<2(=<NlWZ$>jntbo&(lf}c%p^}3t9kz$N;De`<f7UAi8l8Kx
zbp95&Fc*HVHspNEPZYN)mDiZ4f~B()oF6zF1BNBQit#UHCjIyD!!?>@cXc#R{G~_s
zdn~x(o+F=luFsc#yK!My5^h^$1_5fps5*9x^nTh*eI&lw!RQ@eyQ!4IUVH$B;*)rM
z(o*V>uYyyn6!48(Ce5?C24??t!h3t<wDw<jUJ=*}UM!iz&Jm3i)0PhY0TCQH=_Og)
zD&rMZQ>n|*M@WurgAZeWNKUrdSX-UMcH#n5@_hkM@7cro&0R5O{|d3)LlxYjUc$z!
zt)lW#BeofLP<B@{itX+%6HQ`lsC<8l@J!18cMO|`%Pj8F|INz3=@bVyv{uvpqXjfY
z0;+WG+=B+@=feE?D@6{P!oO1&iZ&8wK`mk%HX0gZ^8aD)Jp6L}-}m26X(%lfiH5C2
zL-%!_Mxl(1qRg^Fr4S0CU5X+tL<ucB71ec~Hc_%?8Ihg&R$2L7-@oJc`2+6m$D_yn
zTIX>b&qJDx%H1CXAq(~jGncR6{RbzY?aZEZJ6Bzr8z^CVx9t#9AZ4TNU8yl&9nDN0
zQ}g+4xNhVh(AYSecBXbvM*r0`Yhpa#-xUFO^)7)+&#rvML=}t1WHC2xz{;+Nc+S-k
zSl#xCy#0>S;jl<}eP=o9OusA+kZlAt9ewcDOJ%3}3E~|2wK!&zE={Y?!--GBFlb#o
zyo<C2{aQUZYI>Nv8yImPiN)aTR|LWBCValAKh|k(fSwPFL}%v;G5_HX%p2{_K@aEQ
zug}U9_S6Wj<&38*b0_0VuS^;rI#Rqg-BujC=Y~+(G?gdwad7C-i<b<_;%Vn^IZwLP
zM3I+<lb1mZxNPtiGX13AuEM+OLX8xmVZ=W1Q21N$sHp+F)uE8@6)UWsr+_cie+_@H
zZA31X2cetA9SS%y3?_R{7jx#RqqS!lrS3cceP@)y;;<<cSYMCZ%^pMdK~9)|-ikus
z=8)(4Mmh+MFkYI&P6+)cst)r;pOeF+-(_!JbnrIZw+j)J62H>f!!b0-w3t1Dl~{lM
zP0IOpLh67H;B)^v;p<d=9y)F^&dt%ssqXqXr$rgkPrrp#ry_)(5@$r~Pr8^q?*)Bm
zRH_ahIDkhVv_r>`2jF30EX>gh!=pbk$>eGeypS{s-mWs|jV4mxyXqqP{^W&tVDV{)
z^4`hyvz^)SWr=KRVMTSt*Ab`;YPh}4iJdD)bFXhX(A%VhTfS<*JKO#oYV?A)otlO2
z{$uI+5M{Bftrzrod6CD>t)(lrx8OoW2zK%f!u!`}2(^ASvf%@&D8C^Od~3Vlsi-(n
z_1b+}UUm;=F36yN<sDFQ{~jz1G^41$ao{J-faR~L<B3zwc;TJ}&y5*_tvxn#>BX~P
zY3qsMCz4qAM<GPkt`Ysbqq$D%?+aPKkGlW7Mf>iUV*UmV8n)gJX4E|a<2P&hah8;m
z&*=m!AIO6kl7}DH?iShy1@S3WK(~vze9QMCJX{tdENVRk4k=okDxHhN>75ubH6Get
zoS?Doy>W|_DX?=pN9&6K*5wq5w<boz{XxA@OWK(~tjgdhFyRSO4}0OqD^<d!g?MzD
zJ04$RM!CgF!npssR;!PQr*US3S*5%5nYra`e|j_fN_i9G3)?t;`Y3!N-OV+Wq}lkq
zf#kWTm^7s`$o8HsDW@%g#`;M3){3wflEvx6)geH45NF4v$=+$Hpx<2sERZrzZJU(X
zF;MDrdb6Ijr>@2-y;~S<)D4{mmSY#g(>&|?XF=Ao2G(!%f#<~nmh}l_uR06%4R}F|
zvM<4zgPJHWX<}zX7Kqo9M&j%bU8LEMHCoO;1~!*R@zR02sJTH}3llQ9-pfVG-3;Jc
zNxQkv_IvPl^k)k9+)WxSZ)i%Q#17n_&xQqQkg(R0{_Xe%ht_AnSXEt|m41^N9;NY-
zfl`0vkOZ!JV#1?Np8<2J7f|JOl@v?bMst5gVdr%f5Upj0C1aH#BrBh%nvLS>cilP2
zW1{%3`ZR6&7fYp0_h4JS5w5-+iJ{3Qk{8#V(p!x=tF}L0ko?j{FMh%4@;NvU|B&8+
zy+We&=B#y3fLHMcU{<3m1g}t#MvmpAHh&Lt_7-wcy9U1&6~cSzZQZC`1+uwYNVzJ4
z@7ZM2u<!@8aP@X9oeSuCaj4L@s+F9+7PIPnNu#-A&!1i?qI?kp4%OnW7P_c;ehZJ&
zJtAe955Q`xnRMT~7VrI6C?4269dj<(uwD7V>OC6EX!p;)RM{z3&{7Cs{iImf@yM9F
zFF7V@Xwh)evVsODy%#@?j-}owEO^028!o!4$Hf<Rv+2sA^n1%TK{2D4OiT7c@cat#
z?#shs{f5zaKS2-8LgFEB%~7#-`!)*Jw&9Tp`Sh>iFU0knNgGtVpvOipaLzwOi<h|3
zap+5dI+buxX(3qALew?L0UY0t)Hn4N-)qkn-M!q!%FBxU_1hrq_jDvjeA@-tH~M44
z(a(al<#loH>|q%A^8k(8F<<y1KN=sry&&$f`#k*g2X(raE31Ajv2#1ucS4U5S(sha
z3C%7m;+Qxu2>$8LO^=j#mVzd#Tz8^HTODzE)&=t3w+Dht{?XS2FEA^#lJ4W(xcd7L
z-Z-Hvc|X}g{X>Vq_DhrT$@B%X^wqJje$)WSE}iO}@xBu`<d>0=wE>s^GD5FrW1jan
z8ZRD7<2@TEaBp^E?bx%`mMiM1+sKtrUi(N~G&GBS-Db-=3ZqG9$s=*AU?kmt55SSs
zk({*Sw{!SxZB#Of6Ku;gc=YU<&}pdy_v&$m?otV;ANIxmeG){E#;v%ovppG_o}nVu
zq5QU!29|Y;<a+IViLvAX_4iJ~B#VP!Ypx4Nmk4|{?~9PWQI*Z2E>M+i3S7CjlWJd?
zk^1hH_|~r*kI)>2gRb@Ez>k}`$D%l1vR8vYU2&lC8Y!?*VmVLW?aoX6BRHws9DZi{
zf&P1YR=m3?1l_G4kh8)#n&2d3i<merI~y*Z>eLfQY#NO=@{!ojc?y1B_kpHAx=nJ^
zfj+)0=k_cA!QaEp!p-n^V3M~Tzt1<oRiZrqO-`e{dk*~czfUwqsfTp;Q6#fFDv)+<
zFz0AW_lG5w)q0sfp>fDXXh%=>d9l3O^~VnSr=AF*k|#c>q6m)j>#A$vi&1aNQL@F}
zIJe7lI4>7Xw|(95^7Ni?$8(<S@Ei@+xwn@7>09yFc@JUZ7<X~YNp-Ah$mhulaj@)N
zDfRqrgR@pF#^?X?V7E&Ltqz^T`T=(AxyDQ=JK`<z+Wx_&tCQeAhi&W;>5IoCqNUOd
zf<Yw~FmCxX(%!cooNv1DiL}M|X2=p!oa&7JBLw)^G73DR5~;`B>w?0HC@Qy%f<Y}C
zIHF+%YHvBrc1b(I@@QY~aqbE|iOuEIvO0Kot4Yk#(MC7D8uW_)3JWhE6rPH0bR*G|
z2FV_<``e%JZ(uV;UGU*GiwxjUP!#VlG~};;;$X_R%`n|g3G&B`#H9fi{HO0F8W=W@
zrYEGZkF(Uz?%SW|{Az~ct2GoL-5HO}E}-l#{V*W08(REuqMaL*Q9nrPGvnnPk=8;R
z&Eq)dPA875GJyZ020-9|D_~)qi_>)$@%~%3IA&NRA9q*f<7w}~^s_f)9Q2kru6@Dv
z=^@%~&_)>tt?<H+2y6~J46RpWxYXqz?ff>CyQliXss)*3`Ee1LJ3o_ket)07$>q_1
zbzLN$xjnblPe$ElZ<cfSsP2+<vl>=x!nNDI=+Um>cwx&=`c&-0O1J__9YP=|qQUvQ
z)=tpro+sE2wc+_j6=3@6Ck!1+6gF=+EPCw;d8$uoW%_o!Y*|;mWyK`aFdfdy3j?tz
zY8Ib#G{db^YQ)=1GvV<5Jy0kXV}#B>(4E``6-Ml(AteJiq5lUGADBrwv}<rS)C#q$
zTOogP7F|=H1Izwq(S2)MetR#7mtIfj|1NwJin8vIfh--(d%dQY>P*H_J?TbKH=Z?o
zDo(nUL?3S|V9y0##rmc9;aIC32HFh9bNjAWe{EGH!^V%Wr==gM(P`Y{*bVPy@2Ni7
zWv`?^2$X#NA=HN#QK!pe&~V-ib~!PE?~YeRoBcVwTHlPOI@#fn?i1N%>OQpE7>DEc
z>EoYA8_;`O11!v0E%@dg6|Ahj397o+#rJ0v#nko~x+3?B;?AC?$;-m|=J$=zCgm8E
zTph8d%8oYF#le70<)D<bkT3sy2h9buxYxA|Hq~AtaS+Nx_vDSd``k4=a_WR|QZJg9
zpD4o(%_%%)L9V1pdZArM0ThRv5q;k=&RKh23|wwOui~#zwy1#j4T^Z&0C()udZSul
z<Q;na@rZD~7vkSte`)5ZE08r+0Y*-8X1TWy!0Wd>+d5Ci@a0NuF#RMvOZGvf;GGmO
z{UPYu=!!RXu4RjpC+Od#dtkjcPuyXUE?YG17Hui{CyY0{$g2ZlFe0gqdgoU14YflM
zqvp<gdzwP}%2lYit_(E%<k?Be(~U{`3uDvO;mEBhxah6Vx6@Da<IT~0Iqi^m+WjEt
z4?aXKJ5{NzW;*Y(cVNrd*P=tRIxTuTj5S+}DeBf<!Kqydj~d^Hbc=3q&{2)ktmgCZ
zpp$UnX8@-8YNFGGt@z@yi;%svS}ggaf_aK%z=j=g=t2pEntX=5J$FGaO@?}!f8mR~
zJ@|~=PJL5*@n)SEem&I?#|PxF!ans89qu6J?X8Eh>JfPJ_Gvo&Aqb7LpW|X_-rc*d
z4sV?d1D6r+!OC6wJKMX#vv;{@9vDk^Hnfvo%?(&YE@UogRLkuS;*x=}TsiEuaN=${
zPa4#l6Jwfb&=n&gJ&9q%pJCWA$p9{GaL4i?_2iX4A1-(g!ou=%vd@oyKyTNbRdRas
zxYz3);G%gSimnVq$LXi=2^qoej$tsNXnWO#_+yZu)(59;n~s%Fjj`XLBeZsI9Le^N
zr0>7N2q*N$DSy>4)~+-4mNawg_emr?{U;2tt|nQJ1biW}V2qRh1OJU%g|0)bxuMh-
zS3k4k>lL$Mewuf6`->6SqWT&_H>Km%b;;02`8+J~GDo%B{n&HwatK-F%I7WoxS(Hu
zBozk^D(r+yx36KpFmG0yp~TNGUL>_f=>!Q{M5_(9(Fi$TPOk7Iv#pc4{^ddNi*S(M
z%^{4^+}-HZa`?Djo*wO6fy@27^8P*H{C0;q%T`<wzix4+qYskt_~jDF7osZNuZ@9?
z3HODY4)*w}YczxgHDbZoE}Wt(-BtgaO-{mSZZy0Dik*wa`Ln(XuBXPcf$Jl2<I{H9
z<OMM8nJG>`G+KO-TSDrA%XxL%4%%~X7@u9Y9<w{0mb%_^v5TDsp1K@@Ro4q3=l3em
zc&z|EZe>;X_<k6cJ#rIob={8Jt?g(>el8Sm_z#k|#dH58IoMy&O6%$axw4^JlqhIo
z`^(=FSF-^$29M*TArZW(-JU(tPjc&AsgKaVj@-_sVz}Qap;pU=Cy$MRtH&o&jL&+p
zZh*wY@L$IPu`ROOC#SMR%YuSw(V|`#iEC@=A-FpA=UK-UoM#-EffjP<)!9q-(dngT
zxUS+XFWZoXZqtC8-i(G9Pb&nazrFZhTnv@lY$NkkU*V>l4r*s@LGwPT=yP@}hILy;
zy)_2o6b+G|q5|D9iI%-yJe)tA+=Zb@I^ypc(?Pd73@@#-WV2dj)Q-rKm`Xok)Vas-
z)MgRRif)HTa~Dx!)G^v)aGQR64&jz>7BK1hMB4Snp7%tIkfoF)(#xbA(3`RnhdkEA
zIJ2=ZZQUcXQmG_YyG`8DI+;{E-Idvun6RC_Hx3vtW&Ag&;F<3t9ow2rC6S-Vtk!}T
zk4xcelb+FP2Z6PI9;SZ7$KrFJdTLlR1FbSFVf@fOqRkOcG;<yzrcBZmJkBb^hG{L;
z>h4)!vfu!wcJ#nL6SfG?w+Eo&&z0;|V}Yk?4??Tr4{{n9LtjjL@-F!{TKz|#)~qdr
z&C_z|r(;hJvQLB8ZEtDS_ffFU?I|=jd2;5&YoO7VBV3EI!3_V$Fgn%{<?k+~r}Lx0
zYvVp_lf0U*K3u^?Q})5VbVqSql)?zG_y?D)cfg~IC$M8e4e#1|3zp9{XPE6tWh0MK
z{-`jBem6tx-#x-HmyOuBi#8W3`SYWzx;RB{72d}bd{xp9hgh7XMYUnF`n#8@{Yxse
zef5U`SuDFOPNH5%Z&Td$47xdR3Mwu?N!wlz2EBXx#V7A4!skJnsOpnaooC*U&WtRe
zHiwaHr|vHnz!x%*Xu4_X-DD3ZwZIEf#{23gsISXK@IP>fj+73A?G~RQWK0d!S0AjN
ze#DIjtN#@|E+z@1l#+z7?tb*oyhO}cI+|a6B556L5!@F|;)<&Kf^Lx;TZYb|BYk>7
z*?>yIO~$NoSC2dWbEhN!rqDi*`83ul9zx%=(|l>>_~q>`Ts3+%{iqj&i`N|Za?5ot
zR#(L{>(b!)()Td(>~ProJDMK$_2M1BeZcIqA!7J%(9FmJ^?pl0;rVF(G-VJcDQ=ea
zYa3McNT-sn%#>IJ-AA)YjXlOnyt-w|ebBykpYwLfBURg3nyXGT<Q<Yf<?8*#;<zW3
za5wY|{ms1(WBm4T$NX+^>_`V$E;FEi4|`BZp|dEe%|oO857?z#1DiV)^1H~PEZ>-h
zSGD#7oVy^zoXLVs<t1cp^OW=?4~L=SQqlHrDcrc;hl=ApSl#X<Wo!k|JTI{o6O>Rn
z@~O~uyt2@F>Ui|4SVgmkzmu|y$DM8TGHGvAQ}xT?XTj;f9NPXzk$26x1$7~goS&Nk
zkKF~D*{hzeqB~>``6Nc(?867j!|8YDJbwS`HT`+D5<oZtmiK&csl{mVNssf;baxBa
z&+ko^LG{$fcL?X}t%UGCC&bA+8pQg&3&hSRyU|$Hp)~Q20{>h#L1_Bv#HA~i@Dr01
zYIxd{Jq!1ec|f}CXZ&T@eMN}_q`mn<#sOhhTpkT=Qo)-Nm*$(qf7&d~IA3~v6kNYM
z^PTFR;<BXu{50H1()LT?aK3bgx9k!MN<WGRQ#bIi4b#y)s#$oYI1ZHeX`s5+3?i*I
zx@n}1FGIE2UFyOJzug1+{8Xak#Qr$ty(gIm?}W93w+Kah&3Vk2{X)%bKbA>Glkc_2
zd+nm6ZfXZ|8`%|28**T6e=p3rT_b)u<imZnc92f`M~V%d&mPNmVeZ(gTs1aN-0*BI
zUtMzx2G{xXrt@+mTqO<5R7YLn0Xjl=wP}=9Gl*lCUKDzc?!;%4hQO_-1c_-@Xfx~q
zJ)WHc7m5e)>KY}O^x`!wXn73h2RxxyC0+4|%nOv??!XUsXG53JUfA*dk$51rJ1;f8
z1UEO-&^+}h9PT2A8CO-LEb>%x85%@|PMLhmss&ECr%<JG7d#%cNBAq>AUa2$r!Mzn
zgi976wB3I}qqiM{O%ZL<eYy`0n%EUIthPd@BYCh%zLD}I?zg8<K}Amv3$@PvoNSTE
zH^=*tXQ}~)-_^w@$FD$dmjZgc`Xm+q)x<U%WjsDg1MU5<2!@j8xoT$(6jpzd`kqqo
zebzT>&Fg~8bX9rH+rPrhx?SL1;ln0zSzzOq$tAjt5L9qRIv4WE>93ZsD1RC2ELFy}
z0V`?UZGY6fTTBNt({S6IRN9Q~u)<rY_ER+Be<dT)sPeclgf8KdkXz!;;BujO6mrI*
zCVKMC9(|%?py^yRZhc$=YwM?g@3&M;(Op8z=iU^qmS>Um?=a@wJJD~6D!#2bjO#q!
z(rn#tH1FdBZg0L#9dBE}+G#aLM_z!NS(1+3=^8t@j~8ns7Du;)B)B_VhMFong{-Dx
zfVLt0*W)+@ygDVbY3fYVioXe3{TI-za826%<Tof;eus}a@2U0cM=?-Ui8SBrqFF}i
z_%-JY#LsIHJUhvwN#Ia;Q#}Ng%Adgh{Qv*+|Nqbb|3Cl#|NQ^|^Z)<P|NlS#|Ns2|
z|MUO<&;S4bkN;n&OA{WJHo~0EE6KN02Y82gh#7h7c}DCKwkhw*qiRa1NfwOzy>l_S
z`yoiFGlK<w5?B4REl(M0P;FNKMtrfaon|%6;maMmeB(|LUC6rwZb8Oqbw(XC=G2ht
zexW+3W((a+`2<=<li^|Koq}UXiL>dpPjvW|YIW%68tAl9hn*fh6V~V$(~T1!A@GkT
zvT7(6x~fS$!&3UTB||u4R|hU{Z1HTm3U1l919pskN(<@=X~nrTywJP{&h#E9vEygL
z(ds31%DRr{OjW?eT|>C@Tt9f>*anfaN8lLsXOxxw3VKYvLcfo_r=6ug=<k)`XiyN%
z-|J%`XM%q9wSg`?RPm^2_~a)Qx0~^3+eK7=%9Rzeugl!F?&V*f3z%+baLC_{Ec2A(
z^*Nid$lilXHXNdNpGOJaDxSRO&pn|cBN<&>^`N$>3cZZF!--0DZ1EI{N2&3HvqR`q
zLxqsMU;-U~b^~9H7zU=s6&S8I7$^0JhHruS5H{YIN1v8q_qi5)vQKBp2X&UJD)ptD
zZW5Qpjf43aYe2=Ikle&+sN1%Rjy+0bx26Q_sMq7>?mICEJaF<vCmb-Dz-6$exb}Su
zoL*2%DIcrh`tDtD+HWNpr$^G51v|lX>ry_sxk%VO$c6faZ55x`Y^QD;OR?R_olmUW
zfZL1*bHM1XwE2w!ztFb>^N=0jd)<a_$4S1tJ`x}O<V%vx&!=oj|IyPrMs}(S=o^^^
z%}?~O{aY9*JI3JVLF-t@a}~DR7l0dCgW9maaQ@O3K{d6RU`VL2K;ja1wr_xQQzx+{
zD&ndSC*a4V$1+<xEe=lJgh_Y$(vS8Ncxuf7e4h6j#N3@#!?OdSTIyr^*-wdm^aC;Q
zjXqC3pvqZ08-ed$6%QC4pphP7WMsOTZ5ocS>e#pBc6%rbKN89PZ6x(b91IISOoG%&
zPIyIA($?E%Vcn_wg3601%rt0(N3~0Ed(?lBrYUtke^+7iQV%jxtKv<S$~dIkSC);Q
zkM@fn@L)Yv95DPcKR>k#=Ed&k>{~W8)uy3Z=b}D8*nSy0u1@1;BMteuPnqn;-#j`w
z=?$)4kRw!hr(>!xgJrH41kJHpv~}AS)CnDexdlBjrcwX{#d-XAo08<swBmm6R#j<c
zX=7mX4oavPjP>o)@Q>0KG8~iz+Gz$d^*h^T<eDqSWquK~PNed;jzR2Uq{C?yB{VbF
z7=C`(N!{F}jz8xRSgUqGczWbImbA5Tr1Ml<vAan;f2a%IpRx&me>#c#n-+0ug@B1x
zTGcmItk`0e1})4g=jd(2`RT~Bv_Dk^oi=&kxpGOj$PGXaJxV&O9dPjY1iW#08@VZF
zaKu16nAe?HK{FN~IEl36vWeK`+HRo2qZH{Mfy2H?T;iZNWIra6J51{6%nNPsKi33V
z8IQ!s;1l%R`UzD|KM3VAC2~1ieI_v6mpd(U<jaL~`F77&&?zv1Tux4exznoQ;TIpl
zrF$*Jr>SG7b?u}c)gcW0c%2>zQ=rStdNL6P3d@@wQucjcsf#H<*jF(V`(09GPsi?3
zCR&bnI5t6qaF~=o#ZjKqP;P!>#D(SA^fl=eEKbZMxjo8wec&!=kvP8Zdt8L`dGW#p
zkk|$Bk>HfxPJiON;yP!9H<Fj6b-5v*3NPUHBSXcz=CcUBf@!CoM)e_U4Ze4k=+=Q%
zH1D%1{3z(e6ITw#bX)?lPs2#TX1I{oqRuxDgmUh68IJt(hWv3B9y>4XtFKjYQm`p4
z?>r7xolJ$Xiburj%e(TK$iY}0oecwJZv|6}%|eIjCGj0j=HP5?ytSr3h8JF?)6E*J
zb^Rc4!X^GA>2Yl*li+h=CO2nU@`yE>Frj8J#qUfd%beRdsq+rbd$WSiuX+Q))3nbn
zS|e$;GyQ1JW;s6o&;fhiyCqBOQ!I8%JIZJF7V*xIL_y>Gees%UE>4S1!e`!rw4Y09
z&Ynx;7CVtLH-<pclVW(1n*%Kk-rVPiA{X2?;P@PKR_N6ihiWv#_w`;F{Heci^!jgc
zMTr_^XRhKjl?e#ao~p6#Js6GuMka}UxX!de_}To8zP!Bzbq15s)VznZnHCFsmG01e
zv$dRJa*jf7G41;O5F+Q#XSbzu;P6LPS{~X?AqBl~>%&73?V7@NLrp<7ei?OIqbp3C
zWQ3c?k3+fjc5Jg<r19B%giTjW*-h!5VDj>{u<m{q4ZWhm?!QihQfWB(N`A|R1yx`y
z`B2qgkA+p241{Gn&sR;AJ3*f2M%eIg6S=>Bi)|eRw4>KR{B^lA?<zKCy&+*7QF;p;
z=4J5gwbEWLznSIQd*T|EE$k(Az^c0+rpKK-bD3Nkl@8uQ&tCW8+wU6%?I)>V@>5fA
z8m)#jZ5sAY2*FW}A*geF0Cq9o0}q_;)6wrW;x}4^OVl5L)1jkqQ*RnV_&%PW-a}AG
zki%Th^Wp`=qj1*zov8cx0}XGhrdQwWu*j%QXgtzUWmOOb`uVopQ`Z{pz=De^6!1&2
zDz_y!5VqK$_HkEX)_<EJ{`p<eV9GFdKkmTS<65OoABk76`n%|5<iHIRZ7aVW`%RB6
zkHDjIU3h=#d%;7!m_J)Q75Xc>i(74Hv-?mTh+cM^|5MZDTYi3YA;u5lrCzlg<}<my
zY!P4Sn#50*yYXJlZ1j)&2y2&imH4Pbu%h0YeN+ICFCGntZiiv<@kp`XEfF@Tu&`V+
zf!g=y<2Lo#;&-zp_<i4W(C_#GrzXD;ZI)jZuKjyW#swK1x$j2hoZ<cX*-k?=K5vMy
zP6=<E5~@$e>0s07NXpwj5bbZY2oGO=fVv|hmH(%Jx4IahO1ZA=>METP3CrJ7y5%Z7
zH7iB<9yp#enpOBvY=W>*IyYy^x?)M7i8$oXYACKbMY>HInC!Anu)e<<g0)+~+(}0k
zd@q!r{XGeP?%bto(OvkMYoKr?>?RK_b>)#i{MjQS1;UI~Krhh@m6S%2UiKe&Xj3dk
zc{PLa_h{_XmchlUBhg@A7LV=a2wzsMWW%jx!p6Wt{`@n7cja1AwZUZ0`#hg(EzPj<
zZz<<|ABs8-Kfo<>9pp|j#QlZlG&J;$IP%AC+AsS+Dz*{iw6im}4f{r!r);71h7k*^
z9mK)KDQw+xmFo7pa!;MZG)=;zXI?C&5l_m<c<>eRRfIEl%nG0{17avY;V;xn?ESWG
zvAAVYH(BcVO2})s#~}X%u$9(h7o#q`F;9Wj?>Mmf+a1vD{3F3NL=G>XxJH^UK|Fta
zFm{Xyr%VSwJU=-a7W*e~ruA^HYfZ0ism<WqZuW3<`WLWzTFAC>#xO22PBfqF#=j)r
zr+$m0@NuFEpHeb(-q^67t&DOYdx;z0BOAQBY=H1!a{}jtr3&sXPOSG5s4+egwPk~(
zyMqUL{IkP@oyT#Xz(M4>exC3o$UwNetP51=ra{wT8@ypH$1{34h~7?mutZz>zK6%r
z_{MU4oiw7_KHCYMrhlS|zukFx`5yc;uZ_0If>0GbxO870Ty$N;U8eWOYl*`Mn?BI5
zoKft(=nN-5JVkrD_ve`hEir1w8K{k$B8(jo3cY6a#%EP!^j0xQ>d>1gJn6QBbw3Qk
z0S>_7CqFyGD;<p5>%r{?bNO(uifZ#!q5NCH9OJ?QsPa?>m3-6Xg9~-|mE0)0w#iut
zIb+2SRQ`%@&o7`!HwN&i*{4u`!BE~d>NXTVSV^PySdsY7n3ukv1jFmn*r~27Z@zxL
zI%D@PGW2pp;qf(b!L4E_x^E(A7N^6P`-kD_)!%~cfLxlirySP!55sBoYWPYn5SI_?
z0&&|W@oJ|zXq@m&bT!_D{(9xG>PcTpG8@JZOQt|!&23Qyt<aTMqp9$c^1ReAVcBb%
z-SAr+_VO1vnmOU4nmcs&5W|?G*SX*949+}Y$j$%NiS9lc)Vb~$>{;Q)Nm>Kw`;i+U
zZBuCSDhOYn0MgiKEuFVYeExJN9-%S?8#h&h$%`oJ7Q6wrhUalneqWq*P?5b74ng99
z46usaEO8&ABp%9C^!`vn$7gKDj~aoZ{xAdF_sf@3fB55(*Xgpir3{n(_TcZ~DtOP|
z1dDn_;No0~mp-&jjMaYyj&V!r`k*3iGX4#A4wISf6Tp#P3Vw5T3+p96Z<?zsObwIA
z-A!h==Hm#wn${J2c0NE;cU%FLcQdKwUm#wyhy>SjnXvBib>YLTi()jK6O$%8<E!*|
za_D6Z^F1f<fq(*<o;{xZj-IAP4g<s~BVBmRp<)oeBtcELGgUtOx?_I!E}lJXH2rzA
zgUgn`gA+gcW97cpaKvK(4ttry&v6_Dg*)=E`XMm;k28Ku_yv8#3gCL3H{@RF%3US@
z()5T!;)T+gFbod~pWTh|`zJLDpS@4&YITLjO;u3y<CCB^e<-G>O+&R(Tka9%j^enh
zlr`rl8_sM6hp+MMS6U2K?PI{SNEGGoSg^93sZcRNVp69J$2E_3;U|Z_)ML*Nn7(%u
zn;!cuW;iy;y2a?SW4}#wE%_RS?b5^~*Z-j3tsETwE+0Dgle!{w--(^#_4&!_I7mMH
z0}dTZz)w@Z3acN_#4mrpi3`4uWb2))C}>3xUzL}mnI8?&@w+@Y?CODE!_QEU=MvXo
z-5u&z8_T+u-f&2#FWzkEhGXv+Vw>|_=#EdFPwaU|dD-&ZU&_O0^;{x8s?&gfZTINT
z$qS-YZ&NneghGi?BGg=WLi6QgaGc*lUSTu@_v_{HsN1UOawr7*7g$OAlS=i0w@Ey9
z{Sm&kO9tHyc8b%y0>$n<dx&oa{*c(`M`dvlB_J*@hPuoD(OhFG3%<{agVxr<)U$sf
z(n<wSFI6T#uOxA5;4n^GH<~`JH05CTXjXSUE40<h)4U}jrcI2&;4?E}NQ4{M6}WTe
z(`tB~7mrFB2O!E=3#Xj6lAU}N$>|#=;)=Ohbm`n~=;;(L`nzR;R?nj9fLmEuUpW=K
z50S@#!#_azuV^Yc)t9f2>BR%G)p({<MH%J)9d`BHh<&~t1JC|V;*&TBO80c;l)1ma
zTTANhzN929x;vH1ychED^U*?8;t#?4O(_P<=|-ow>fz|Cx_qxNmaFe&V9<b5T=vtP
z8VB~|7lyyt&Sweis>*~zSs~(>GCMq_-2>#ldkVn=95H8E0ZOeAqJN47`VRJn1L_sD
zRPv5lZ?_Z{jxD9dGn{co&_iKLRTkcAK>TxWH+k&nLy?`OY?`|jh9zx6trlZcEjc6P
zWr-BtU6oBPF2eC0zohd{A8XHy<_SJ+l;@&@tF~$2MC+aiAO6CzwvBYoYY7ZltB?D0
zhT}!6Xw>|)3k)8Afsb>iL4es1I_NkT40^p1k5;8pZhaXjd^F){TdlF|q@*Kj4#Hpe
z3^>Ndn{B>Kruk`d{3N1*DsDfL&Y3ebr)~yBYc3`e*+M=u&6m0l^Z+f0rHZ%_G^$PV
zcT1Uoxi$Hs`DcABjMC%E!JYWK?3Xw%!c6G3?hF`aD`KR4GEaG^OXqCwld0kwmU-9V
zMJHQcSk_5)`Br!I>&RvQNL5_kkSw+h`VOYgN5PSoD%g3}AFwXVpsikK1i5A%Fk1bP
z-v>Pv@>(@{;Gt>cn|u;YWX16ILz>XOr<ztw8_sXsX2SEP7O;Arj@~o9&^n_{?Cw_w
z;}x=D*Sa6#gv21caa@ku<ILDg%B@vt>+)DktO_<yhv0@N$XGi=xVUT^=sRwPVISIH
z<BEB7e#&Olwm-vzj5G26cT-M{o{SAs7ov)@J#;;xi8J;-7T32TJ$&9#?erm;)+~G{
z`~7R4EMVwY;rPg&V&?p-Vjs&S3jMH2^xmk2WmnWh$G};b7eAJ#)R@Eee|t%3YYe?^
zpMfFEj);qQu11#!BL3Qwh7bG2A}<R?-(~4sR5A|B5;s!I3pIShGohwp3a>t71n)FV
zxvOs)PDxec>)%$h-H^llPwJHEKYRe%O01OHOXDzFT4Vm5PV!HSVNwt0VcNHGFJ0QR
z6^1Gt5EEA(5&Pwe@L`*P3(qvt@NWdO4}XTjFVn;;MkUz!QlYRazn&JyFxbRz2g^56
zP*^X`$SyOVfl3ZO7R`VDc(U9ZeSE0(oZejjK(+qKeB{m1>fhgeV0A))<gMx{c_Pfv
zd2|N7Y&ZmU8T(~#h82po&b}0QS&KE4^7+h_y)b4>Hu>4eaDwkJOf3E)lXvr`g1h!K
zZq!!_eJ6D%cRon`XchW=HskE)S~BBfpT$I_HX$y0B6jgzC>%bOiiPqYt9wa(TH2$x
zLCvh^H2!E$URwH5sCiyWU0ds6f_rZqH9HNgwq(H{<x`Ze8;=9u4U^u|iD>vY6>jPu
z5Xs>qIQ@PM0oPxNNej${-+e6DQ{ofkFOOj?Q^fQQB*reC%3%wi3Ra#myzs9Dl?hV*
z{pcb*-mw^U*%^Np+2et>tK@St97l=$IripRx>I{m3~aJ+K3f<G@1jp*?(RW+AX**Y
zOrA|^9mi9b{qu2b=dM^O&VV`Nq^!@%3$WAcu&CSrCXAN&9BX=x<C{^<u-tPduP?F3
z9Xb1Hrsa1r-aY`%KAT1n7N4N~Tsmkf$)oxhbLjYU2A({Kgma(O*vR4v1YDa>Yf?(-
z=Gw*3bgGIf-7ZmUi8tPo)_I=dYV>zL1d2Tr$Z*C3aZtvN$`VZ-IHI&$%C-JS)!94H
z)cPgWFI@~Y{EzS&f5OszA>cFq8`#HaVShV4p7!?>&HJxJ%3dyEbvZfM6W|Djf8D6q
zSCgvLyP#KVJ_UcDk71X0i$N0)2u5x$?7Se55@+3|^dN0+w0jRTl7~}>vLPzf&jWpl
zC)iZr%Fm3;Vd}8UbSZI#_-$cld?=q!KN3}Gx^^y`nlGf6rjPhXr_o~51rHwcrd14F
zvw*sQ0!&Fg3hj+K<lK~krz5)2B%@=Hv{4O=f1abh=SFa!j)Ssw+GP|sMWk<2d*Y>D
zPszDsJ*2IeN&l86Qe4YeUT|+NZD{qRO}h_+!Y6YqT{RO1#*M;~){E3|s015lIg16p
zOYzh(i3ulf1loiAX!QjX{L?QU%5Sa*lzMEQ{p-ip_IB9cHJp{Eo`RK%_N?nWn(B_Y
zVoqH&emQN32mTv{mcj4&i`8}^)%Q7@{F{h{K1E`$pM9a9;Wu#WJ51n<V=(80vCOEt
z0jiSnd4-+<KA3fz3`f*rN-YVpp(`X-M*uIasG;jxn*9EbjPGQerL3EsId1oQA#Qdf
z#b$nmAiYa;Wyum={P_*sGOOlfw*<_}pUU%R>EfvCXDGqtGpW~|qN0x_d{0`lVK*hd
z(4=1AFugArY!2qJDO%!&t9$w1gHGTRn*p!SxpK{^PGnP&NgDY9;$hFDc-yx(<{JDF
z1#NY*dgjJ{`8_bS=Q#1U%mtrKG2oZV3eZ12K~y%)#8+VlWHQMUt9vC0qE#YM^=BO_
zS|-7kd5dxG@Nm97B^%ZsOM%?{<8WJNjaX<EV5g~`!$0Cz!_(^>RTs1Si|$e{=~Mm5
zXjf#!Puojy!JhFL_pp*OFG%ab?3cLQx)#1<4#UzB$Ax)jwXh*}0iSf$fe5+BaCDg_
zJ0woRty}bP?kY9j8j+8KF3sVcMaQYkF^cXQd5M3gUWDX<H=%KE49uO^osTy)2@wx_
z@#`Tg>CT)xa4gf29Y&r*;ofqHA02={zW0E^H*&<omm_(vV`mDwQB7;5JMrih&%}a>
z19{4sR`JG@^s2zL<@jrr3+&k4DE1pU24gO~;?yC90DE6kz-9-^dUhPIhN*+S${{>w
zoiCQC48RAbTA122QSh`#!l5}L6+IXXhuxNdEW-mvVuYZxDH6@y=X1x!a<FiygC}QJ
za?9oOsCMT+T32R-8S~<y;KLmBSv;8aOQX>mMzf{XMOkQR0ZmjW71n%A;KP5lD67*{
z9{Fv(sO4fqrfyTQqSb>+E*jCz*0FGYt{QJUu|a$rWRD@9eK<X70zS`~NsU8vvGblo
z)eW}6(Eg!6`QL0IGigTpKzbW>mFR#$_OrzssUK-Z*C_m$ds8TQ+6C<7cfk6Lbv$*!
zO}ef;7V91_lhS0T;JcQl#5u}mXPc|=ttY|UZ?TlAqk=P(<4A6BAP*dq31^;p@U6Nt
z!saC_+2D&CKP}MZK_?SQyW9o+&F(<EG~XLMUYWBLM#^?PaG-hQ#i_@2QEb{qO^Dpz
zPmywRm3aL8VnJ`fVDz6Aj{6@z7owM+giM!J;`v4W#Uru%NPTIoaJb(?YMx=t>x`a5
z^n~}~_wBZ@t>uNVBE$~5ezE5R_Sul3qR#1U{z7Q(77A}f)H^YhM$ImzgP%2d=Y8Pk
zz3q78_(+U@uoYt~dqUHwQ+TQIE?xR%&iOvc!jki$-1EdrdOg0JR!#E7qmAZ#I541E
z-Q0&d_r1yykL1zyb(l<P$VkZYsDRMyWnz`=z2M}j%ALnMV0~}~>zpf*{&yv!$8T+!
z@~e;VF9vyR*<GRSrz^|1%|;C$d*O>;Poc}~Ci+%xz`v4C2ua<$A}{gB(eqPi&HSFw
zS<#V$$1kSKOHWkG%fF-Cv42VVP9d$k&{=Sp@JNg)v<3GYKD;kxD7F5~rz_GK&>@S*
ztx@^ajm=M_Y`rH1?U&;z<F&D@{{U2Lmxr4FN^oV`A|Xt>NKEM|Qg1B-D#%IT(EZc#
z$Hri2n0Z*3IO{Z~9LuJUej)hX<rCDknnCI82uL}vAgbM4L_VfEQvc^YI2rv6#+ljU
zab-)`<dPu-h?#sLeF9v19>v)w3gFL|Wq5tAJ{Q~N;mkQZIM`YR%j2whSr=7&WY7s`
zbPmMS2qo+{sTQ2VZqlCkTKZe*EK82+gF9cZ5zCH=5TEj!?qr6up6NYdTk;azXuU{S
zs2hh*jNGBga0?$ZybPlstFvZ7Jk*YwNGo^&dfe7>?!B%TYUB+=Ya?C!P(P1!7g^#{
z?N&NGe-j<t_mcwl1flJ0O|E*7gV~{%VT0hu>&^1SvzD6NYs(AjrjrVneN13g@YQPD
z3G;<J5{Iwg=1*a#nIhhOD9^Q}`m8p*fR1<F!E=9yqD}G+(K4Qd8HR)K>(Cm}By=6b
zON^Vb>YFfQ^hBDfycL(Kj|1V(Zm9lH3y=FmqK1np&-<c>Su0HF_@x-q(D*Jq8PpZC
zz1~pOo89oY;yER{PNY2j95Hy68z>Fw54utEQm2g_zkXOtlM;(rEi)9yq%VcA9VL=S
z`Y6VaJt_XVu}6@by$n5Tda%jzEZR9->Vp26%69QHDYxtewIrx;tNRn4F<2eD@7c-I
z^PbYiy@T*jNE)x*C&xc7=~FMW8#ITzaP+JnFs!jaVB5{KUh4#Aei;R|N*%&Pvv{#v
z(SHOlBp%1(4&hr`A1wVCEC#fm5$<mtL`?_p(55F#!~^v%uxsD|2xyxjIHzlYOZ5dX
zRgDs-MW{(LgjN{mok-2TdHAO#8UIR|=kLjjapU0|)k)*V^3yH>EGsnP`8{(;Y4u(j
zcsBwncAb}8(225zPEt?%*)#Ox{W?-?I7vrI(t68=@f$G1$e?ljW&C_GDr6Rh9qmJI
zeP&5I-*{C1cZx6cPNTp{1Ee>vH_!2nLnn!w*sL&5^v%9UQ3L()`i$Ffxqb$9N^XIx
zA@evz54pywR(xeRkLP81^Ufqoo)GE+7aLbV$e)>LwstYvK7UNx)GOgolR5QR_XWzc
zSBbtqDrr#JK1!w-{?Ms{)H@V$=?x3^zLNmW|4BM+@fNaQkOez(eXucgAUCg9gFZ%O
z)XjOCsAD)^*m`RiMla|=1GUYli_e+ist?CP>7)z{>pupU=U&60Q$}LLv)(W_K2O~C
z(~e6QZz1_Bs;ocnZB?44KI9At$HIyIVXn*_a(=AmbT1#4Xd?V)tSX;A=`2J(>xb)N
zZb7fkN$5DW93==et^b*gi`V&xBfMH6*LEY$R~{%t_KK~3os<mDRh8non-5WK);xSa
z@jeV5UIK@uccXsT3|4<wD`-|9l=Q}hVAB;iqvruyzjh=aTlqp%KOWA?mn4>Dm;Q7s
z`Y`vYok6oh?vk9MGDq9G;*G#n)x|5N&Zl1w!P2-JY<j7Ug*JB}=E(^%gre&I%N)O~
zT_-H*{g+0TrU|B*w>kZoz<LvHIH`3Uo>nQtlF4=GJ7ODloW2I?dFh~*bwX&kCF8N(
z`$;;&SXNzjjs`mP!KC@$V0?cAP|?^WYVNy7hQ|c-`#xFHO@s0L{SZueQRcij?vyZY
z`Xd0}@g&SyPWKg$ke!1!ubO<B$`<t!AHLKAlK>#kd3S}%U&%D?ojo6H?tpq<1)iAQ
z7pv>M#e-ec=y-uX)L5^lNS*7j&nFnWw#*W%uN!0kzC*F_xi=51F2%u8etE^ia2nfn
zGKOYkQh~7sM}}qL7kR0tYJLQ&X*t3EsH31Z@jX3m`zV}PCf#F79O+$_2#=$NaPq~@
zc*MzqZEyB~{g3K|YxBl~T)+d!cAX8b%jTi-q$DYOA%|05Bx2}SSI%&HE9}onpjn+S
zQ_(*M=`Qk|>^nwaoix|OTnk+JU#-*`90BdG)1`dpyXr3E-$L5YWXgK-9JaY{q`Q`>
zbRsiYRLl2fjjQLxa~E&X&vYYPuznA2wS5OGe;$Dc`nt4ei4(ubNCtDYY}j`E82P-3
zqN1gvDMjLWnpjT89b44t>WBy^v`ix%ufF(tU=oeH?1k_Ad-3uYV`%W5o#Mq8%OLTQ
zG&lQ?;i2PXm>K#>X4JD3zK*J;%!;>E^S6*I-gxkw`{sOA^N`eIcMfug*rAC-0eF-q
za@w;{`epV&)|wH<i}a0gy7vdVV$eZ_f)ka-Y~c4M1!84XD?RkmLWA83Y?c-y&Y2v*
z=Su=`^+y@~NxFwQN5{a5pNqM7(Nh}rFPn}`ljE)~mr)1ng=0}3)Z_jiuz794rsL&6
zr|<CU%u&|xwD>t)N_Y<@6Jw}u>MHo{;UsjQJPKp{=b_v7!BDDNBI!pt994N&_@{AP
z^lizdG21k8Q^5ha7u+FU@y(`4i8217wS;|&K=wAnoZlzz=T3dbz(pH9csH_~dPJ$<
zql{bPZ0&G<7HJ~J$8@DyrxDz8G#Zx-h^^AyXaX&oW#Y%j_l45g$vogp8?2HT^anm4
z77bL(BrPqsGT%QPCr50dk3Y`A2+Qeg*7%Xj8VzCX&n({MBL{t)jtf06RqzGpJX(Hm
z0b9&@CXTMm2Bqhlsax*>oG9gg*AEQgw<fcJ&If^iPz(*9-YUF$F0nhb<oI9rG~r!n
zSB@@s#stOg(0@xYtk|C+%`CRVi7Af4M)kwU<EQWp<0>(`!XMI7&#}+qG2}UN7LHvU
zMf>h1p|jL4={oZUT|HPrp0B#&$><!8y|)(b1axEjmwK!=Mj0xfHADEPKQha^TGcj>
zJoruW64o#JOwo4oJnGwR`2E^nDAb!Mac26$y}#S=S?hGZ_<WqiiV0x#D0{x`@*XsX
zMA4m54<#Kf3#tmNU}2^!tZe)ST$N0>J4rdiIycx;tc~q^z2JUuFqC%~vO(Zt9-?Xu
zm#3=>cLsiyeV98Fn_SE|R@(@N3L~NYy)CNVJx_C|bP|W|D;Bi}MTkx2+i^y7BpfOp
z#PZgYSoQ5+{&w&H&0iSCS1BEitCU00lnmOdoro5FDnX%JFBsbMxM0%jENL0;C$klU
z>Av4$d}v=r;uc$^qyE+A{bFJ9C&_=Ly%#c?r(^fv85H}UJ_c^u0prz9^SpD7Q0{Yp
z+mz<O4Yfa2Ga}kx^rBY!G(?A|ryiwo)xG)diYBQ~(^EQI4Drqn8>FrM@WK8GFePj>
zzT2MxzMJ2``@w4TdDb?Helv)y_N9>Tz4R*01^Z#$P+RWk^9EL(?adPvqM^~}1$dWM
zR8G0yEV_^)Y=}={f8idSm{3n!6Jnv?&+eSG<pf>wImdhA4%07X8TZ{~iM7_L!Vcp@
zv@_Zm_U?-2J#|)iwM#tCmb1n?W-rA)?%$~MrCWS5b0BvcG6B~N{0<(`Nzj_3f)Dam
z@|&v<MdMeRnEYfiU^gA;HD)x_lwYFguSWc;F_L~MCULWNGc{$5<(okgE2K7whP)jJ
zyS@yhX`gOGSh&Q6u}Q%RR^K6{NeBFr;-qX^tS~bDX?5DfxiDo{Fv1o~9<Jq%mTwl&
zsq~BBxYGiUuFyw!*Ttau!xWXC#Im2%-#IHbfrrfWM0djs`mgaKd{<h|_MS2DY?U$F
z|1gK;QjdCx)E()zxicqgrgE(`J3qEJn!`ub!O@y#s&VRziu=>h^JNk~9k3Zg`V2yK
z+f@K_Re509ZpgbZfq&Y+qd)7Lsd~D(Sh4n^*#0*IXSn|mX5XBFDt3Nkb-|159M4oM
z7z||*-LSH(8RmPhp+5y(@Z3=^e)(V#%OA2r<zy>vb{{Ne{_x>*i#Kt>+>>lx-&R$c
zV#?tQr;&C_nsagET|sNuT73N11q&BAieu%@N@xFB-ac(4{yyXgNyE&b)6F<=tWqPF
z`&RsE|9UKX+K*QJkOyaFWwsAg;2F2{=v}vr>ZRsc;NN&&yr*G^XV)2FV8KcpZZIFy
z^WRE+<`J-RW(wIgH;ajnd?0vN4_Yc|qYj29g5qOsuq#g$cDJ;FUW_hk4;wDzs7-~n
z*1Ced?P~5?7s*vMBE4;yLff}$aK$W%%ZcIae`X3Njm#1!$DS2V{+-IZYgBMs#~7~l
zyaFK^6NN*z6Y<SLdrpzx35$2X6Q|5qmpWnFt2*0j^Lm2-9;VQf_c-39+Rl;z=iUGg
z%qilq8aIltT*vJu)fjZ-AI&gag?c*^cyF{WCG}gw@{eYVCvT|oN7YO4WbPVVHmE!F
z$dVZSkB5sY3*y-D!BRBpFY%*}sNu#;iAj53j{j5Kz&qBQ2KVj`cqm^L#pbiHw#N}V
z7deVE?Dh#Kvdd{$yf0pPr9f>#S<sLY1+6n<_?!b&H*qApwfv%cTO@noa~*7(&`R<R
zdk_QSXywLqal@N_VCZ;|!(LnwWD5UCUGphT>Ju;i+hD@wJzXW9=6WdqFNO4Wm%yv4
zUG#6$88KXcGkq=j4zV+{@&7XY&u{m@Pgc`}UFR}|n3prKyI_yk(>GwSzb$J-&4AV&
z4YV}SQkdB3Ai5}igD2{N^fgc7Yh9O^-`8FE!pvMqwaJhj|CA%XIJ*d1VoZ7EgdyyB
zEmPd?KTwEI@u_Y*dWx<*F{@TAQa#%tjzaCvGdanRadqrt0FD30-g*D!{C|I3TRUl>
zG*m)V2+{RACq-5vqY!0OT1G=<Hz_S8O477Rw&;4DV{cg*WrU23tnB%Heg2N```7EY
z>vmnQ@jU1Lc-)8L?*sPotl~>_#ajhC-IAVN?{c86EL$)PI!yZ?I>7J9n>6ckC(-gq
z1}C#VPnlaSjy|Qt;|r2;=b6Ra|K2U~`MI2nAL_8luD{?N(G#OuON6|8sbYoHL)bYt
zffvjThmIq+)^=<$htATDRdGspINNRk*K1@6C0_%C;<K5&{dX)5T>4%-73;z!^(bX}
z6?jx|7(KQ-3UBVbk$L^=Duz|2h%ezAR0qAL8}pQ@JX#yWwMTRQPbF&l^{KY~txE2C
zOkLcljrcN3Rfzwl>AJpEMSLmVfrw5i!dFc#?q3>*>zD4Ana19RIpbyuV~#qKMo=qs
z{`yzErVscqM3>I3ZH9l>I-=p(UiiXoHhjs5hREAYYa^#n^t%GuIW!%V>TS6D<b7cJ
zG!^D$I^xlPMR+N(FNUSW;EB`4@)ItRur+2h4vd`3_cv}wDIZPFJ|m#5LJ9TG?H9>0
zkX>?8j!!BqrT1F<sQ;-^xYT}|JXCEZ3=X|1=-j*mlQbUFCf{DDwc-=?U10%{K3ZHf
z<&bz#-iv1#zLM!JRYQxE9C;iT;6lefWVp7Y=-c0i97jH+YJEB7?yP`tG#7$w-ayK)
z?{M~q4xZ3kCrr=P$4#IN$0}_|!>1#rk582!x_cSaq|SWT`n5FlpCjja9i$tw4eb4B
z13awOf&HdmsOR%*6w!V)tF8{`v9IdIdCFPxSDBLI!BmfvmPfLYQ*YkXe=cqR(*a+f
zdIWzW9I&I+Rl#FPJY^4>%OTSzbM=>t^1}AJD7Y>fRbTz4Cw&b`7@oyD73Y$F_lKf!
z(go-?^qz3MTOUv{-Yh;HdzE%t>Z8}XV(Q}IMn+c+aAwgHT4fk6&eoa<t13syQW_HE
zJ93Qi>kUh4KR$*9uT*+E#s+S@4}peD**MX`icEZD{I>oKPxjp=WWK1Twuu$=txQSW
z`1J^n@JeA;?uDazmP42{)A-%n0_tuh(nB4|v8~vS4oCGvp-zeIEG9z6&S==M*GDK<
z$%GJBZ`_wE%@xdd3ui3!cw_w^+THk<v=*F%w0d3Cx_A?(o0#IzeKIVvJ_Wr@{aH5Z
z0Op(SqT%Hkcx1IQ-?Qp~Bfj<J+SdlSIm;A3_LX*zZVRBavy_F6xIpi{x<HisRZ&6d
zIrVinps>|ldHgy%>~Om`U41(Z$5(bj-*yS4x>?$zHT)x~%^y46?8qIiTan+~Z)Db3
z##73FiqSKaST-x1Uo6k3lG5{(Y1CEx>~#o({v<%(!DqmFcNg9>r>D5aEJQGxbOhI@
zN<Agdj>;n^jKzJ&E6I7C4qxbf06QDT^ZXAN#KR}M;U8-&4wGiJlH5jI>M<X746NX>
z@Jrn6l|XWb+mswR5dRAL=<eJ_P=77a3=3C+FPx(@E7QeJ_h-?xcr(a!tD)B~F39pv
z%@hAtt>x;4tAyoiPYM|!*J$l7XDqJok8=M}f|=+DiANFRWRL0EtOL-ohZ9~H+l9?)
zyyb4;)7a6-g}P_hN&PWL#q?$OA;;U8W2C#qoD03+%ke(2X;mH>3~PeRx8_2RTopc&
z=85&Sc|2k3V!m~9B>!&8hmEajTr?yaA4)w?w6~n)J@SOTpQ`D~)k)ZR%L$8ZF4S)I
zQJ@>^?Rj*oHE(p##ddp5c;2oL@M>!!R=l<Xx33krujV<V1SQhd>FMnK?h%;ozQ-wt
zGH}2v8(v@4oxDeNq9&7FwE2<~dNrnl=gTY%``ax0kN^LV|NoEw|BwIwkN^LV|NoEw
z|BwIwkN^LV|NoEw|Nq1Puk7;yr2c=NdngK)&zUNZ{&kd&{Bnk8TQaFf#RakGL{GVY
z;c?Nw+=6Fr*iZ4jCiB~{09JjJN}8Re8FwEaymQ_PR`@+aqlP!a3MV(rG+B*{y*83j
z;2IRohmz{K!LX<NFqwo_Q?F;`La1XZYnnS^noB$t#$AS!K`ZcFQZ{HNL<@(V;>73M
zTzFRcvf8|$jzc=QJ7Z^?7m#!5mas8(g*e|?k;f;ekbdfLx%SN~(7v&K?XzcISlKUG
zjy@q6lGGsVGF^q^uOFhKITko<i8Y%HsG!n`zp0__6|J^Up?TGnf_(Zo8W|>I+1w43
z`tBfa3<mM^3gFvG9ciPhDyxL8<OA;ygI8b%-}<12`?pM@Z)!g55U^7=-A0>!ly+kU
zA7eE4?9JQm8DrLjh47-_IO<($5*$yc;X8d-erC{7h+0@9);smbw%?Cr7c<Q{E`Kk-
z`*()R&ras<HnX@sT?;3ezlG=>I@lB;^*ovMMceqfyew4}I^6t1Up3b9qF!%d&~7iT
zno&t7Trbn}oy8D#_Ak9tb4Jgym!K7q1P+xR$f}z_I_|^!5t3_J@WuCAgCKR+DmY$Y
z%=AQsO}tNVUFA&dmsBLWNN$8<1Mh*9CXl+KFHl5j7ap$U&hnu-!t{M5+!#4SXlXZ=
z4IW;D&6^(x&Bqd8p;95(8-0Rt+9zp5+9LcgqKNgqB0x2%m6Q%w$cvS|VNmpBnZ+Vs
zDw}A<HfaX|RzIZkYO(lNb-XZP`Zic{-ul=ODf2XHn+~PyS;vZvFT~cK5i}v%0>9k5
z19M}|@wWC;p`AyL=r+nm?Ec|9xMxIy_2EPqm*|VR6`eRQJsvl2sfM06uZxbS&cm>M
z_WV>9!X-xa@Z4_>kG-9Ossmf;Y?22C+9pz;cgykIKNg*%(rJ<LDSFf-vhKRYLeQ-~
z;M=7qn%vzfbN{zXOjFI{!ki?kDJjG&E4=aSxVw1zPprhZKA_-pxooH427l7z^tG)B
z!=-(0#g<`G@0%s`3;hG}9cJNBa|^iUSq2BC8Twx-1I}y>Eh`!+`C=5O*9xo`*p(~1
zo5(k)E4mMGN8gwndUeE>ZY@q@+tX_N@XsgETT%vReBBXdsECQrgE=phWWf_-cuTM|
z)X1b=K<iPO{+3C(C5*o47RZNe)q`to1Q^f?i|u5B`3wPF-yee9`JM4;JH+9=OW|C=
zc=5_hN0%R!9XPyaGFSwqgV(laus^H?I&^a2whnQ$s=k8Lx;=y5{?BB4HpgMk9T{F6
zkS6R{qDA_rPQ%BWm&v)Sl=EFPnJkVWbh%)`O`Q{D7u_N$cK2#Z?Y0C~J-I>kBUHH8
z*{<k(JdRhiI%86uD!U$({5Dq2!YHZtcE^(xIOsi}qkg6cAvbfu`rR$)RK6Qy`__Rl
z_OWPu^}5iS^h#WKs0y4NMxl=6RQ}<V41VuxAlUP_h)Ks`zr;;8tk^;8^zX3NoOPmb
zWe|UD_YjP`8q%BAIb^cy1m$k;#IO9K<?FK^QhxR=2;Mt}ozveyQe6nQG=3G!{kxKE
z=}hk3uv73F<-mgq=ZgE|V+apqK~roOYFpJRtNj6#r#O}R+AHwy!P{l!v&PDL1)I?X
ziMLz4_L`s(7X@oyWx~}J3wdhGYr*M~2T$!f9rs;qq+Rm@aLMyAVr*VI4P877N}P51
z_?%5pQ}m63cXY!Syjt?O%!E@HQ#mN_Hnn|`k#<xxyN0CEyax|iQ|fe9t`+H#^H4PZ
z;fCcgi=aX~n|lX43Ljr-^57Sa&^PcNZu&Bscb>3CbNMdu!1`LjJtRx4Em6gf7PmRm
zCX~0kj^UP;#XQXZY^|dwV4qnx#NF3|F@BZwf4SU_JM^p)-W{sul75*mMYff)%Ddwq
zGY8ZgQ6Z%hie>48PQfXcr}RbYes|9pN*5<zh9|x?;CE@dG&h`&TaL!@@3~G=uls0r
z4Jw6Tr=DyuMuQ9FuX(`oa=xZLi?%ei%Io1C966T^1{<G{_q;$}In-VF(eAhW%1%X|
z^vDPkCu#DAf&j8i)rW*%sgS?u9%<z+z&gh{u>GIZbNj#y&+2;8rLK=id3F=2n>y2$
z{W|zYeq1>BD+ztW4fy6~GmH;6=DM2wFhje7F3<Q$e<IA;?uscj&6v!gH%;;L)viL{
zWv<-+gBi|xW6jTQW`fr3Cvf7ycW7s;!c@5#mb`5bBf1MT<9-yb^Smx>D|||v@sOOy
zY>_WtB;y}ePocqX2wWIt%Z_6$ME@>EqM>;=X`Vb*+M`)gL8U&9HY^jY!j*CD`chJw
zBlRC=JfqBQi*frCZ&I8Zjm=4y=wC`SY6v=<@w6Y#*`O{ipQcKE+O@-JLp}=O4}0L>
z=2EdXts_}I@5ZaVB?qi`8cb=u25LJc4@6!IT{>FN*DwE}4^OvHfz34h{YppnahJK|
zbJ+%GE==JaEn6_&x-YKoTOr)++LM<`zo^-ykz8k<hwrjYa9kfnRA1MFD#B-SR%<;y
zTey;D2J{o^&jn%H8a)oaWrzi2%%ys*qV`%1bi0!wjvOSyYmZgpT<?XPecYRxs``n&
z?PJ7SgCo#9rAQd=83Qi+SCH$^6A=1S>Z#wICw%c!;G^mXLCd)(>MhB@(!ct6IyjcM
zTsCEm;uH?{c|m<b^Rd%aEsPv*iw?h9psMDja4_U4{N7ptNyGla_EWduS>prxUC<K~
zuj}yinU>6xb4A*)fj0+qk}L0(i^gq6@bh=Re6)Wl)pqK`<2+B$P5m$q3pA#ORpUj)
z<Qg6`Kn1%$sbcZYNO9u6lhF6?I$Go&2e}KB_-H}}tz5f`s>gIb?q@!jb_FMirtOEZ
z&fXt#mGtYdc2>TuOQ#fRo~z1>AHL&x(QCMVcMv~*IG3kS`VAiGI|S#WZG!)u)57%t
zcRc1$%+Mzc|J?7-zn-oW?<WpI)mxFWOYK&Q@oyC{&ubE?TlzzA=qN0(jf5Ckj(l(1
zV(>Q9WnB4|`UNY|2ft|$X*xruov<3!hf4Rzn`T1!V`XmbUN45HJm8;O-@<*JXfD|^
zT-=@KAwG{WX067r@WV}s7tPLxJ0+s<Zc$(Uk#e3I&u)}C9!uh7J!fOTPg<~hbE@c-
zbqKQBSBS%}x5za1F2MTEK~TMB1DrS*$D@Y3^HTSGK6|etdr7-0#lwSn+|p?@cA^7o
z&09m?bgRYJ3E5PYXTXN*dT_{^kyLxWkUeSyR6H=6iyz47oYYNjze17!ocsbk)epNU
zZ?s|q^VveqVq1LYRRE#yd*RU^6NI8WMm%?DDE0rli+WFT;)!pSaP`?V&K}htPZY55
zVXFamyXp^<8Vh;KuXuV;{z)8q$P#Zy#L^nxC2rWI#Sg!3V%4wZP*<Zt+V8c{G&L5J
z2X#e7!*$qab_RBxogn$cXW^>x-m*bjTS4RNIG+D4U*hW2Y9IWVOD8UV0PBw_92cBV
z`kj2JXv+vXXRCz{qi?|?<#-tLcrlI?T(RNzTDYm@i3PoC_+jmHIPRkiN$VtMXP=W;
z?579A_V?o7ZaXOPPA<Q6_pP<^I6%Q+BAuBl7i{Gwyd@$Hj(Hn$#M*SuJ61tk9~yJB
z%^jiV<WAUuzftWEi8<{M#$$9fad@>M+nTnD(YtN<{U{|WKcdbXE2g1CRfqr@KJ+>u
znV$K1&@H=guGg11iu36(ZoHB3=SfezQ=*8YO4qS{!xQ?T{fT-mKPy{!T!yLs5-)v9
zx+X_Paq<ag>H06^vwQ!+-+fi|(qo<&GsufCWsQaMu$i!Mp&6WQO_%x-&I$IL^?8{5
zqj=UQ5n2`X`RuscaQH(H7<s8TtTo+6mj4#v?(CjCCB+i0BR#RkG=|d+BzMi+9G*E{
zM|jhGjfUu_3d61s2d@ni*iOCzpG424`GvL+^Ro#0jp@afN@o13ELAo?qc`Vl93|lT
zg+jj{?ozkxG05~j0(l*DcxA;E9yckDosFK+?W`=pH&h>H&$$Q+Y0hka_A@m0XIQ6E
z12uQHL9LMicbt4siW0=Jxs?h&KjP27O*F)dA&QiGak9`)%1ds1?L+DxvLHIj3bwZ$
zgXRSXG1Kh@_)XSBi;tgScPCYjIxl%`X0POu>=p23)_n>%wg@g39fR`1v!r_6i>H^k
zl9-n(e_3CEa|>-S+wOy)J};e@Z6C$P>3iipyLOaC&0Yp$x?UC+wkq@e{ypW@pEqG|
zRX@`5v%n8lOX$U^8ZrBz3s#qUV%g8Rpu1k1ij&fXZfDjE(Yvw$HKiQl8S6#xGS{8G
zKLf{BbmorJzEH=+Zmcm~8zW{;1pTWIp>sPwRLYdo7onO?rxnQ_ckCitd`64y=9a>l
zsU`gRO1gM6sw0n_5sumM9vo?Djn}Vt;Gt&sgfU%OWTm5iK#=4LYm2H7Qti%*)0Em{
z7FCLydT8T=uSW%=19Le3P6(7K?V<3&Dt!KlHx4qpB-U)~OrvjqgUou#hqrjMxOBiO
z4EIT){5M6oV*6TB9KW2E{<%`@V^6j;F@+@K;aGBGyf7|*i!i@W1n*zEi2htX#F?qq
zvbF|YQg%wN&CIjo**lc+-EFlK7Y5#=(Uo24OR1c?Op@})BdYPI9?^g;tzh$GnArH$
zTFT?rk&}%l*M73${B>she(wX}d57g_w7HFzsJ?<J^`9wr-DU7o*ap2u-NRAa+v7IN
zN=_Vz_{Q)AKO9&o&eXVvF*)k&u)P_wzBR(t_#uMP+c9E6<Vi>$=*5liR`am;yJ=i`
zC}?Nf@%hK?aP=Nb&cD=&>isNW-<5D2)TfTl3?E7{c4n-}c~qBs3<fpr#mRb7*YnRf
z9P4`o)@zT#y&ApIbp;7!p&6KTKaV@s_rR>uyJYM7P0Y4_A?#lLLUgWM11~=JrsUp>
zQ2WM4NclVxFJ-6TyXPjNPN!(mu9w6Tnw+7>YmN$2zA%i`OA=R)=m+JJGid2NUmU&B
z4qNLN!yhTXap3L-o_PNxv_3rs&LwBW&YRbRD0N63{#V5E>L*mz_lCG+)KB`7a*SNQ
zKLqQ*CF~_1iLMfZeQMSyHjK-}2A_PF>oXq<9iOywEgSP4ZkqMO;ai*F_`7yIFWi~S
zf6oPLF_bl4GxUG@1jY_;6AiX>$4QR1oENCaWGT5=`>4^+VomY9wJR2#9mzk}nv<hd
z55E7{lwGYi!IWbTbYanLO8o9dB~#aN`Q}~V>#q(^a_+#_b@qtwl=x}j1h{!t85ET6
z;XaM^IB~G1>x48HRGY0V)tmdkd|5yGxbh^~cPtkRWj|=^@;R*X{ebY`4bd$ZGa+$P
z3KiRCLS&sQOc52({6iIP^+Gx_T5^9mE0gSKC1snB!Fd-Rir(4Fu-S1D+{kPJw?~Ij
zGou0aSg1j_>&p1LUn94!Eft&sw0YBm1#o|ql>LST&>gq{jvFnZz<`UQ5Ufq7Cc5%I
z$rBN6V+p!j9OS_Z+Clx<^I}uDB{{TcbH7an`03nPIDPx1xVcu^zxPn5vY&^zyQ&TS
zwLT&K>NA78KJA6gTLWQ7cmoW2YXnX874XmP4*mL9Odg?2g*$iD@Nr8yc{{Ae<zGhO
zz4x)Aw&x~n**>2(N-W9KL5Aq&nGDr_n{nUsSlAv^2Ui{?ih~W0(*<7@%zSZCeE6#Y
zI=R_$W&UC`H<dcq&Pn(9!MAYuN`3CW#vac+%)qk91azzEjM4eZ__%f!TvY4DXDA&H
z?v>7GPj*m5_xr+v=bPw(k{>Dd^~J?=9>DGSn($k{xA^1wL0Y}&l#qNXRCInkmj;v#
z28+XgXzGEk-1gM~=6CaBoxGl$P*5U7TdA?Hq74Zb<_l^&dg8JPLF}z`1atJX_*{K5
zotc#m(?6~uqgGwK)m#W|i^t$leLs5PmM!--SuE_&|4XO(I#ajbYsH#P&*WJ>R8Zqy
zBkXG%gL+0%hT_LTF!}AlP752w<?UtsK;m`wTgYk5UqQCQyE|%L%tqh32KZv<4pF<8
z(w3QVyyM<qx-eojU$ac-qixyjpShGze7;866E?`>t$krfS)}k$+fSSqYscE>l1ayH
zBVBi3YSaA$UH<$f^%hm^>*!DQo*f|2=P;<ING_1BA+jNH(|O?bGc-&uf&;dEfi+dF
zeCY8X@X{*d&b_obH+T@M<Vl|S%9T93M1|yua&g!6e%Kmg3>n%<*llXI@cP*VOkTM|
z+}b@xe5NX$ze2~+#B3$D?3ay+bC==*D|L*Q+(3y9>NHm89=w0q4o`GV$BQ#&^XI)I
z@T|cJ8treuU5>s%({;LRIJyRGCjNi{|77etMHTjRSA)c)Ji$vva^CgT5hlBT6uynf
z;=XE{U^ZYmn_W^T(<`|Ixp&3SITN76t3+{l-wd1-x07xgg<!K@JJ#`R6XxV*(Ea@v
zVB^T4-2T}_T=jB`{Dnhz=(L~(hkxBftKLq)idg}qUVoH&^=`+reD0I=lxgCR?W(Nn
zYD_afrQmDHS9dbc6K%%XVdX6sE;qXa^>L4dPo))PS~U@_c3&X)wR{wcB7hBk)q&&k
zNnqGna++19;+OZXkQ#WOCM_I;3x5BSm(2M>Z@0}6CbmuGP>=TfPG!HK9qul-`_~b7
z?l1*I|2mhjb&JsP!%o3^zQjd&pNDBzl6cIzUVKEeQHc5(%PAzMrB{2<-;O`XU}r8A
z^xPxsqIN)DC+p6CRW8%jKF`3=(iAr&%$3ufJZx)R$1@JT09n})*?}QbXwZ*N)ZtDi
zey(N>eXbQihdniLKkO=0VkWQubDt~*IB{>&46<=<l2=(>qfS@uz*|KFTr^D!reV6U
z=JQ5=@1uQUefcT+uIwNTd$p5zPZT=H8|cdE1zeR=imS($LR5({W&P2jn%l{&JE$AK
zXNDKyeQ^1=w^X>{5N+sRO<{StLKl_ESUmp;bdF3B63jK(YI%UTtJ7Eb8aWd@r(Y3Q
zW=m&YnHG=R??CbL+v3Z)wm8+Km9KS-WmDsiWNvbVGxVCUR=Kb6{CF6RU71MxnvFSY
z=bze-gRD?@oIkc}x<o%lTEmeylDqCgqi~~nBzQ{K`7QP16k`*JQKzgqXkUK;t4vU#
z!icSt6!2NM@%S!Npb2ZAh_)}&VUI>Odqwxe;jdg!Ype@y%jhlJmSzL)aSFV+wn><D
zK~<<9&<~?mIC7xQ2eEib5V~kz5pnwqSQBQ2Jzs6%N&YM4E;jGMx+n_vDK^oUy`$0o
z)JfJY`U)O%(_y>9bI@L7&!;Skg(dd6{Nj&{rY+P&XJH3;PZ@wqE0*E51N$KK_%@i~
zbWpf)DUP+Ktb-deQ+R)5GKS9f!^lrzFw$=kDFt1mQ-74fb^A@hG<huz(flGrbn3;r
zLv{G(fX;aMa*gPo`<O=Lodczw3;6KHVt8};J*amcg>h0Q`?LF4ah|3gk5d@OHk+l{
zR9OMFjXY0%*N@}M9wB%pVVAJk+7d@!j>ClL8MuD)BzD~U3vZ;@afQ!%DtvJWnsOt#
zpQQ%P_%jqAdu)S|nt#OPkyk+bh!(xxuu%-0)*$>`9VBM-Pv%FX^;sU~1q&zpFue;x
zqn1c+SCt37jTi9Hx<vGf>;evD-@&p&D2DfMqqdwjxPGlYsa#K|Wp8RhXX^l-wo_uo
z+Ie#J^%~G$kjM+vr@@tfdE7ZjfPzEDyu$bxy~zGRf|EWr4A0;S9es2PPNi-^3LN7w
z5=$G$fE242Hcxvc#H!g*o!%Kbs;5c%E=ABRd3_v|Dq!O86tvzbqYw4w=o4;1wRMX)
zG9eNzhA6V=x(x25{gb_zt#x8a*$vS2b>Un;Rg9hKfYU<~Y0c{bb{zFn=vORter76C
zU59YgJaLefmfFIQb|cwuV5Xq#+nW@nzMq@Je$(f82Rt$`pGsfmz}%HakY0a<wlprn
z!^@A${+a<A-_%2=x`Sl%d_6q=y&h`IEa+udbv#zxDmK0`CvA@*^533mD7+a$#wOdb
z@<TW@&9US*!v_%UZNu+8lrVL#4LToTyuRii{Op*4%N^&D;XqUV9jPkYd_|Mj4ADnb
z|2eR8O%WWn+d};s*FpPwXNj|04m%}xR>0}obTE1V1Zbzh$19~AJ9ZHM`nQuBnkI@@
zT;1?Q?@jRS?p|ysIl=lGti}<Sq-Uv`K9xW7!=q9bU`fXjaA)=q3_XxXu9KsvtE@LH
zU6Kx6zva@Nh#1(K)=JqGZ|Fw93}|&y!TYDxapC#~{5oSD+^>I*=LczEU&TOdzq2#M
znH7-P=2{;AWHq@e@8qEUCg^|Kn4G+}LUu%7!FG5LepauJUNQaX$I_qB_t-Az_Iv_p
zrA{E5Y6D?!$b#cnPRDTkzFn|E{Se)%D1gmQ)3Dew2_97?;D)JtL>1hE2lkF5ufh_j
zl{`Zeue#CaMK@Xe`v7c?juod$*|=YSgYZq!6(K3-s*rMZG?tD|qqAE#)9cq=rT17j
z-n-`sbb2wCQZMfz$>D=N4%o7rSt1U8ehst=lj!)XqtN<!0Be3<Pp@JQfe`5j3O5&G
z{dF^v{b-i;x>(60q)vj#mo?eA{HK&>J`6?ngQdJ{j(GljIF72FEi`77;fcr*lBduf
zZ}w;>Wrt3~(9nm_B%Ox>&PCve<LkvsUP(CO&=yX=JDT1Ets-}yqukUQk1ms2Y5M1j
zE)6SpLuj}0LSOBvIQepKoV#%|jvjJLywTc1dIgsF#MF<YhZW+RUIXxe^%mIs?Js0~
z&Ve;)`8+lI2Gu^f%&+^6<mwN1=*m$=R?JRjn=ZxN!Qly)9!TZso92Ocmrk5hwMJN1
zs!Rjjrt@OSDW2Yx4%fzrd~Q*VtRVIg-H+`qrv6UElI078g)Id_yNu~_zcm5q(4iUw
zI_2P-6-D5D<~6KN*~DE=SL2uDeA&v+(c+TLdbr&qi+{(tVX(qz{HJJ($;a2={I@qn
zO+TqCrOpm)7hgoDcSpgj`ZwJR*&#w@N4Bgs=6P1vAST8Tek9qm^ZPkCKWrk{g&DDq
zYZZQK?7<IeXYfDMpYlZCRs4Eq5SQUNjGa|WiM!g-0E6LtK-$GLnO|c*rwhN1cZMXl
zV#*lR0L_i*wBq=5zW+-Pe|DP0$AXTCQ`&MRuCxz-Y-toen<&ZE<3GwtRDtMx4RYdS
z`P*i15?(B%;<Fiad3^zk+j9jg@eQpW*nxkQ&%oE)efU>DH}TKNzi_tGZ*isnK%A1S
zhZBNUih0FxLi_iU>-G2^DDNDJ6Rtb~(~LVH*(gOzdwuA##+rTW9t)0-s(4}74!ru1
z0z@TPN!belpDb&G5TDyTWJm-ZZm#6rd3!)bR|n&a!=e7A<k#~vXRm(iXkSh$l*Xpu
z)&*9W*QEkV58Q^Jze_lGzBQWNIxISl{|)m~kHMC4jSyyT%x%NBg7v5pnQgTr*!?oV
zSG|hq)vW+nYc-p-bODd_SSZTM^s%sG3U2A!SD31jj5XQmLedjk`1s%|=>%(VL*FO#
z_D@$D;PQpa67;a#L-O9$ugB_lFNIT5|K6|fx2gGV1la|Q=k9?GGL0G&JlW5fH%o5F
z<&PYA{DcvFL(1hoE1gD0@07@;rvhsEuEKub_EP`5DVWrj!(mQ`_-!A1j*3;FgSz`c
zd2SXSnI-l4q;D3DU*yu$A4SwUuRp*0<;H7WjoG%g3QV#!Sp9M=h8){Uox-<pq?IbI
z7F@V7;@S{jpZm}@|Ef#ceJ9?!*@&)9)PioNO3?5j4&zmtWCzxKq)V!{obfD|>n&Az
z#lrjIsACo4BJW{nzh)Zz2`_}l*Lq;e$6)Byc`^LcGDBHbHg)wi=EGm_!|LFPy!nVZ
zws9Z)qdo;LP7NWe%bs{xZx4GmEx<k7!sMpgE<w)Ey;OR(4Nh$x%dsOSLT{5kh*hiM
z+=s2?xqm<QzbEH%XNl*Yn<vDsbr7$ddJVO!XLFBCQ!<&-NP8X<?duAnT%(X~jy(eb
zZ>LI`y<!~3MdHM5DRAn+0Re)0aqZP{XuGB>Op3W9B=ylFyIni!hm$7tiz|d4QvbwH
zqXr1-GXS+djfVveLvT^v2#i|T2esy<;PAEXnBlEPEBeZCr^KDl(X5nZt!Ti-7bZZT
z4Y9)Y&W>brc`v)}JS&?{LwNp=skr8yu5i!d9d&&25|r;sS-oRk@|4sh-14P6M7=%2
z`_d)<QTQDy4+?~g1t#RxA(sq(7V)g{=Fpa9g$`aj>ArF%tdRV(GGB9wypRKdhwM;r
z+=wT;&XV6XJ#f5dKo(3M^FWN8a!*zj-5+o2?F8j6=`iJQJQmjZV({4Qv}KJMhI;GZ
zW6xM_j@t`0_x&llGLt=8&k1i8H^Jj&9voZWpPKB=p;h$~X>fZ~8o3jKdz6sM{Og2+
zZjkD9Bb;<wf$x@jlb)?1|NZF4-5q`5=DmIx<YkW|CM0titOCDeACB*lD<luK!aHT9
zvaG44ykSz4@Wwnu@<MNd+Rv+C_I3-{wZjmF=t=m&JOL*@YlkY1A4#KT6?>lgN~hf~
z!l2LgxasT#e7|To9B7_id&hP<bWm7Bo|fCt@Q^l3GCf=yX3N_inz2R~EeyM3!EqP7
z`1<i|^yKp#^WnMh(9r^ywkvjCsA~oV-yE@Og5+>^`z0vsnvMy9gK+t=GuS@$9F2>-
z2zQ$@`S|dc;BmbWat^40N^u5{eX*04k2uS%`|UwD?JbpNn?t*>P5fRMgWWuuX<XA%
zeC_dDn5L%BnO}QzS$MN({>YuO&Xh}hz%QCUv6Mx%Bwp2M&c0gb#1F@BzySB|_&>Jb
zx^(@2H~t4{LIZUPG=!&bBDtW8CBI+xQLO)0Mx&1yaHl$T=y`S%AGvoP{NxH~s46)@
zqmQ7$qblg;z75<i{S*UtuLaB6IC>wl0*&%oh4miN?@`)?dzb0+yTSRK5w(@~Doo|n
zvpO_$?N09DDZSG>1hI1EG^q=?1Qec{i!Ln-#eS}*>2R(iHFsLa9S*$*-;e)jRn;ZI
zBl!qCrt7rtTa?h-+@0=bU7|@{EYRRi2duiAk7GV}#n0G|-cCtot&kFan}qy#rk7mj
zYXd1d*<qriH!G_577aTFU`E|cP;1?flipXtMEgvzvk#`6R1@%$rNfW!QeTPg5An_?
zPhJ>#f?upv;OeO^Bp$p=_E3UKMc1iurK;3x*erIJW|46{Z82P}SpMg*71qfv)2+tk
z^l-vp3hnw&dd?S%{HBGfpex@>?!f!5tH9BFqu9K*MmB$P8o||$;&uBREPStx#}`Im
zMUokRd~=Vm$4NSQyo8o6P{vegCoGJeK;Js{#`5ww)Y>{4^yDQt*72Tfv*sASfAKKM
zey*n7+h+=E3rf)|(+F2|@W;}S4>a^*seDavJQ>uSfxdC}7_KuD=Z-W4(aC}9?WCDx
zzi^Ir`b71?Dmc5~2p6t7PaYFT(%ynd-f(&Z><@h>fAV8B<wW-2*XPpV+3xB5MRhkO
ztcwFh`)e2wJsZ4Brip*cPl~<Lwb_1k22_+~3A<A3#l7xET(f%}70!tgsx?~Rk*hk!
z6s{C{AG}Y_$JDuXk_Ak?*h@^htA;jy+a<4a1AR5u!qqKRB(K{7-r0>XD>X~j>9Db=
z(Y##jd$5=$3K=})eh19;-@*eY_vg>uvgqqUIUiT;Cf<1Ik4E{UK%-w6y6<@m=AX*N
z+q<vI78X2&@Bt^NAFLMohu8>ZzM7zN%?AHiPND^uqDW&v4!_k3V4WSCr8%*jW;|O8
z`!g>HE)6c^p__rm0XE#>@)a236)jwP9>Q)GDY!ZAtMFKPz391cHmD}5afjA$`q5y@
zU+;9J{#O3{!$yOf^Bg7D?QppM#~mUkPUMtJ{rT%wr4xQNzXmTD*Cq^^;KP581wmMc
zA#~h+8||{#PZvVdu=Mg>b{w!*x;Mq(Tb-LUzk31l_SJkcwi8>|8=MI8dJQg1WUP8O
zpAJm$N5h-;tn3^EZv*Cm!j)Lc?DaujR(A_@`}9LPorQHP6JXdk7uf$hh&t!ofG7St
zuw}&|{JVEK?sS<#<JA_+vWlu`(pOXJ`6~?PcRfZEc5339b`vP%LKc2mFov!C0#RP%
z%njZSbZSr<L@6i3*rAb>F+ZG^hT8D`h1<#b&NzT+s%Rh417}`Ifx_@&_*!`b!X9@L
zt|VRqpH@H8ujq-vD>u@N>T~#KRg3WCsR&W?kXIELQ02HJuKyAN!AnP=|E(Q7c=1Be
zxVnT5^(S%rDZNp)^E9x_dvN{Wj4va);pZU(aisKIX}u<OlP=Z5_W=Ca%>!3`=}0eH
zdSm9|bHK$TVNuCg{BAAdnV|wU#r5R8_C2^o%3^7Cz9efJ5`}jX67laG6%MHJh0{l*
zIh5NoSuFMwo6-+ZM_Cx`f7(o+d{@z%*b-sgl#A@~Cyr*BlygANc-rwe3I3F|(PHN_
z5ckEOx0@}bvnvwCi(1DZG;0G59qEi`lHGWi`&(N0;sbP*oW~(HvvKnHR+t*!CN4da
z0VD2~(apvb2;X%{Jhgr_ESJrqHFlAFURDYh8Y{Saw7JADsB_<+`snfc3{<&}!s{w7
zxYqrKP;l}98QpagVz-;}?AvF=$Twd2W$qqc-Xe89zGxAsUKKMWex-G4DxPo-=Zw9!
zIC5wf9PVkzOW$pU&V!!OP_sO)|8@#&lrtzT)Eg)xi#oNerh<0w$*;Qz79X$B#)1CG
zyR6tGy-Cblo+^HRI*s22Z{{Uc7CdN1A<yhPUbNHbBK2K;lja0#q0Iae1>X<EWiQuq
zQ=eC`XGL%Fu%E#G-M3JW9%<;5q|eK$9|AoH!b2q~{DhoQv-&w~uk$2>pkPofsREVI
zOO!W-sn%)|ji`-cef5Qc_pBQbu(Stu?<m0NmkFe=_msAs(cwMCw)m&%IECC{i5GF<
z>icQD-0lVJcj`&g17E?%j}<Vr!*cSL-jnYn?U!~Xmms~5p=jFSwroq?W%;QSYsFn^
zJ8Ne?+5^3g$8++4F7PitlrKdLh50sNtShlQP5NWF?9?Vuar-Hc3)AFMr_<7z(FG2U
zoQ2v87Gl>|=`g3qB+z+gz&_IsQoyS;_?5YvYM*J~*|~|dd|DMbO*7$ZTaMD?Yoj>X
zvQ{=e#ssZoB2_G!j+;%Uk^Y}bvV4CEG%hv>*JhUpf4xO;Y#NBWXZIF!S{C4%J3UDM
zuP2|q@mh?(`H-3{7otr{D&AKqhO^f@V(X(o_DLNIm8*uM)$>;{)JN*#QCJV(j<-?&
zrB&3~KLDJ!{ubZKJaDb@dD!4`9X@~kKvQoWqw^QqbAx{rpFfgIziJdY&LDy3ytpN(
zP2Nsv(mUzy1vzkL;7@6<n*|fj+Ed*4BAn|q5%2mOKu@Pxn7igO%vkJA-gjNuNO7+C
zeX%AhJA{Bu;x(c9bPlcixr83xn9NU}k3so?1PVSDg3fynL&nbr>NwquU4D1RVOI*d
zdXhStmT0kM=l8U+^IgGerylwqO%o$*Bf#vmAs-s1B(#lj6XryC;H4!uVayzVq0R4-
zOTigM_6#x?Pb_2_EWNjk+o^>X=WB)OJEhECjT^YVFJuMxGt{Yr9-FTjfeO|pxZ&(7
zZ0=l7{&j)E*e^bG^P)ajDM|VK)ooNT#v5P$?!qN<dmP)NUby|!1(I({-2sx1;T1Y^
z<oR`?(vN%^7=BiOkcm>?Ts$aw?0{^~VOTQvnsA|eUrcpBLG~k`(v7l{FeGUN?*vC2
zarGOWSdvJ4gFv?D!&mCOWfkh{UxNjm0uUmc@z3ufSavfTN{=svocG&gzw1qD(7dIB
zt;II5c!cETRPB<aJ&K-uw&V}?c6jC4W(*F~z{dCMFmIkM=q&$EjbYMxtmPlfKRF2J
zKKTZ9Gt1~xr2=Sp-z44Oa4hm;dNF4-y%;_a=f~*71XX`5pHhUhTn&D7Tfp&?t?_c@
zDju1q2nSVE@Zn`sVR%3&DeB(>!@&Zt9O%w^QN_G!*KDEkeH5E*RY#xLQK<Mhif>z2
zLe!vFqE^aV@?0wMAOSZiLPeRoJC9<G@A=|;FBzV1m0ZT}vN1;M5jgx_%k6$!<H;pk
zIP1^^%KdHuJ-j^m$cDkFI^2%?kIjc(0kdFtR*qm?xC*{Yoyh?|UQ&sAtJq=oK59);
zpt`JZ{$+j~HXPC8Gf&*fbi+EV`x7tT?%E8li`P&eR~@zuIzYEQ28eqXk3g_=q1a`b
z(s%A5jOe6|aU0E0eZqRwop=}CUrWVqI+joryc%!yX{HH*m&BC0shsC$51ZF#a{9ar
zXd>-gHsNZrs(wfg=5J|)8N<tQ#dy_LAF?jY8S<?yo;>|$aC6Hanl!D2l-oB`Y~pQt
zy&w^sOa^fL@<VX)W&&3_^br;8t;IJF2T}CKO|oCcKoJo#{wnpCP96GP^jfBly^K0=
z+`nkS_iPI5G$v7riwAU8J1tI?=244#ufRoHI$+HH4R|Ian0K_w>Cu!Bj-TSfl7bl=
z23-`gnw8k>@l<TG?9V3Bo;F46ye#481(=aq%(vIJBYm@E$@dXY2h%%2qgoE>E}cV*
znukH32L{+Fp_8DIa~=)8ghIksHLPhJhZkLg$#@5WU0hGn_bdlzg&dMcdWzr2--bJ3
zQU{A~8E$HA&nd6+;EZcF)Ks0L(4?18a`7%49jr=ok7~lV5>1x(eJ_U0&!xfzi@`|k
z0nIiJ;>!bG!2p>w3)2k5@RO_I#kj$|@vH$ph<`%aojowLk20L!kPKaeM)T`H5ZvIr
z&_&x9vz|&_yXBrVyMDgx%<esuY8xmR*m&Ww5_7OWX$$?-j{~XeV`8U|aCw9-gr7E}
z;l8&yVO4j2rQ%4HVMJ(B3@xv;@o?)drg^zgX&1*wx3uT1rGt6bqC!Z1+mRv{EQg1>
zOK`N@jUz^7L+FM8;JY38--8vXcv|46T}{C3*GR#1wGpi#2h4BxK<M`JC8<2gg`d(V
z7D~oge8?Vmml)%#v{UpS|NkHV{~!PVAOHU!|NkHV{~!PVAOHU!|NkHV{~!PV|A+to
z-}`@`s~v?QwOVW;<ny?@b?h|u3%T!mDmHCXz}P_xB)^6V8-*T|r+%&CIE8`GMdI1K
zGw#qSCmFT{Jrs{_6Y$PaW75$Mhf6_ssrWhIOA}ov_>>Gg8&YZel_aW;?}Im*H-lp2
zTD%)$1<Gl=I7|Dwc<H+;d(68=j>B%E(Nhf`+*%2r#RV{OaVFWN<ifxn+v(7;2UMb6
z&6~7zxNUAD^>QhOm-b`u$m5xK4g=vNABTxA3dvA+m~`GzrC#5HF(rM6tkUzPXe0Tp
zN~)USl#wg{c<qcae?Nf7i#1f^^coIyd2(#0u$=DOYhZ#w7%o&uhh3^`rG6!;pRF*L
zM%$Fpb=&Wvf4epk&eg&D)C_+6uR+{h6vcjyYUrJ>#_P@<M5D(69MqO1*!5q^=Kp$f
z$iqDpb>=C|`{jo%J~_g|gVNczYc1XFkVY|HljZNP-4(mGP3DOODl}|*6*Vi*<aUv5
zVBhM<i_Ux?!~0ih`6dgxJh6}(J>Ek>OcAwBUM%Rle5JE%ztG5<Lei;AV5{|!QXXhC
zHEfuUg<j$GHtY*5_t)k(hUe*R_j};;zJ%so=|CH^Jm81UE^4lwgYQBbA=<(MvrJ!7
z!q_xA^Eg*tV|5w2^bCc&OI-0)+dBHac8WZ~aTi*NF=RMsH6Gc1ndY7EM=zXSQn85%
zuGH)dp32eiHPH@Th=i$k+X-V<2Vm5CHFExyEGS<~#|Kx9c>bvCaOFoXblrZIvMsxy
znaM>+`ShCn$IqqL(jGM?I8l@hEQefGC-`2JOy83S^0g#=dSN^R`y?L|dP(~f4EH3b
z#`Ziw@~jTO6pG8Lbfy1NGHe>Vg(lC+;v0vbi_gsRg<HY892im{&TlvhpBqNdw<2Xh
z;jbOqzFWrp;2Nk$FGJn?yXarPe2iF=Puer}`EkiqJR101Ox&h|Cp;euvkmk)XoQqI
zv(5#xd~*!E-JR-(xk~(LCWlXaO4U`bL2;AhpMDyRu2)Ua(^iXYi;`%56475%Gb;OV
zSlF@eEfwFGjJeaj_)*Y0>~i6z@O}PNEWA}CzRPh2v$9JRx9<cM|5L%;mo!o7NE-Dh
zt-<hRM`+c!^KhhJ20peqL~6~iY2wu3wCr{<|1ZB?Q(uL9%O6wS963%KwwgSp9o>gn
z4y@AQB|SBpLgNOxgSGE)aq@mMh!}BBP;XmBt889@_F#YdE}zKGo9Dx~S9j@9a~>og
zn~wp~Y+}K=Xu7yFA7{9Hmq)Ejgc1`2^l+-7@PQ}9S=I+hN&O2b+2%vVx(4xXhh69>
z<pcayn&K|I3^*+1u`)hJapY@D%JH;f!@mYNDtaln<PE_=gLlxqQ`K<$MSraBnhEo)
z7o*eGHZZ8zE_mHk#!ZV)(DH#PIBCW#&gvLM$IHuV!?ZO|?0BjVm!|%r=OfhU{`m+r
z@g0n(caLK;WmnRxD&YwCJo;GjT!1b^(Qvo{829#;?|h=lO9LC_S9Q7xKkfDlhrTU?
z%B(Bm!*NsT>l_sj&%T8%8F5_ywhi=mWwQ8ozBtrRSMo}1pu`=cIn5}W)t77+RJ?lA
z=aSA?k=PmCR!Mmr&+C*H*es}hQ<l~HE#mk{$zdtE{W=~F;kRRVg5}oH_@rM0g_k4>
zr<b3H?LI}+aZ(B;7wpBcZ_Rmb$xvLTsEUUlm5VW>9B`|{Aea%h5?h}o3WMSw$tr!5
zNb_R|cX_*yI;)+5PqEX%qhv8uC@c~q<*VTQnL0XO8Hz5Ew~lwbu#iUgGUoy{bJ0V;
zAAOyC7k+4HV75XRmv$GlxQ~4ooYi|UWj!1~dA}}zL+=56)LxSur)pr_-k}&fO%I=5
zord}uy|GIAFZKE`6ycWxN2N=d90h=5M~A{ah4$FRC;=6w1i|%R{-FEsB%L2Ifv2V0
zu}hqs@u~;edByXHJU=+BIvW0bxkA^piXbj-8|}E52=}Z9VM+fkxT)`IF}}-s_%b_z
zS9MYs-46_+mcwaa<X6TyEv}fXYH;HEF;CpIJydj5$$-+DEZ8Y!ux{C<bHvK+FtT$i
z4QM#bm-TH$J)J+a;;cSy)ZK?^OZ(E0P8zVO<rN2>G2u(LE8v(zd;WcSIZisE$RQ>D
zMceRR7^D}0UsG-frU$OkKhIPOl=d8t{`sIvs0bS#<k6k+A*}r&oRq}j@N|mg!2R|E
zRMSTDBmeEJd8I#ne^m(k9wy@KtL^a^71MrvC+s<wMvs@p<BRMK5Oj0`fBH9+UVb0U
zIRm%CYuj$vaLOB%p1r2}ye&d-VInDxNukS%`-L6@KM8yK6;f5bJ^vjfF^xAXn5*4T
zD^3l=Hhh4dQxD=erNhwa@37kSQ<i|@ibBE_?ZAAb8z_E!B$us!ET-Hq7sGFA<4bL%
zwkgTjy8k&W_~gOm--FP`doG{8d{G#emV$OZ#`KN7QC!qd7~bhJ(9~4O{JITHTkgY-
zS#N0Y(LkYMv?Z?VJsITQUYIh=hWxLE@%F`%&->bbs#<6*?GGkl5BWiIk#drI+?UBe
znv6uHGrkzpm?Sr?kvP~#OT;OUtsr&gJE(CwC&Ubyjiq1bvgynmVc5F>rp(jqli!{l
zCQH4KaR*?tvK^*(cEqRol|r16F7F7l$7NEU?bN#fOfAdA+s#3=tighmTK>|J&Bw?`
z{z@1U)CNA9_LyP+T6V3yE$(Y=5)>?o@QBnW-f_zkENIM!D;f2)DtM#lu55$*`(1!}
z?-J2k20~T+0@gFx2YD7+yyrzc#-?RJQA&#VzGhBs1Sye~M=~s)Gz#A-4<y}~B0+36
zBEd_O)SJ4Y<~j%57tki_A^Qw<i)~qVkB%@$HlG4Y!-ea8ZjtGj?y_pJfF~-BkcU<&
zu-Y3R_`PipOqiVkyNXxSc;An7Wl{sVPaD7!UYF6EL<{UP^#}dvz7oyfrHHGFr%~TY
z18Kol9co_Oo2PixgV};^JUz>h6WZ$Ng~>dA)yqM!k@g}(f=ys|`7x=-y+UyO_KJJQ
ze}(}Y#=)D>9k9K@By7KLJ!LzbkOxZL#fSQI7WT|fAg&)Nj{SB<2pPQwHr92M*uyyN
zCpl_n&p!dHe!Fp$&Or$L<3rDDhu~Vt%P>GwRalsCp7u>}2E7<fzGyfMdmB8ZnfJTn
zNtUs3ek1hIN`lQ+Q>m)>B24joA$3_ckW!uon@C>J_NfQB&5S5DHkET`yd#UiK!~^A
zOE&9m0K<2SnvStF>B2c^_ij3zQCtfuznbCRts)^+uTlJIVouBZe<zQMLV6s0pDHk!
zTG5hi)~;lg91HF}CX4T1ehVWWoT6{i_wXMZO6}cJgqYFRg8uPtsPbzgR5~5TPqljZ
zW<U%@;}9^631TCajl932l5|h!am>2CbV0$9d#DG(fFtg#u*O+1mO7MoJ159fga~@D
zPwMLbyGnE!(Lx3WtMH410&5l(LxAUJF*Hy|R9!WdhmQCG>b+J`Y1kO<p^zZ%O^D#6
zy5r)Yla?4;_7wEym4jiu8TU*X!3$<(@{KGvo_PH!otHBED&<dv)fIhlr|KnP*^_ZN
z_E$b{&aDUWy&11lzYh6RGr@E7HZrq!=ReoO`NRw}t`3<AW7gR6z<XX8Y$@Ze$@j&*
z#*2lkI`?Vt)b?E0<V!d84hoq?#++n$47az`QEm1bY;@>`eI;hK(i~y3-eK9Tq=~#e
zISG_m@<L4c?9xA~Ui81I!p{?)LEw}H*rqJ8IkoG=#oqmdbDPfbQ;GlonX7{LZoA-i
zBR33*M~Rm?15zsk=#Hz#mVyW|!6FN6^v(Fmr{S#ZIfO>5>G2wAhq7DjLghK#CC=?H
zG^r1h+;77n^4&q&eeyNU8TU)}CVvn;Ib8sS`ZF;_epYyql7=DCqw)XPJMV`c|L^}J
zD@uh_5*3L;Nu}|;&S|JrNKq7p$lfC}+EN;lG%3lBl<}gjb0VcAkxh~Y*~(t|T;IRr
z`~KYzo?YwlIFI}NcJuc-R^)baKX$d=ARg#(Oi+zzk#+NKrvxJna>ar8@k#_XN2*Hs
zk+rC4COOmuL#{6=6vyv>OEYJ5rJtp%!Sv1qE?DhDdRCQ!s{M2L{I(a?*VN&+Zp$PV
ziUI!h+l;N%gV?}qJ$7BYM;st^ho7lGCuNEIzpHE|>uQ~ceH)5Jh3pg>W84`g`3&Q9
z_wPcVvk!#MmmHyDbGxvq`2qE~Avwepa_FJe5*S}~2K>gD^I3z%px<`OO5?Vhl=qCr
zFE_OyTP0MOn&Bw9QKs@{zqeF7_O&2iY=mDkx?sh+4C+~QoE~Lt=hm%TXoZyF$Sc$%
zhs<KCn(oS}s0e*+UeoO3#&{v%vv|ABm1cDG0LKn<s=V4C-+%cbi<+iHy_Kp&{m-}Q
z-10EAUos0chDKm%MIRVZHIkRMW{5-gAd4Tnz|Z@maq7&O*!O-I=08io7`@}d6zGEI
z0u}gVrx&vJpOz%&U?9vquPg3e5RATAiX5~y4Lh>bcwDEU(9z`!(ciz2Wg10tsR!Zl
z;}Y5Mz~^-LpBrah-v(+bgZWUPjHgXn26ZQ|vA$Of_gwn3_}LB_^ODJ8^Ls^fIq*ZQ
zZXF_dy29|c?>6#$Jr;Gko&=qv6F?<Vm+!Ttf$<bInfisR#ft_Hh7P@p!iJzYSeN$}
zHrDP&VMQSr#x&4RjW96ENMZHTlVlg|OW9EyxWvDjYEH!p2e&uUq`_VA)|f3c(VuAP
z5^wrZ-UMa6l*NiH1)ONF&w(ex`Q5pF?3fiwL5UV@@yiph&mAtlihNu=@Ou$>O?KqQ
zfAPXQ^+cZW!;ocBN;ENmB>uy0+;Pknzho54%9qCjJiH<f+%b{+cUvv(>-Guv8nnrM
zb}YW$ahvAg0dW4OkCE#)a>bNW!h-`V=yCdTF)K|6%`B_n@zpgPsuTzMm3~}F{g5Qq
z(~pzW$aB3+FgR$%ixv)`f3eCi?@%P<pVQ^C&PxQfv76W=b0x;+9h00uG7SAafqz=4
zv2)f4e4`M<il$9yb~l?h-qHhybv2}#sQ{DqPDCHu-T2W}0X+>qg6q*t>Ul|yLkz!I
zf2zJFb^8$0Hu~V%x!qZ(M+n}N<?{8jTZC>8Jz;ZorTD`49Bdg91owV-V}}{>!eK>S
zvN<uF$Hn-8^}TI0|Ah)?Yd?i8y>_ym-C$@qvQC^;8iJn;`iYbGAhri6@-8uqt4tq5
z(c6dOQS-Y{o7e&OZeFFZa39`zsF2caPtpNLE%?%IiDu5LFnQfAcy=uo`e7J2s*WT5
z!eRI+#TRomhhqQI&#*|d4|?p4qt)m7<K(81g8UeDco$}hOP5Z9XBMuwb(<@=S4EN0
zuS&W#KpI9UB^R%EuL6BJOIn_>fxT?Qc!8rlj~afQ6u0J6*G>{I{XsAX4b7IgUiILt
zS|;<?@W2s{GP3_tO4Sb0Sm3dd{?wk9Dg0ao`>wfibmdE#oMPhTG|Ltk@Y{zX7o0_V
z%`L@4q<xM5t3oQ4P2s<F0_zp)N}1(xpztT16Mm_ZL(x7Slwbl=Z|srPm^)zDwZ3rI
z=?1w^(!j;~1!OXIGj7pe1z7`|;7ZOVI38(9E&ZLr^!{XSf8Itr_tX~~Yf7H;BZ^qA
zXUMkYhv;iU51#d1`aYN}!yVmD@(Sr4)jDV?cU<1Vs#`kYvp>3&Y!M@!v4?T<!$8tH
zlTQaKWw7i;9E|jDl$-)gkGvzuOs+=wwO<>TCI6Lq1}J0R<k!OKg9G^eP783pmd0l~
zQYa*LBz*ehiYIdg+~zj|b)<Zn5cG<EwGHGG##?cn>RYLIeh$~V74o8`Uue|)3v5=_
zm8~uAlO5~f#!X5^o!*avGhHN1)06A6c)L{L$Osvpd1yz2v)W~a&0Q&3_deAHrc?f}
zL~;|X>GyUEuC`f#k=bv=UfO!N<@qNVyPfE){b6W08bXG+5URen(N!fmvE0RoHRc52
zdY`o7|4v-PnyzbPx`Tp*ik(mBXQUnZPAnCAHq3)AFT!x+H7Vb@r6>An4d>|&N^t$j
zY<hboA45v5z{)TR&z$rJZ-Yqb8S;YEg7x^)&j)(npCl&FM`~)_OSg}hQ@xK1E@?K#
z&(eEt`0Xut=Di}FslH6V^%y@&y})7WP7D?A#lpvGoHx9#$f?jBub=SX+ycp+VDHJk
zDdED8$3Ke0PKDxs+uZQW!Ch4FaXPq~rOJ|PBSf_`2Po^iGw8${^QR3y>{Go0&MnUp
zEGG|O^{5&6pZ8q;tQ*2FE)4{y;33e8GvU*S1XRgYK`U!RzE_tA%5r@$TVV^Y{F{s8
z-i+nvi{4OTg&a<|tP?-yG{J~Vy~%8RQt`eXnOuB!AU(OF&o3A2@}9vR^dvu+_DmSc
zo9A`KNlh<`lYVufo`p8-d>`m_$O+DqRt3%1|4|ozdyG2#g*NPrgEdMgsJwA3I~0F}
z>i7O|)}|}om^BeEw`Gg<=YVS?uCUy>@9=f^1-3U#!+Dinps%LHcN?$Kc&P_0yQa>1
z-K6jP^;FonejWdul0a=eS3&8(i^VVH&eN&uM}<F{$>6WmNS_n8p~B8^ShZ#tU0J-0
zN)xJtK;v$#TJ2dpSIQ7RT0TvjVJt^%qD|Wjis_@2U%VeOnnsUSg=9xl=7^QRS~9kD
z3g<*46H#ktC|+4K3*SsUi?eoX;jfZL7^ixQGW!n3<yY$IQCcu<JC#Kb8h45}PP@{E
zwkzZ@Xsk#}g1PpZWRNOja9pQ@{*f)hbQ;db&z6Js52@a(SB=}BrI5OEod7S(S>eS-
zSev83>*aQ$RnIJ$ty(o@)E}cBU$#(nxhD9hji*rE9Xw{Wz(Ha%rw$LtWzX(jetT{i
zde*F^#!Jizz1rwg!U|p-m%vr&^-#EBh}1hjcj@5%jaV|g4jfk1l38FOoQuunNoCVH
z=!PQHT8yDPO~>KvOgWkumn;k^GvVn&q_ykgxj12=BK|Oz!|cAwaOuw)e&QbttL~{l
z#AY+h?qMrA5~Nwe?ikKZn2(lcWW}PdH_9D(NE=}l&03Ph#@ALONx9PFbP5-IP@+z?
zM$l!VAjX?W4)tC4V9uy|Xv#Ya9<f@m=6Ic8rf!8*`QG4J@gCfg+{Gt%d|=$=NnGDh
zDyVETrmwM=;a&IrxFuU2zPg&Cvv(4wNgbPrK2vF5b0j@eUxk@{Ch?-lDzquMH;pgs
z%+Y(y@uzAf^)=fg7{ttjiSC=}UYFI_SocAEkPt>&uhhWNR||Qp+I-Z+&!WTAZ<Ma+
z0{c~5InZ4}Rw&;H{Z46-=fqQ#r(+|t`PWanUvE=Wu0GbjQ{lO{<nic!CGcJ}SWJ9g
zMlbd*WtUyXJYFG(`vzr*Ple7j3>D~>swbQ`iier<yWrFL-LR@&0fr5#q@=R>y#Cfr
z8tyfQ2mC4IFW<kD@=Ar0T;<zh&yS<HvuT3lw6etT`+p@io+1of?8zHnkL7P^bzuF)
zgXHtRh;qI~;w!)IywqSP?T|c6S$R?ibDl4od`_Vr?z*^k>tlN1p-3)$vmohi1bd1t
zbn9mnt~c%=LG3)QP^%UG9BGsF>li{ScPq2v+C1pt?IxBb=;4JhBTSqY0)tDwQfpQ`
zRaD8b-TpyrJV6^@-LZg*T63Ox@hIT^2tHYoBStg@iq}i;2)Fn5<(qE~iO#p*7WFN-
z2(kX@cq;9=aPU$&Y|%^QcYlWSrz5RW4}U2n?Rp2cQ*1D4jxlUc$fClIU&6vx3%XZz
zO?JTcKk%7l2wr9nMLp>Z!K-N}`NqAYXn!^SRQy}wl<XAi!&NZhp!7R?-WPj#_26>{
zHH4sJO8j+pZ>YKd8$N2e!Ncooq1P-`44ZO6Y`irQD}O%`o&1Jif5%MvSl=7Rj2p(e
z9vwp0b1!M;>R;3z9!}a<JfV+=HTAt53YMeua133byKV8fbFmHX4ERk4O=|_Menk-X
z;R2hiccUp6q)wi+H_p7V5+A<UjV2kAce(FC462r9ODV@-$jwZ+b;lVaw+^P;hU@9>
z@wp`T`VovX?M~P4KZMCX>x)0HlDHLZgJI5S0hUT$q2GEU1T5Jtw0yn`M~zNV`{fiN
z{KP8CIwi+GU0%q}ei;Ef#hYT1d^G*@9l+DNt8(AqTyfIkpW>!3e?&Pc&wW*U8P6E0
z45#lufjPN*q2lT?T=C^0rOwY5YUe()zT>+Kf+YXR*9Q(fLG<M<Cr5ICo7BnEy8&J1
zSCGqjbzU}JhchdFiEsKGmK-=a^j>!b`#Bfz-Rj5W@M%20b302*$PXVM(!lIkHO@Mu
zi)|yvll(_@tmw6YP4!dITww<{I<JDq$Hvm6Z>6M|tHV>}$$Ik67U9mfv+%`rAf8Dw
z$M{dJvO_M;&~NV*2$)_VxUV?_H=m@!3Xf+H@MIbJR$jxwSv?>o@(_;un+zFUKhwSm
zGXBuXisL>8@tc(X*r2-yx?~%neN-T>xBMVzs*mS4HGg5d*>*mqG6?^)48ksB)96aR
z36->#bLN^|oMchJdw-a-_+A;GZBgfv`$ni+`%ai;_8({Ow<q)Q#CDP%YVze|LC<6m
ze)n#I)wN}0sPG@GoD$6+R%cRr_793!<;LF^jANfxHJC780E?IgDSx{fuU<BSXMc~t
zE3<4ozrH6V9j=kZJ$^u)+&#IxzBlh^nM01Im*C=*L0mUvJbo7%#RX^QVnNzo!rhK|
zbj=6K)IA_9>rsyHdrX!bJ~24WXD27sY=V=EYh){3BGICz2gcqiphU|n(0^eNmpI*r
z!<%<e&G<g7Dkck}x<FGu+GFc=8@wT>%ooz9@G{#|yfe0*u4`Y1l8$@~l^=u(>IU%C
zWH(LzEaS}=ipY*ea;B9o_IZ}ePlJ>2Yxqc3?h*m-H;<<yCEc+8tp@j;ybLaGOq50V
zkKoIacdAr=B+B%cvwWBbj<FpptV+><gsZdgrNlk>JtqSSA1d*~=e9UPnnSc&y5POF
zi%8jgkYN9K2!ARJ$Bpql@L1Uu?3Z^D21w77hW11rP<I~tWc`(VXnVo={R;lD--y2?
z7Vw?5op{&)8IK?3%cUBj;)v*T#a?qBf#<$W;^)0<#M+CIZ2TjXw|<&~tpT>I9aD}b
zkJaJ&(e>bTOBF{Nn@h9Dr5LX?4ep)UfCGPhq*^;CSQF42-BO)s?l*06sxFhY^vYr1
z`6~1&`WI<5M^NDS9J)7c80}j+jTL-qAjNbWiCX*cw9P5%88Q@SKU~Wr+dl}QtNWnW
z%4V>gD7hQ`j*0t+1anl!0CtwRk-c&ciK`Bs#6M+eaQ~?@ES>ro-Zmt1kkw6aTs(nw
z+eSlo<6f}mV~NDQG{bGNgYi%OSD5=X#Ck*1WSCN$K;3^HvDW&v0KJ0@c>vZRDSRVE
zxe^%k_Yy?5*3-Z5eYv`5Jl@k~{w_I?Ca!an&5zaKwqK+1l6*Zlto%obCX2CgrUff$
z&*R)z8r*l+7#@1vlP4bfD7O86OM&_tRBUxjxHRDax?h+JdKXCaU2|MKx>vD8xnuww
zzj&Y8%O)cX52F_jws1Y9i3`&+XpLe7-IW-50pHEhrNa+O0>6m04-`2~{+sw~x)Dvy
z%oi#jbjG`b^~tOt1KPJv=j~Z;H2X*>Z+ciI?%mKtV>ji%rw~8RA;~}7w3@%^S)jG%
z3yFhbNZBiXfc*1N{?~9o%;T=SB+L}zFDcSWeH~W3(1*$0on`(zN#lmp#f#VsPqtox
zW!sgYRzrp7%&mcfmW34OH3lWeJ(+d>51whiDqc|i9$pR{0%Z#wanovVZjM_BvqCP=
zrNVOBX1NphOF!N+Ru!K_bmJce2kG#T!R)tBkqvT;#JjdviVH16p<VS9JPjTNk=f3y
z{4SLC?N(=_svwDzEsu{zJfy$UznSQ&E_Fo#V=m2yi%+{_-kHb6<%3;V<551mcZ#L!
zn`aBlM`&_iw|r`Ryp$tX*22+0UD4ciAvX2+3Ci;FJgsdW&h(nbrN1X(OM)q~$9Z<Y
z9s&jdCM<vT7dbB0g^PcdLG}4iHVOA3ul9CfZLK1|*RjLAS{v+it~=@`pFyqmLOeBF
z4VCJvVB5VT@WjiA4NDWT;&>L_XbHqkl3#n%oeR`<p*s$KsL40=QgBjFFLE%SC^?&u
zss;s%FNaJM0!MY@Vl#8dxsgJiNed~b)sd$O!|AJrHvfGk@t=YZ2_N=r0V%2Tq3S=R
z`uqtjcK%08x?aJ7C;H>?v<$&&T^>3a#K4aOl|ta!T)2}kj0f1+vFXZ2ne*@Ckl$?0
zV<xF%y3-E!Xjo7BH&4L&z$#K$+yV1UhH}`D0buPh7(YFX2ctE~=y|LKy0@Hzz+gEn
zwDZRimE~~P=OfrUw7|93on;et*Ta1+1(X!?*v$on-OeL$9sYvTO0P+=;Ij}rTvN!c
zI>oQt#^H9;S>)DhpVYfQ0TE3mnEcg`I&CmUy~cs!l0Sjed}KXoc3VpmS`4vaod$nM
zoQKV=z7h*#Cp>(x91IG)arn7`T%tOG-tC)7pL!V6i&weudqfbPn}44=51YszitJgZ
zQ#g0BucRYdp9sbcg4FMRynKwaF#C5V&NDeC&RJE2UwYI-=b9Q(bDj!*vm1iuPa{Ef
z`4CjHm+n);DHyJpLA&$rVcpYtxcBBPd^KY(rr2GO>C7Jt$@a5&!J=LCb$c*JG|KU=
zZ^t3d$QF0bZif|L;?OPdgpewAc)y2V71qrw=7=-d^e<Tz4=Sfnj<5$(H=Gvg>#VqI
zA>wscSJL^FfsXwa(xQX*aBTGrGHg$yi+2YUFM1;JM8{kQ-_N?}?rMN4&i0&jsz2YI
z`<ebzn8!=|{FAl1{{Yi~V6kkqGLLf`fIkL0@y(IFa8s^5I`}2i!ay6i^~Q~CzsvCN
zd;>5tJdHmtyc4X>y`b-x9R#E8ihMS6G}><}lllD@k1uBCl4_~+|M)Lj;tIyd3|ene
zR7eDR)aH=W(nWYMxQ{e_cug(Q-^txOg`KtgVZ}mOabxsx7*@3l=N~GA7Gp!aaNQLj
zsinZ?M>E)>RF3^`pWrE~BREyoQ;>_i2mR;iqgreU%{Od@o0rV_Q&e9{%y-07-`+!l
ze-6BLjKsv%O8iCh9$m>E4q0Q&c%NW|n%maECp%p%dFKkhcg4dpWqbIkkSr=#9iY#A
zie_0&C%bWK458UD${T3zZE0@84~6hf^Vpz#0tLJ($K$6if`(0Z(kc(dS35F!Nwg_^
z(Z~`b&NtBXeh0<U;zQ6Yr7JG_I1#gQ|G<=pHqhO*8P-`{rVTaEVZpmp9$FR1Q9=-#
zy}m(1wiVK*4?Qq-y{&M_<2YG8T?}#?r{i8}{m=jY1H0o#;f7fPf45#tPfD{P{+bGx
z`X)hjWtlkbv&7wZ(WKKDYJF~%4OtWfKy6Ab+1PD?Vv6T=S2M&p#~eA=aumk&cqw$s
z8H#i51pF^+4L`6|!4W|RgauZIg=TLne6-aH^t5h}&QxRe93T_(&J19`+-|sKr!&5L
zl!l$pR|~Hn9>?h&<<Pe4k2r8%B8r7+{7<`tPQG{H{#gfTP?QGtDf<q0KmI3}OWTv$
z<v-~8tFss?w+9zR#>x!(E=Rp1GJfQB0S*)&g}^&Uq2tOEvAK&H*Zx>bv#m`a&T$aF
zsZl0#^)7hi$!@kdmoMCv&Y7mgM!<yFw&I2@h8P#1MGcDMh2|r+^k78<TbWz)z_t)R
z)yoVWVLJWRn2npJ=D`d_75ZJ+2et&Z7uO`d5iz!a>NjMQ!3}lU4~;U?D6qnF#p7V&
zv1okbG9F@9F2HBX2PH1^BN!|7kNsVuv0<q`_CD?Z-wI<>{I<`6Yf`_7%Km9MXv!j1
z{XR>$a;rD%{;A^BvxZzA{a12EcBZ<?tHo8jehPi+GQcuu3orgs2~P?fY3ao^!d2H>
z(5*HL%6qrc@M-;Viu?d@a~O-tUq+(W*fUgeYYyzwWHR{EhpsF)KzEu&W2*lNX4fQ7
zxLG2O?C`;fPrqX6z;1X%{k!PX_E=nO{+Esg-J&=D0)(vPG2qL~=*aNVxNM3E<wfkJ
zvmU{8ZnP&0Ch`<LZ4%D;AX=S1Uyk!mmQ#7ubFtt2Se`?p_*#^uw4YSu!f#vglEHpz
zTXO;f0@s6A<yMS7q04`aav|MmEyVb*X7kk>P-cCHZZ_C*bxu0X+a3zXQYUl9j&?Zn
z&JvP;E#-rQRcXlQqvG)*9X{I2g%xJ~7EQDR>FEw<%IwbQ(0>Tz-Oq>9`PG7Zc|VLl
z&?1KJZ-k7~g`nfFCvMtu7P6)H!GG!-B<FAwZwhoFlQ^Z4CYN1wX3<gd*|`Uz)>YDL
z!$4u|TQl@8m#)S7cyXC&Huv<6<RfRt^SBYwcw^BG+0UwOuv15e7rdX#-iC`|WW+#R
zbZohJcC4AWdU7?V4~*x~gU#@MrW`v5?#0+I+c9>U1+_@rWQDmY*rk3b*Ju~x{0q96
z9lx4>Pl`eB6|0MLE#Ht~TrPH=Z7uQDHiE&*LbAPdo=ugn!phr~Q0kcj*6ZKm<&V|E
zyWw)^-xP&q-}1ywL%s3ynG)LQ^BO)Y7Lxs>5Lv&Nzo50lpS`6sHYN2QsM%BpGoEh7
zq^9qJ<NlB0NuiRCem;ql4W&8e{w$oit)A4npWr~riTL7}Ctsg;8)PNvuybG#_?2d{
zV{jK%)J^0e@kt!FeK~3zH=@#RzS#PGF<8BtCo36X2<dvA+5D3-E<fSIM_;bvA&1Ll
zsR6gR`SwTBxfuh4d?PqDT^lvJdP85|F+8kCe;WF285<nAN-YwjDNoH6`%aZ(*IAdr
zwLX~CB6>hv##q$*o*-mg)RWG}O!$8Hccj&IMzL1dbFfo8X#KR0#9h8_&2bZ~`FW{=
z@F=%4)!x<+Ys#F+q2>mivhssRN>_xtUP?Gg>KFfuol9rFDzo|q<l8NsaZgdP;Bv$j
z!aeT55x;kE+pahNe)ATFPaMp?e=kc<%u%vVz9o#48v;|qG`MC;H|+j&9&gv~%)Z@k
zfXR9v(e85|YmAimZ7Wntg8TZj&s|kMyD68VgH`x-g&aNg9Rm?=>3lMGC;s|yh^`s+
z#!0r0u%lVp7hQ5Ic2e*m%^Gd8&FGKj6{-wcQFys_7VND5Oj)Zh!g|kZ!qv`CWmcMF
za8u9Y7<ED(S>X~^Oy7$qJbnw|Q`O0I*}<YFqmkS@r5QX=zo$lh8(LB3$Jr|5STQXU
zW6m6dw#UZUB|*ULpGx>fe-Fq~lNUzbodB~RPsNPkOKFx_C5>|H#6Rke(SuAoS`hnM
z&@ve+<h1XCU32=PU4sLdT#N?qJO}LK*N^4KC!_kdhvbvI2tM5SL5UfsKs|LZr!^Si
zxCM7XC!-%{c{t+UPRn`u@M@tXElseU?ub@@<>6at6waD^o|-GZ&=b`)G;ID8Vfx#l
zJVE;<u6DL&n{q$g=K2k~-vOF1&z@GLm(jnOrF_S%AD@C5WEqel)?dB_a~sZyrOPyV
z^^&<1-G4ECn7<N+CM84X^K0m=%O`rLH;H$3yr2si52^d}a2WXQ9$3z-B-z1q6f_&f
z@mm#Pi(@hV-e1cn^dcdmF@RiW-{W^Hve8eL!<8@P`TTWr4wqcgQ~b0z!1g|-b=%E(
zpX$YFy%(|by@%xGv<s`;UQ7G&ZTP-nAG%ab;)*6yIGwG|>AvTILLz9inJ0z%q|@+W
zVHEKr5&k_4Bi$uW>C#jsx{zbe(PutG`oVR$wZ0P+1pa|Fe~j_euQZ+-^%8%?2a;h-
zPt0363bmKFlCSeF`qP#x*!J;a>^%+gy51+l9!>B%{~Rn;3L~Xaz44k>9FK06)(9Fe
zA;dWx7T0;<@KX*{HS!RZN_-Qm`Kd5^P(Hm~QVli7EV$}*7&M*kig*5u$GDTW<l?lL
zzK87LhNyouTPsNzl6{u?v>X-wla1z`#W$f`zZsew-$1X7D7<gAk5B7=gLQtj^r>zF
z1-^R<nGdtYXWs9u9foYf%!j-2u4=m|mmZI*PL6zI)=r6+^aoDl8$jjM!?b$eMIm(4
zOTkcDi|1R0;+-NVJktFoT^g1NmBLT!W2aNd{-}X<vvnO;FN@?|k*8VW0SUD&R?ypB
z+5;U6DT>HjivimMc^y_j_OlSqyf>6P6y`znjy<$d_YQ<e-F%-PJ^8A068360m%Uif
zirum=fa{_#Hrdh{I?7u3@Z}bG-D5k?G`J4q2Ye>W$F(q5B^Y#DhmpzcP--5p#yLU?
zY<M!9M5~j<K4CGek-LXyMJ-~*l#g&TI#isp&<c-#mJSX2y5WoLe4alc852Ca^6Zq^
zeD&rG4yrjo1)tO;-j6<pbTP%-l@bG0Tc6W&!XUBi5>3mp5)-DMqR8g1s3kG!mY46w
zS&m2Wzel}k@)>^^Ae)Dsk4X#}`D`?vJQHzEH~i*eB1{}$j7CXqwE9Ym=y!4<53QSp
zA<j1ZB{~&jR2P7sQ5W_ZXd|wROcnd(1oOw_{xq(;1y4^HK%*urVN2~Wlsmi>w-jWF
zqgNRe&u&P7j)nix+5&Gd@9?IYk<wY_j$GR4aS9fDhJsulWnR-|GLGI9&EFPz30>nR
z^00Z&=;w4RwtR9P^meDg?P+sSsZEX6Sps@jKcN}<au_{i6prsv4$00g_$t&1Pt`3!
zoqhKNvlpSztB=Ulv-S&f@9$y@$cG-zpJ-0sy?p7O4R_ZDF>GqJtS#gWo<0yI9*A+I
z#nJA(Ym^VHRlh3ApG!rjbaR;hd>6W@-xXE89zoX8o@DxEKes8V@q=D+{Nv|v>N>3u
zJVra>*pJ;=an3fl^0g<aOH7*Nbth!qHxvV#3=;qO*rB><I%F(u5LTVh<xY#c;QTHL
zyxx2tF3B24%OtLI&A(w7HL#I#H@y~hw`>xU-s-Sy_IoO46|~t~N~Q<WVCa@f_~p((
zs_=L!M4#0`uivTk%KN$4^d<{4Zz;3+zmwoxKY(9K=LYV5?RcfbW@2q)RM>GE-ZYNI
zL7#pLy?>eV*!4Z}X5)Dpd0hIgZ#@A{hdv2E`@Rw`+l<3ukGgS<zdIP3>vELy6x!~u
z&Nm(<Q_-P^H0X^f{4f9izx@CI^8f$K|Nk%l|G)hI|MLI;%m4o`|Np=I|Nrv;|NrFw
z6?<jk|MUOu?>G#GGe1F7X)qs@wS&pyn{c@EYP`I;41O&O#7n=sNVB~kwE9(ta60=2
z?9`hA%F}G|?qO$WoUs7*`#cvE8aCp7&1`DY1!?|V3ArQcpnje;9=OrlW{k}!Zt3j|
zs?Ce|n9)_L_}c>&7A8sT;{9MU#gWXr<-(bON8mIj9kWuD;Ya#FUNBRUyIwiMIs5j|
zyxjZZ5sf#+Bkh$LXWoK|)hon~%c)|umm0h*`$I$87qiNLlSQ2qTKu@)5e%e`r~9*8
zaKP08Gh9@~JzqAUc7>D~@iymkIlJ)o6npwUhG^EI5ftp6$E9S(&G*mIn|)sB)7%ph
zCWb+^(PA>qb>|WT2jBoB(PLX8z7<FCQpM}A+&YBobo^*=d}nM|ZigKcf^m+Pi4fCM
zuekqwT_|1U0Y`%LvB$2x@a=_+w47tP*N$J5J#mllHe(3?4oJY=UyCSnvp_lb*5emf
zRor?z8&~y}L!;pp&|2z9x<*N|jaC<MRJl4v{V_p<?e~O#Xv0U8o5&z-5?$1=Mz_wn
z&}S8r(*DDssvpjVpHld4wmQ1SW%2E}YO!b7Az|HQ$uTMCPhE18;nIP{*gRg7xA|w_
zo17qV-h^;U9Gwk*_2!Ul6v}m3MVE_ElZUKY27eklL8wjvUVfs4FMTIq=%^snvNOgx
zadH@vq>G!v)G$>2C>%NCgu+2>_*1+ImT$A-%%tP=<X<YBoH>K`ZQUWrt*r&Wei__w
z{v>68%fPyeRvf)=7ruUf0XBy$;mgwo$eN;3VX57DdUyCS-I=C=%Z6oh-83nK)7zes
zwk1OU6J`?Ydpa)e^n(gb!qH>%RhrvuF8qnP!s)?RDJbB8*yy<*x;(v#x657%2fj56
z=Bt;$#}Dyr*lQv_2(qESw-QN9$^lsU1+%(hq-@5kNqoiX1GRWM@$`_vB2>!@R@M&U
zP(c^h-EkLZI~ib)m5SJXkHj&wGK1yE9+S=pCPT05!o8kqxb}4_?vsjncho<^ia%%R
zy-p*|w3b81X9M_ptrI>tISQN{UQn3JD7<(`U_0!_<+WR>$GsPnP^(S-!p&eo{~t8w
z$}V2oO#^;TxXO_}d+;G2;x1lYv0g#y>z?)Fmr~Anx7jVub@zbw<+CX7mK^=cJA$*+
z*8W#BYd?jY&E%?oc6@DX7k2+;hMKfch)xKHzVkALGw<7r@;wjW^+w5mGx`pFIS~oJ
z@8_|Fx*Kf1dl0&AOrkfB>*3G=TW&Ggi}jSmEw9&M(nc5QoVFjUpFd3I=M><^o(jHY
z6i>Z72lDhwD|plKDtPz2J4bnFP_X!mv#)o;%f_d8mUS}Jwe-L~vxDeC^lIuY<%rkb
zO(%2D)m(F<PGTO9XF1yf_#EwvzduI6Z@KSu^6^LV+?_=>0WloCR2hq`CUCk%hj7tB
z$_d?R7mtRGV)Os@@H7Q^-k{urN9E{nhkgl&M`B3v_a*vR+#QoEGGM^{)8JB&C-~Yw
z1^tk_RIyCz^6u-5W$Hb7-mS45x>$=APt9WA?+#d*XafOWv&Dx&TDYcZEoBcJjy264
zVuMV|8VqR?TzX98PWiL(pm#H@+?NRt3`SrR4Wi@kZh~QIy6}AHRD^Y9Lf_@Lpx!jE
zs8?+Yb>BUL<05vF#rU_>=cgH1*=mw<yB6sAxbgb30{Z;Zfa8<3=~3uW+Sl&~RH)nm
z->x7#XYYi&{9QS$(-$gDydz`>4d9%5Z%i_@r^0@FNU30^;3;VGi&iDn%~TVge_KFl
zpJsB6++A4I?FA2boQAvg&$IG}-4rSL5t;@@OF6>9!mArI#YZo@%dYruq4;(K-am2(
z{%e}RD|FAm=3zHwvFBQafbw6$;`YhH2cr@id+Q|B&QBGFd<eq;lK{N6Jehs`hGLcJ
zDIWgz4rMLQ!S@qJae{jn{LtwT?SEVeZ%s1j+OJ5QFgRH3c&RUR99tmn+K3o#v5~|r
z`e<`!G~7G-RrF5s1f?|{!t=|0Fn{V2&Y2g3zm9*Tt;esx#ne0Sba;eVa&{Ddthc2L
z?ceF~kBj1Cz9w#woK(-QSJCrJ32>I{$Wp6{w3MXl87l`heW$~YEyHMyQ=aucxe-)p
zbdlO(rS~B0#@(lD#Y^CT_wDONvB;0kTkRL3o-BuX*Uez?9etj?+y?!Hr5y6=Q1NZM
zAbdL}il5ECh<WMT#Q1e<#K5XS_<KPcDeig;Ir7;M&{N7`J6V#I<u{?x*OTW3j^PNM
zlVX{(BOK{{7dkCDNXu@FM%%&|p0(gNOwit4-2F-bjqLRa_{j|+xnveR>%SA)x~Ia}
zqGm8$*8@HFN8psNLqQ5ZLRFk8$2APb=>|I?<kE1=*AB&idxgB)CWBUa=JA;LVI+<W
z5hCq#1qFLMSbe#jUgh}WrLVzwE5`yo`V``jwU@-_XKsq&Ge%?j>w4OtuZ+DH?q*-D
z@vL?tflIpSvg`9a-2CzorLAwnwza)@`X)J^@>v02yZN#CwOTa1B*4bR(Qv7=5`|(i
zA3QjL{<JIdiJ9Xdw2Udc{vbg8D<SLJJ}^`%MaZ4V72(yG4Q_1PZziUer(<|0XL@+}
z8xDAu$K#)Uf~}$!&!{;~{Z;#6|Emw-T#5DN1N}M|PxhNaKkrTBxE{(vQ>+8%#Si6k
z``%vGI1nK&%ay!F`SIB6pdT14tAiK6vgpssQkr8ECzhV*px`#?+Amb+lOgiFc3L0V
z`A(YnH)xJ@#fPD(MFqWja8Ybnb%5TS?}P@w?BQU~9^5fuG(4ZWj{7R*%N%}W(#`6#
zkleVx$f0wtIO}9QHt&t&c>8b|vHupGd9sR)&ol{gqbtcf%7`7G8S$j;4~5|hl@xT$
z0ZS+Urt4uz!iGnQ%r{oU-3x!{u;Vjf`Itj2tL?{e{UtwKiz3&aalzwjd$Oi@usFdW
zhzh!N=b!2paO~U(82Kbo9Jin!dY12@xhq%U_q%Gst6tr4<mZ3T+P%Lp@0C5Ij@6We
zjxVW)W)qK&vxOD&d-7^l$IW>@=-$*Kp5L2DMpj+X%w_~f+Al>d&waw}eox5c<iX-m
z-j(9%VaI7jn+uFv<0qZ5r}E}64`qAp)nrxjCu#bm?Pz&$3AZYku-}J&6!vYLP;jRo
zMNSXG+GmM0-=mU@-Q%Fp{}q*GU7{zmMv8Ab%6Za!V>-CL5w7mk;%1+&HXDB5XL8&q
zJ!`?7KHnA(+>I37-dqE_i*+=wnaEmk0qFG4hb;>CVT(#B%%5vWew(g9O_npP9u|l9
zk{gQM57!D2o>4;U+$ioO7ml-~{^$!kWxPK<LrDId3FmFc39V1w!~CFcaJTmX-YLCf
zl=F^oN2s#IBp!-u4Ts^f*5OdodPdADoh2r?1haTx8R~UYXNNx;(Clu(o-_!D+*!jH
zS`XnY`(7~VT4C{UbilVs`z3X&vE=U>#^sJtpz&oXuTyn|J%s@nTOWcmFGa(jOdo7;
zT}n$PE-%*E*#kG|AEZM|rtpg!b>ds!8DtP0DAUcFg__eNdH9C8bXYTh-?V9hl|cy%
z9?)5IJk*oNe(ui7tE60cY72NbzX!#Dks|KDNl!z*(!S+eLEktU?&)ntm9R7LL8=I@
z{qTdHEefOZ)DxU}*N;MdM{xPDfz&(mHJ!WC4+o!}!XZX`p#8fAWIbLbdCJ0QwtK#?
z`nW1zws&L)JtIgw5<n|bf0CkdG`zA&Dt7L3jSTNexzjg8cvZMJ{BDcj!mu-VA?FG|
ze3=3n)}=J|z6#gf3BXBiGb9%Gnc@?VCi9dcFIN2>Og;9Dq-7J=z|u9k{Lg(f4-9nV
zDZCX|IH=)E-Q8%TF%ehU_9p+M((j|)Flt*Q#JUY*+47yzxn&PF{m%je?)$-u>~Hk#
z$VN`S9?u5$NmOMZh*oBku%PfX9oV#8^fx#`k1Y+!s%Zr5a!{8=HU<hiZVhGk&aa`T
zv_7!xc1Ii<d_pYy4`A<BW#R42^(gm#54}v498uM}^s4hLc3TudP1b&Ra8p6C{03+E
zIP0m{uYNhKe7%o*MO}wI+BLMl%Wz8Ap@0U>Gog<j)Ak<Wpn2*g^^~{{?R8UVYM&x7
zcS)glQhu(6#|V+w8x2Ao*+8KMy81lk?|){~`{(hLwpoU;!O5uoW(RHmYe?FqbGchk
zG&^*u0o7d>;bZt{@icyb*b75>`0A6Qx0NYJEUOm8{R{ZX^A|G3DMQ5DeTz^hO}bYP
zlwsn4fr2Nyb6%kr)IL||*dD{Vd6FI!E=s4ji!vof#zuHAUB8Eu40y=1$?$Ce%QTdm
z#1Zu`;Ps8O;=3{0JWO#BiLEC{|Lb1RNjOK>^cYl<TWRUEL*mWX&tac#vlw!9kF4z4
z0m6g#X!=Z7UMA&M*9;8i`03t!C-M|Ab?4Y|TLp{Bx-3S%74uzZQ0&WrkQ%xLZ|_T_
ziiiXXx?;w6EtC1-)|)g?B^AbS0LeOy#7ASELU8mhns!Vcx~mQ2Kwe2fuTNWVuJuHh
zm4%Qr{0+Qn%p<>J6+-@tR_UIefb(5G3aZX8gua_Hpy6$IE*w3Y-?hJ_y2p7uxm63d
zxLVSrXVI8AG>uA5Jf>vHuR9_1t#C{?8vnR<#tj!*Vb{2c?CSN0^h%Yn`RogrVX2OL
z>;~|{kfE$KPfq$tP4HeR2}66Yq-#T$!v39>WcH$x{%g95Hxzc#yP`00P^Yyl=zpUN
ztpePtKZ!rn)}iC)Avpc!ThgDHA->z&g?GN6j+4~v==zZz*z1}zFWV3Z0Zng3L$jk~
z{PfyoVUt>MN>^7tK5ii_DSk$SNA?sd6546MMJ8O#|0=H3(-iLY)xyCKMl$tMr_a{@
zv^Z`XTu*Qk!`D}U$AV5=D)lhcsx+bLl?xBPp9tOM6KHOS0W11+=D%k1_*CmRP<>~^
zvWu^+j#OP23<FhIbJI+`Q_=ti!{uo76c19WCE@nVC%nNx4{d&4<3=gRma^?0y-AzH
z>om*-ySeM}O0+)xntKSwNayC!Wed6No*D9F8&<4ajVCw9k&gOwSlLi5j5x2!_Qip4
z@5f^@=&eC}UL6&qtC08MIsmi5e0*h8@n5BnGIJv*w!E|#jv6LX`Tefw7ZM0cC0j5D
zhf$0AIyC#1MZ0g$0#lYF1GgCQm~#es^cxGyEtj&x!K2h*xLMZR=^I^Yy9UFIJ;CVp
zRi2UEB8*Fkr1*!HG~`?<{%gxBZqZMnl0XuMz39u65|={vy;ZXO6+2*b`vh^?%KmtG
zqZS7Bli}cbF@jgXH1fE=f_nXN=k$D_H3!>ispla6*%U=b-WfpCCTniYnJ77mHnGdn
z1$<p^D=E|p;^?mD$fLlW7H9kO8Rrqauybd)=@vmjlG|^P#JC=MUy=8JOMsPwqlFyS
z;5}mtVV=aZw}>yH-<!0!QGFw+PGr6%>Is)cYsoeJL)_JHUhJz|EbD)v2}<(INN&k2
zZ173pA+@F8d(m1}L=G4;v;r?ZUX8)WI^)Ufg<u>OC0&O|air=b4Dnh*N5f*VK(`B8
z?COqh*&pJLyMX`CFXSh!FaLHNf;}<PT*5#GCeQt7+LmXO?vh^Ie)25+x|0q!b#my{
zM=cgN`H@@h5|$}<<H8&lVQ1w<>T5b5-zl6GgS`vD@@gVkR1M$=D>=v!j7a%kED1eT
zV0`;k+U>a&{%#*4z27F|$W&K;r>ck<&ks_`<PCV?;x8DN_ZTmEU!m|YUu>E27<7#K
zldZ2AMkgom3-cmM*c``U|2EJF^)56xPM+s3?!w+>iDKuQ)1ZH!B5Zfuh0pY7V1?;C
z&iEcg<#KuKYATQA$A8hV3_^wM)$ILlITm*v##_d3p~h(k$n?|{?xa#E`_g|Bk9j6}
z3-VIo_C712%qJ5qW-kS|UcX`A^$mC+JPpE+{t*MUmSTb2T=G`g4`B*&s4aCw6B_Ml
z%i8ID<CZO}3m1h?UOpT-W*1hRw&GM57r5|svGB$63l+E=0JZ+RC2rhR;wwvV!@W@U
zmmIC3V;iKu@jcMjkYP*J3``kWB^;XhirnO;bBNRhUpeCy`B%#0dz-ynk#ZSEJeEA8
z`I-2iwL5LN7>z0ulEmoDb(nWm%4&2?#JxLou~1q=_1QWD8@tAeS9<P48;`SM&YHn6
zw8@Y^I_>30HQm{4POeaMyci~Vg^TG6V$mu309$!<#o&Gu(S(G{)8y}Cw+l=0?6?>7
zD!eZq|FM`utb6hHp8bk)vx8`_My`-NxPj)3A@HBNkk1cF!PO^+kWRq_ghjVxu|uW)
z>NyoY{i-L|KCmNKwWq>018KcvrUEN_9;a}la#`PuAEYaHm!dj+VO{JM(lXx-6|<I!
zR{2a_{@cqbWtJ2%;WB&N4ny^pi_qDA9j>$tKn>SQT7OrIPK({S?egH_eMf?5;h8NM
zdcmKro-;wipL=1R{tS}UjS#eF?iZ|%o*+RyFQn-iz&BTQiRGq<hfP+(R||VARrA2p
z<3@v<e525O;S?#4`YJpgb^xv}8$l2Hf1}>rN~u$GF;ACxEN>>PrT0F@xX9}`Jein=
zt4#&!v3)YeXFV1zsz<SX_*U9)*9djp2GEN7gLpDu6@MI<0R_IRVT-iCdg&F<_uogt
z^1Ev}t>B2*BcdyIv)h1&y{6-7;TOy)?uw&pYC!MrN+C)&3B7OD)1@x!z?*Ym2j!EO
z^(&aMJDWxdt10qY4aJuK6;kg>+@hXt!n$x}Tr7_;_uNXDF}o)Qb%en&{d5W()EynH
z`(uj5F?d$-1DD@kOyax*JXy3`Op$ZKyG7YW-V+V+;lV?+ZsUmJ<$@=5cV0?2i-7NV
zNd49m6Z!F|W0bJ53AQd_P`ma76wHpmo`Ip%B)KT_vigJUZ!es>)eGO4ZWMH-9*l*>
zZt}GGKwC3H;AF=dvZ`zWr$GVO-fKRzNSwVbrtd)gZ8c4q{#Trq9mh7|%fxR|#@GGp
z9!$&0px!aBh4zAUF4Quo?XDsGZs$QPo;nCF*WHD0{bMP%dJS#pZ$j-^b)wDDH8^;w
zHh=GGf*H-5NZ<OZ_*`W-eeW7fonFinzb^sd{?+#|tz$CSd)shkt1@-%r@-!mB-g`%
zahSO%9=E7;#zh+z<BGQJxZvMxJXyFGZ{C!L<K=qr+qZ@%Ii440e4oavByLzm{bGD^
z&W)$esDa99HPrmG3$E|3jpa^W_}Z(B)JeABlh>}%`lT_v$I~0UcJCAIwymR?xjSWL
z66@)qx*}17<bBj>6)IJVSbK~-23n<a@uGUcDQi0V-}U76TX%_8Cz_~s<_ylb;Y$ym
zsY~o4c^>zt3si(<K)zBshdro;UY#z}(V{Z4o7yTnp0}P}r)%;$6)rxR(oL|>PLLfq
z+?zI~7K(2+PiOD90kl){Ne__b4o)AF@#d%fsQoSmFf>Bk{C*rp?MP!M`EZ<Cxt`oV
z=EISaD_G`gOwf>swljB;iRmh`s_q9yR#o)SLkpLr`U#tde*>j+z3}RrWnxvpQiy(I
z2;FvErJ3M_dUIydjm)($BUpiZYHtzpsz#G@+C=<zc?yntIUF5JAJN^xx1i~^7oQ)!
z6=sc-M`a%;D3IR`hi|MwU))-3bx0mRq~8L)0rM!|vV>H|y@l65_VUU$KQVrf1HT_R
zjE7D)gIeJ|g#44%vR6I|&(FEa3cNzdSFQ?9X8PkFzhGQc@Ei`!7>_!GE(_0Z9fScZ
z*5mS~^O759jqvK(b;wVT=lE+$g65=)q+4Pv{(C+V3ldL}_nT2zvLl!6OX482Xa|QZ
z%Yl{g<6)iUIJWTkChYy>$uQO!`%hHFpElRA^-3S=d*?Sb{8ePRvz<$FdX_@K+Xh<M
zc{%BaeibLtEwQy?3b)7&!|N&I#pSsSzpMlZb?=GRg?AurR2&>R?2cbm$|!vDazRmN
zJk!k)P(1byO{(jO|3nKIw|cGkC*~8i=l>JeSgO-r!v-pp)^+br{S|NL_vg0ndvRE0
z42|kv1v7qbqTe@kI5Wtf=WNu3?Y*xFMSpd$I`5Ng!Imy)xA}pvT$l~L{!>AP<L0z^
zsu^5LNrFd>bFi1(S}c6fCfM~lA@Q>`S#fqBPU^NERA%j=hDCZj=c+oO{W~zu3>P+z
zU@42flN#*JIj-1((;s+<zAkwrNW74wJ3}evhZ9v8xv<UoV!Rw5MapaPvDtSjt=w}E
z#&`4Jl<WnV<j@J1eBDV{xfuUxYH^eNPZ}%nReOD@p%nuH(8OmPy&Q29)F!o3)7DIk
ziSUCbBeUV<?c3sr`~}3THnMkPCZ3KY(fmjpxJ=L%yh{AI$6*^j-g~|nwJKex@a)M)
z_h<`a&icS1^&LE1c>}&TDWV$+20VQ8ZCU>Yf4bx~MtCXp^vc|(!YlR76yjsd`JRJO
z&+#D*$mqo#nj`VKt1Y{2568MrM(7xIA7|a2PE!&zFs7x3hM7svhkr{^@~blJU3-d_
z>}|#e>SL&MoGE=t0JhupfU2i2;?td6NXT+vefwVQF`_re&b%Z#tq8;CyT{<+#ywaY
zb_||2_Q$bF)>7}^7_*c-_-@;57&r8is9-P}yUZ4$x281bjJyPOKL+sAN5h2L$9s!i
z2Dy-h?pYo$?du<&{?2;`$8(Tvm3Vtx3Duex(z4}wxZl`<-F<Ih<hU>rmc&E*iZrW1
zixjEi#SFabTMH5M-$RH0M@Zh&0IV1fb&=_~;q!VJ20d|}h8MlKI1Y`Cswv7z@~T)!
zuGkf+tQ9j0<>If=_P(k-xL-VYeXgSYZrZeH<0{;Ka+&BoLleJDjN-Q2v*~u}G=4DF
zn$LT_gApwnY<FIt)#9$ggP;DaqGpCFMGcTS<|RB>ZwmuIXz+%lPGtJ%AV0mCNC_YI
zLWa&7uK#fpHhkVEHpC(ydEE^IY$r>-TsL+(m?V8E-%*vf9pNHNgvv1(@b)H6efN&+
z?11wtE7{?l8>s$T&MRKc1r3u>F(oDjJX$u>X30I1(Mj5uSkGkl4b2eub2aYX-V0-O
zy)faZ8))_NrLE2<amUX*c$jYm+oFDo+Lhrve(5M)=dBDHX&1y@rxufLp}zPiESVy=
zUZgG$_CrB+DGlm<j+FYl6?fF%2HUhx)*c&l@a6VyLd=*)^k_{Tlq_!)H@+_*$FPMI
ztksQ$p#P{>d=uElYZpB@)Q2ACuHvW}r)fiA68k-sJY;GlXif>li_H}<FmM7bJYP;`
zZ_iV5m?|he(MID<Cs25{MA{>Lf?fWXsD7ioEcb>P>MW4vQHOmob!r{GJ(mbiO81DP
zN3~g7SNr3~-$n#chv>2E4`GtgM820Wo`1U?ps#%=a`<U~m^o!Wp1n5}kK+Td?zBhv
zF>D0io3sy_?iG^Sp-SwSU=30I4X~zu5Q^$`G`AM1<kuU)vc;TFyv~GS?$^aXe-~26
znta){;RV!?;v&rZFH%(eFs(Q+w+ym_b70kdEgUp;AX=q_@gSW}G@|#AlG+9T*WP(Q
za`}J%pM>lZA<3wSs6^)TI!AjUB9wN~R1z(1tCB=Qq9~zVBH?+R)1X}%l$O%cPEp!@
zulM)g_`H7fiyxljdOjZK-0!zr02S1IAmb_K!rraB&}O+4R&T3?{c)!FYsFQlh~3ZY
z<N`(?b0bLk1~(5MMytB>uz1}#;bT%Fwe9>2pT7e8ov9S{-}VvO&Xse*IBQG|EEYbl
zegONwdg7Lk3Gmf(5Kh(!fZ9we@f&>-dVN32(=8Gxanc&Gd)MLorNA7n<#fia&89qg
z#Vx5LH3wE)^pU!9-tgjl7Ok@$0#BVcp<?nm;SofNS*N0@D*GCZ@V!jqlO><e-#YeJ
z(!_#?q3pJ!D`u1(l+OVb+|W0XpKF11_lkwoCrhczp7t1faVnL{ABsy-Jur7vnRK?^
z20?ev!?ldRl%}jHdYLbR<`erc>cbo%EIx_Km#wCm3Q@#84+)w|R+!sbPCp`D*uwd^
zSfhQ6A{1uh^L;A#RnHWETgO(uf14%$GBa14IX?<!w)TVrhQ9D@{sS=XHiSkRUxIc!
zXF^l?FR=5F`n{)paQ)CrFuEmQ{IF&r{ZaQ}n7Ea_F03c{_YXAA)l)pF^qd}!i<f6T
zI?w8sGVtgku^_^Fu}8W!hC58>XhmCyGI<V9w&qZ%NuGGb`3I%A=JI%j?ciMS97yth
z4ViKhRhm-htF;0Qy|#>oUppd?inaj%6;&|nV>`S&%|<*kt5zH+&5)i%*-HKC=`6PH
z7JPH|($KlCupsgr)$RBST0yg<uJ}RtKI*mD@Ic~Nq=un+y5!)qR=@>2+heqSgnRJ}
zGoExs4L^DJ<4f*cVN`o9{<cw%*G9NND2xKZ$wW+&?$u8c25`GxuK4A>^!wzS25YZW
z3;k{%r&>?o>3Z5Y{=t5_9PEJK_Df8J<VO^jyIcH{W(<W%!NQxP$>5iHj>f$*q+C@;
z4DI{?x|?5s8m%of$73a1M5}O}5XXHVSzwf&C9FQN7iM*zfC?i&(84`>JmFpqJ@XwW
zJzICs3xm!=fUzEb8f7FVn?>`9l72Yba4c<TZ;ttkU9krqhDd`WFw-j!qe5+A{lu+!
zP00YeEa{3#rMF?<?+Cfx@7{7N%Z@x>-#~af$eJ#l-!F!4h^HsZ>uGM>VN$<pLj7KR
zr6XPX!H=A2Jmi5Z+-RQ6t=qEc$Hf%BbS8)Qciuw7-%cRIUT2`q@EUe0GGw22H{tQ%
z6V&h4F76&M0dB68i77+32pdKvV18qUFlLDo?U`Q$s$cuEzil@xK4pyxokmdIfZ-T@
z?v*gDX%BcU(?*4rS-31>GXEH`0LSMY6AdH1D7l9lY`?5Wr}uB?7e_Y0prv-O;!<bq
zvAI@kEmwe5SNHLNzi|{Y=7^wiQxV_QXVR5BnE*2%@QP(M6xm}t&m7-CH7o4laJ$QN
z{i@{d_3VWKe!ckWj3?lrHiZMH7jsNn0`Jvs&jFrSDaf}0dd$kf-!r%H%s_i=`JBKX
zy7uH*#k!DDw}p2~48?b&tLVA(|CZ|nqPz4yHowt;W!cAJYP(yc+IJqDRWO9ZE4}$&
zE8?teMlg$#ut)hrxM`!#H!+pl2TtSh&vjU5tmFb6xekxm92Lh`SHc+V!}AUqvuA@9
zAL#L2xHKjh20fS$pS9v}bo^jmf6Igwl<VMJ<6SZc-G^@{5)^fOEbfi><?ZofA#OxC
z6>c~T<3A>gZ&!PBTdH&i+^;T_CK};p$>rs!dliiPXrr~l8hCj!9v!pnx#`<FocSe@
zC-*nT4UYz(S7R3SiuA?#uQt-OzAr%W`)6oWPZbX|hjQ)7$z0uK4_`et3?mn`<62!m
zJba*t4KgzM_v$T#=I1#lqynqvBImcCgF4^O!vKdY+UoeILUZbP-jw^6W*rWMDYsvN
zwTdm4I!>YUI;+U1rkb!SPn=-90}j_4!3wKqwCK4lPCdArE~THqq=A0W)g}sN+;0!6
zb~kA)?g!u7r)52^cZHlnXR^&J7ZS?y=<;2H<*DuI=XXsMj26L^!tLOVon*hv)|19p
zfo4zHM7?bv!=I%$Df(-^_(j-3!{a3O?Q>5kT|Jg#rUL~(%Vjh1u237I#<!JAaPm%L
zNV$FjcV_sIlR`1L?HGtfdlKQaPbNl$*rM*V-$LfBuJ9rGpm^F@1K$42g(YLlA<;_%
z($WLC*T!Sw1j{@&t9u~y{FFutj#+Tg@-%#L=n1(A$?)Vz42(G-JwLk+z@b0Bk<V9M
z)c(^Tc!Zft&*e3ecQgiG7kb0gYay};QCiUSB9adE*#*^|Dxg`-4O9}l%jUEs{QnO2
zv1SYGwP`q5`fsD1+L|yb{W?9qd64=Htpw-FlNDE92T5nfcQD4-5Wi?%!#XP;=zjSK
z_Re@nv(`T*??8>p2g71%)o(TKzhx}UJ#Q-bJw6P{zCpNA^CHUH>yX8Y1nBp`9^UQE
z<4IdousO>cRX+!U{Y71z;A<|tEJ$Dsb>rMg0eI9i99p~IgdZpUFk@sDuN>G)y^=Js
zrD=qq<Qj?#bk|`(=2F%$aKvR!Ru%tr4hcus1>s10E#b+%-dMI_D8`&!fd%tyVA*|D
z?9p(_{gX|hJp8l?$~IVub&W4cbKGtrM8zJ;j26nCN$<)27z-}nluwt`EjeQjlVZEB
zunYTR*M4!}qN0RnA4&7u6dCzR=cIvAJH$?_H-mC$HddU!O4Ebdz-;Xfm@?FxFQ57-
z)b2kbR9%ecj~oAz`($O_|92u6HyT56TLNS&L{ioWOMWn@C%@4*=hn4H$!c^ym?*l)
zyc5*<OP5(ZH|aBdJvWF>BzNPFhu2}h;wrwj;w;(e8u8+=fkKRm28f0Oz;r_z?eBeo
zy@RBl)xj0Qfv{RKvhu<uj~p>0-UW7+9v9E<OGPJrRW|!JmHdx7qq>mHTMzCh;rUup
zNx4Q=MrSL(c&B4zgC3rb?g9~gx6(|B_vmjti(WV=@X5Y1X$G9k!R?~C@#YwKma$Rl
z(l^2Q>si#+dnDZnH{=ArXD}iC1<W+;0XF7?#Nu5usJdMYkKAyU4m4k&nMUz6^YBBV
z{p`=+7Oy9<4HmLi+$#7Pc#)^du7LFUz>-H+_$G?skKoETni8d5Om}X-Ly5KDEfK5b
zpJCJ0Ei^7n7aguVg>Xd^P<+x46>blvy!NIz^NJ%s?WH9S^_$PuS+6Lj>bo#x=}cU>
zF9v6a1mQ1fzE=}{5{&L@^A}TZbRV*l<q`d$4=xveu2YmB>L^2}Z>jJz+!K2A=_B0D
zwMEf)i1b|Pgod+`CQqxPqBwQRXq*LRS>?h6xjlMnsz@&GNAgAwEqFiTsQ6;94-cM{
zCk$L(FKhR2l~CF<6n7RshDi$RaBk2{JQ@89$|TQq!|)(B(m4SpLnX%j3`LgjS%zOc
zroxJaMdW#R5cSmVBLoHRVuzbe@IHTpfZGG{+>hD#<99sd4lfXMCpL4C{xhnS_Fgu9
z<K!$c4eXA7q>@@Ke%^N-A6+vXcX>>M3DY;r`gV=R=2Z)s@7$wra|&>UayA=?Pldnl
zMo?k+DZY8s9w&y}p_G7iq;e~sD+@QXt9c;1jj*T1+sz<MeX^kV(3YP(%^<%c5+8EL
zLGl@#L@E($XlvMH?sqa9I?1wO=o2;0%v^_Y1w<wuoyfiCEi|&32?K0vq3d!>JX*Sx
z=N~x<C$x4~8VGT?(4h<Z>n*{xThp<g6wokS3lB`Qp^?4Sq053ouBmtd+Zx?@VPz&M
z1P-U?1D4~?x!s}AHUWoqX@xZspYVlsZ*KOvP&x9&J(>}=1+P3>&7)Eo#08I8Lz?s6
zY|X-Fy%NQ_0n$9DU>+L$SjgIs2J+D~P2Bppiu;+Krc}Wk@0@u@!A3ci9lhq@lAPg`
z;wR%bw^u+9ovwWA^a)n0{R8>mu93XSn2rTG;M_W{Z16fE)-8G{ZacG_I@)*U_WgU&
zoPWJIq;Ld&(|JS_x*vjeHphfct=2sGc&qq)>mBIyLX+PVW<r;d^Ek-Ug#4#mrs!5Z
z+WO`-RJT5-wwxAPy!ax#Z}*D=FFA_K=j6J_Yb@dA`Hoya<3459C4k?}L^|+lG{&r{
zB#q_>A>L;L+odE!@@*sT6}Xn>MvlPb!YW!D+Z$U}N71m71K3iN&f=`G*zSl4+sP96
zs;ef&gin!&>{RAOwO0h+If(Zs=yT((Y4|<z2TW_(!bKrg^lo`JXO3CPR&!DyZ;>yn
z>5qiAc0D+$!voT-eF+=eD&flYXz|OB18`M)1ADniGw;DyMVBsR!gh~Yl|5(vCe`|`
z^x^3)9@$a?4Mk&N+;wB1Zp$#_We|9upN{=C6JYVCMfg?sCAsyGc+1OLNl_z&BdT5r
z9#b{N#XnSqm_Li?+{;Az7uz1TeyJBG1u<mUH$%3b#7%!URD5F72kXx0v(GOn^EbVU
z{GGOQ(#i!e@lbct{S=A!f(L+s^L(uAYfe{1V{rL83nNVwsPE1wSYNr1Zt2_dhcOj2
zyu(pgbYne4t)7Z+wy5Ivjq13ti#F_kJ00NMK<O^x1Bc?@(wFrggtR&vytrIWrJEY)
zr_Mw1p2I<IQQi)wGYmPXqXHKNjOCx|8~I%3C9%_jVN~Jy3SPY$g=M>z(Nj1L$GSM7
zZH7JTkC%R<YOQ#F^GBG`RZk4xsE6XYD!LrqNFQ5lm`keZ;AN2-15|Kav>wm#x5tOl
zj{RGyFRoEphii9_5|dWi^6?jjLiU&4qE7b?sDCvXPq!{0xr&qcZ?hI=DvX2iPEX+V
z_jmGDG13lUeqRpgGgK%)Z-_2Kh6p+_pW)q!>pZRbDX86<%ae9lz^##y82P<`MQKLa
z+ol%UId|hQ*&)pK@qyz<y&<z<r_kq3FiyVHnFme#0(GPMW6phvP5E{*>?bvR>-UZ}
zc)M})Dyd((JwsR-XU6u|H{qA;UNqDr6}(<bEY!dypnh$T5c);~!%eEm@2x3z>9Yji
zE_g^2h5#nb>dD`ZCWt#{%*44ZMYO*C4pFaAf%DV_wmJP$^iH+H@i(n-^OFpCaibGL
zCrkWnS`S9O4r1hPW7OE^OC6>*%V$~@xVs-{lJDzw6jz?=goUNv!q2gK>?e6vveqBw
z=at$x;^{qk#jmOG^qUeDez?jZ{>S-|{0jxFIwiF0*BxDlM513nHL2zxzxX-?TFZ~n
zt~XKGugU_q{mZ23(@k;F!ceY!dmPIqIm3lj3Yhg<1$Q<#iy!YTVw+GEw8vxYxMnnI
z-P;JV59dHL)D=JOw8!jqlG8<DE2-3fp>Ex6@$22El$Np=GM|p%-U<eM<?le2*Gz%J
zhxyd=Uoq|OC=+WP_fqydgw_tHV9Jk|5bPDm0o4tlljp^4dPJ@jzv1FcI~JU!os*WD
z+~eXt@zPVGWeJxdPDz(5basOG>SHk1dOY{pY>w61H9UE|C+e(<XOEDRAoozh#@trX
zwnq_^%+eF?)NW$C=eO8unZzK_V;)kK2px@`@a|4cep>sP{<_`=l?vd(x->j|E);`0
zt%9&2<FSA6TK+Hp|6l(9zx@Ay`Tzg&|NrIx|I7dXm;e7S|Nme9|G)hI|DF8*#xpL^
zIJ|(yELH($??YfP{2Fa*uBNF2@_A-=9sUv2TiQe324fmZDJzUH<MbVl?=}wqt&L-k
zP{h9~MHKHomHO9p<@63)Y1f8z6qF!2X8gy)DLompR2LGSGztsVrHs_?!|?svITA*W
zMAhX%WTjhyomVfS#8HO0v#ta8n%W!xEwLdRwJ~^P+C*5gz(Af}n@&gmEJWpz9XKw^
z1(o9RY3P___At02XkFOCrypA29BGC-`n>>dF&#0iVQ%Hm9f_#jb_wP@`$DtNt*6bF
z9k^BK#P;EB!s;Fw6nM;#Ynz_a{oNNu`#arufL=fBZX3+8e?|#?v!nQHXbLq=-o&F6
zh6}@g@1$;Dy2(t>JD}z>2gs5S;MEHa@owil-u_6D-V}RNbJ|(D()fyO7HoqKv<ZIp
zNTwNotnpuZG0k{%jo#>V6}>as)6PYe^yO*_g`IVT0j}1(ZC-a?yZscI`qXmZAtT0R
znY8VuuG>AM6=KAUvoNE21aBxcasM*XifdFn+;=NIqxng76zXG)N_(})e9~wv7-<QY
z|8(aX4OP}%70n+kQ|Qy7nUuF-FTAt}2an21vd_`Qe>*R8Q(iVkg&n4SZ`FlwiMMf$
zx5Am_&xgUrtpho{-xyZ!GZ%Y3(V)yVYG~O~OjBbzVQWh)c$K_^w+*@Ss#_`e9_G{j
zyZ2E2-8d{O%o1Avy`kGnGKGX^ow?N_2NhTApxdM|D8zoCfAxCsST<bjHrbnR<PQ{_
z<eT`((mh-qcAGw0T&5J`9{hW(iLfK!p;&Drqff6E;~9?uIDS|GsI2}9JHw6$^Hc}%
z`r+w#&%}ubtO>>x$8Yea(U_y}>!H708x2VQ1liFlFx({vTZ~0|WZ{EzC*Pqg&G}&K
za$d}nvcxv$)j55VHcbC9i1)R2!C`??PRVYLxa9E)DEL=R6|!3Z8x=4{xeewH%%M5m
z&B<<1cfNgl5HCCO1V&b#px$em1f41R0N)&Wliwrg)4iEYrCh$~>wx3c`_bn^g8--u
zK6HuT`lTg;urLY_$L*#b&1-SFlqtT{c$&L3zUQr33LO49n}*EVBJ5hQ5kDuTa)MWH
zF8?IJ^lD@N6BdO<d)sqq-a9%uDwA89ec8z@w=yyF2vyd$WBpmV^5W5&yw}HzLd^4V
zTHb8dEz!q6pNy!eCJDsV2hsSI4u0MOILzQHT{?A;{vF;XbRTam_$*cv|Jc~$zlR;U
z=2aMO)3%e?-~CbO9Z46usp9_SS)8kFft@dX6lR+3qNeCNnqj_=v)b+A-rv-@{`g#4
zyZ$nqGM^2;y1hiZf>a3Ezm@xK6Jg()13W|e8<ZZCQT`YCj8zNa&eG>JVbODW$MB8d
zdp(dUbewTx#xel^QtFYrTgV;}$;BU~-oz>oKKQ_p=iSSsZW|DPwjT)3Z<V87o&so^
zspHw7f5Gn5Ws2&SjSm;?r*M;StY2opKRa|Ivo7CBC3z?thQ>hII#s@WCWUgIj^`u6
z0=w<9=Sjofh<nvzP}5RXJUn1J>YaW~;v>m-Ic5kq_8AR#4Z3io_gp;mRgL>Dx(ZV*
zvUu#pGQo29Jl?XTiIPqq7k^%Dp~?NHh-KIc%0qt%E(SB<;dSGR1;e_raPm7TE>p+&
z)@L*!v>e;4ke3IX28Y>YH2I$nops)YgMSXBY$I=6xp^mfIgRAGb4G*t;WD~@$C;{o
zxuI)gH~yI)gXL}>WF9yG2WLcziA8(lr+)^Co{vk#8<Gp`_=R{<a^K3QF2~d4iWu6|
z*%T`(%s4&Ljy8EF(6>Q-`I=rJ8isAau^ZZ`wnhm{Zf)gB1LJtpl&um^@+mEOQ3&~s
z%Y;8qjoHUtlNR={Cqs!vXXyVFVzM7l;ayiYE1x5JWOin&!oR|>IU%TLkOK-<maH|~
zS+>k~2dDa-6mI$Eq0N^K82a%UG)TPQE4}hL(|HsdzS}M?7`{uIcb^mvy;s5kr?ybD
zwLqeyl=*+_&7KE!z_SmsUWgY3u`U|T%|weZ3%s#$9L~2iaS!cN3E_8FV0@^=t$kL6
ze^<1EQM4gGE&U3qD}C{f!5vVWKN>qL=5XqbV0x1@od<7WDz_YreJ}N<(g!XG=a132
za5+^xHiqqy_B`<47U=Mz0WMGJ1{&?wV&4m$uwcRt@ITgpvU7THr-L`h@w4RUxR@ub
zycK~n?ke!M!}Hxg1+5So;~&%2-{;}Rz(=UrV>}m%3b=CFWy;w<iIZP$7dH|}S(Ae_
zZ%Q+qZFfoP&Fa88gN}T6?h_bdRl?&dK9O5VtUTq=XwIrq#qohdIDLW&esgQj=IgiO
zsXLaOebWN7GMw1A`ZVoY76}G7uTxkDU4FiG3l$AaW20utY2L>|2naMmslX)o<_?Dm
zjrMSU+bzo0u7Zxs_d}GPiFoUi8i+yBG$5ds=KI<3l`}0Qeo7{5#Z#Oqb)JrNNa56n
z>p1abI(7N1hXI57@q}~-$^D~<$E8jgrdOfL)<)UaNL`qrd_uUhr;Zj6J^}6;34(u7
ztN1T<DP5Z}gd_Tx^NGwRSg<gI5{(29%%(tfOPu5;eJabYOLJfQEP=8PdtmvJ<+MY7
z7VZWqu%Ff<bXC@1llx(?xTvA>LjErvSm_5-W~8#VY$Vl;JPBt;j1cd1Q)QKcLC|cL
zMCR)+(3o+Jbi8W-v|0FbMB-)We{wno{>$Z==9?*Gju*vFyUvR_5$n=A;<Q;kxOKQ8
zD(o@leJ7`p?Y{0j#pEUUpQz=1hm9fD)f^5xe-{%E2E&tCT{+-#DZBqW$b;4>A(-r^
zEsGk_{M$zS72yea-=&_sk(b2Cl$gVBE9j;6AYtAE3$*KFB)*XPVyDv@#Ej$w(%6}d
z)=O2;{MkIbY`zgz&ASI_nz|6ZARL8zx_I3_4-WU}N~&F(rDy#n@mu9Nm~R~dizn|8
zZEmXavtP$y_OE*ClQk6YJ2Z&LiVGmN-xgS3wMlYqAl_WAkL6{3rM|K#%w15%ACsgm
zZG|el+U_O)=yw-zR1ilN^q2S>efi(1UAXL3H2yh|f;q0cDEa+cxSP?LPsE;ril(hN
zvdMt!tPZ=EzwOFzRcyuFa3}m|^%vgVy$|-`iP9eJ2pF_(W$|7XzR_973$s4U#v3N^
z%(eZwsbG+>S=!6JUXUqy<6enVA6=jjr@g#jjx*GMdq}_DNxrmuyC~mJ6@1Rw@Y=a{
zxV>*MoHA|_($96owN_(cWsEJD-iU?{=cdX+mu#eU(e>hUU2kzj;u*nie>m)zA0TG?
z$I|&R+o-8dPLFi^A<K8t<}QI~I{Q8qb!-z79`uH<4I0?upuzJd+k(QR=N#dq!2aP?
zvU$Dk!8hflc+<ods*1F@4K4YP^Eq+ofHm@o;ag$<ZfReh>&PXA@94<m0D51J_+*a;
z?C}jnqmW1NyV9O~#~#2Eb9F9y9s)g=In%F_Cc2?J2x@!i<H~Iv@c6yokhjMT|Ed<l
zzv#0<z5RM&)=L@Rx2`1}YZvyR1{mFGj2O^ynDjby#cS&`Xi9oJ+&5!04F6z+S4^BK
zNjDK=-|Gtl48OprY#j)?{7zQf`!lUy@mJ>ecni#JT24&{a(cEm9~RmKbI_YZ&=PbO
zu8-RaGq0{CKkH5CzSM=f>|l7f-VL{0X^{=rS|#uBVGx))W(p>%U+8IZKlD1$8|Tmr
zOwn79&Z%=`NiI|6MF$f3_|;O_|M;9>EaggX_t&P9n0`Fz&miuulS#tQEtt6EAopG}
zR=DNOWS4h>t5o{p#NaJt|2>k9MfBj+cDcgM@_XVol}9kCp#xrRy9JSBV!1M`4@MRK
zrC+}!S8ZexEdJUNC#X&3y3>-z^=U2j>>Uds19uC@&6QE_iawvdVorCSO^1nJW@Gep
zU+n*QIy$BY!tw_aAHJ?LhW2ydivwM-#=j%FTUEjyD`%XtN$S08-i2XP<y0OK3RW)r
zxN2EHIGJ{nYF?HLt>;gJ%Y=9|4%rR;&spP*dk@KIu{O`wH0A6!y)fjIIqByPz@LxS
z(EFxpc(b=V+S_fz4*qK~e|t2@7lsLLf+H3=U59@g^zfO-BIx$sl5438ZE{aXn_e@(
z<?$d~_|u!Q*qc)v?fFZj4$eD3;1ON~!O2SGSE`1K?yHijPX(qcmWw-LHj%4Z6Ma`6
z%6Xmb$)YS8?wh!QRVQ^we$)}8b(zkrkqf(mWn$ii5_<doD8`$b%9|DMV$+zfG;XK@
z_RD-iK}E}CZyl8A!GTV=s&_4=Uv<F-<z)~%#tziZX2P_}O7OX`9Uk?yp`Wx0zYjdf
zK{2mr_?;{CLpzSwl=gtB?PsCkwGvYMF7aa3vdHsPJ1%y3NrMO0!OOl6#Wt78VwKTk
zQkpuMv&%ZMp5`XFcSDgf>Z(9*ml1pT#FE9ALDcPyU1jbPYlO$TG;Qk;(J8Q62=X+N
zzjaf@6;9*iHa%)Y3(tpS@XV3q>z|T=%_$+vZy+bd52L&9_X);d^GNSu6Fgo0OMX`O
z3YBp^RNc|z)HOZu=dbqsJheZ%J&q9igwB(4M+RuLuO0VQQbt+vYM4KFuyE|~GMM8v
zj9>O_fR-46gzg8??DKFsg?mI)8^ZI}KBl!UmQd@^Ub?3a=8%%Fw8g6rPaADOAzlSI
zTyiuNO|KN57a!nt&(mqu^v#lI&j!zX=)-S^dbpZ4hs~d@!#i3=ocZV|)XuIEqMRp+
zS#LrxuV@2y9lsROG+5lbL>p2=?RnaFMZRfmBChHm!!}=C_?2-w^}ckB?>s#WQ#-w+
z9L=7>W3&D^Q{t;nw74RE9psJwyq!41$rqe{?4-%#)G7CoHg~#U!EawZfQmy~#fbZh
z(X}l`eEv0-zM94J;uVT)sdG(y=h2UI8wbO+sg4k=mCAF!=i<lo>9j7y2?EzA;@#i(
z;l$*{(D_j=-qaaO1ygkJx7pjuck;ntF5O4nF1!&3{e3BnK2}fH3-7{qzZCxWZx3}%
z+sxU;&NS{o7hLPyBt{RuCng>3$|auL`An=L{5kG`H@!?Sr7;t|{MQME1-In4W0r{n
znm3A#HnDK;eiBTP+&MQ#L_z)AG!Pp;f~wC{&e(YnRF2N&!?GD%I9G-Hm*;bP*9`Ji
z?#^u|>tRQPNS~{dg&?gXqGghFHYYFfLVtU_zHtQ{d}WGb&8A~m{Bv6NBb{c&cL&4P
zBo>cm3mr!_2`Aii_;idJrLKG`u6urr1J=K#lwDJVFJa-dNx2H2emBR&6jj-leI|0V
zF-!Smc`Sq-T1KC`9fdc`ZjhLo%)aIG_<EN}a;phvi~HtS*82$*Zt&raHdpZLn#<Hs
z-Yi%6*AwNgdK~jOgbr=W7XpHg(zc~F1n*A6;fLGBj03fBWLYJ(s$PRt2Q=BoWV<+_
z*$H^577tMl<lv8cASkwiMrH5Dv+ou0u6-&h{oKh*Bd4NMs6QWgG6Hv2PeBi*4tVac
z5&l>{Q#k%zlP`V;IC~vgtF;JhGPlFQ*EcDuNsDY}?+_dguch+(B~m|f1bU815niqw
zPe+yWslaa>Y?(TUK1C+tZV!<()@X9^p!bk)qCcJc(H-|mGZNK?&U|e7Giuu4&)=1O
z1@zu6jP<d^l_{}sapGuUd`S-dRMMeN)heJ7V}ZrfEWpc2%2ls!AZ?{op0Ic>B-}c|
zP4}jX&pRt}g#0MoZ@-aThWO*Mj^5&ovfg<4tsyK~UI>pOI$~_s&s3s9@MPC%uAS>B
zl;3y5<*V-suWU=uVE9O$c&r_zC27-g_2Xc-EE^2}w1`H_u5wMH7rzVh=U6pe@z2Y6
zh;=?pLFM^WH`a#B4a~4pUlacQ#sKrJ?$fH$0^z&o6ndhtORzgQgDu_cXmPg|GRqFe
z#BPpo>0v*zFmKPrlgfCRe;>9TVGeq$I`eW}si%1B3q?2gp*~mR$f;!j?Djt@WhMQ2
z?V>c;JyQ*RYc+AXk_BASXmfuu@-Ux#tO*-G6=Ki#fe3zUG48-2RP%kn39GZ9Z!cqs
zUt-Po$9xi^;!N<b<tj>!S%};2fLNwGP>lb2jHWBN;-}aa@uF4=PFh|ml$Ax2=wD0^
z?@RNdTO&AuKfxQ);#JP8sYAzW@TtpEYFK9_=sw*p^$JCj3^n+9S_2e1cZZ*2Q>aZE
zvYD(K&9kp`<bt-Xg5sPQ-oa1Ax|y#ibE7k?Y0#CqbbkRQ0p9p_!x^d{<AuXd6bW7*
z)wwoEAPx1^oO|UK58G%!IX72v=}R3_+LR{TYL4Yq{oV;neoJoa?uz91W~f}{gB|&V
zn?tk2?2q52EVf-3iS}ovalOVNeyG`xkKL-JS6bb1*8J_bIAtZ2uH6Hh)zV>8*(q98
z9*+e!8_B7iADwD`05A6Um(75OFh%km-n_n^O*-y|Wgqpp^FSXCcqGG?)+^FHaV-3F
zQ6hz>r-ZX#M$^YL3$Sa$ZoYnU8CG?+<1KynL8GUXEsx$Ih72nbOC}V^Pt4s(*V1Fa
zz3P_eV4Z@E;}3HBpj5P9bYJ-4@LaxYLny{uO@X`%uc>*g8t%?i!>gZX5goV24i|%I
zkF18gy)DJe^o_9OVp!$j(B9HH{}$CO7tr|OWWGD)DMZiy2jkMW2~}3}(A4P}Oxspm
zX*#)&c;NI9M8`iu&lv-;OT8y{|9OMDbl1hj|1xD$2KC3-1UpQdk|VD9IUYYHIgsbP
zcIdPx7^A-GkcnjtJssSRAJi@6($8afcH>1DA3lPd{mn3-vl4bqSqsJyBgEk@&uMIA
zl~jA*Crol2j$dUNIAIrqT79<Lr1HbGaQjLAm|QFjesY$_pKcH;l!oxZ*i%&Inn#lq
zqu|)Yjl5%B5{$himtI#lxK(Y27xyg0tD~+_+pByS-hCoF9^U}Em0mPDq6emD6+*(K
z!z4L?g@3Brn0V+Yb$Dip9TVK}+VwC#WaW=W88*-w{DYn?EP-poUFrC!K`<)E03F=w
zadGMmSxxQ@IP%_rhnhU41#jxfudg=h9@$1mDkH>uT~-U9zdRM;jUyTj>n3G_k4Qa>
z$Do)vnrccW;xxy2)Y|!4yi}5k>!-8{WkGRp^7T9z?{ZAM5Ge2%<-KCG)R`W)yg|&_
zEC(IiVN~FwOeRl;3F?uq*fZjx_<c`4EFUuo=Dsr#q=6CsxICV$t-9c@1H;(cMuxFB
zTx1$cvru9BFHyYV1^dTVxIK?w1IzaI!(JM3f=NjaS?jhf<Y2Fb`;5n;_;Cy*wdi5q
z-Vkp1QUX`>6mfLzBN%ux6BI`wuh39{yOr~Ku*+V2p$gpA@Sd{dEo7zX%{BoY>EE;>
zn6Nc~qgQRDi@S{JTYQWVR?`ZZSMz9Mt}<CpTt~Ow?}HQW3HZ1^zOudSF0DNDldM(&
z_uaDtb}NsB*Ew3a;pALiy3iH>7#7nWbw&K4B9m1scL%?>34Ep168jB&Mu*Su2DfpZ
z<Xd!=d`kCI`K@B{W0XGWd$qy;&i^6X)bT}}28S)%4%*Z7<qAp?lI+GFDBWU9t`;WH
zPa$4D(JY4cY`F(lmf6tX1>b0_+?!W8Y4a!9eR4VnSh#E`M(Y0p$HZH7H>oQhd$WO$
zpFT`mudbtC1AC&IhZ{^+8-;IejPPBs#N?0zO%&Q==XnWK5jjX)Z}kP7n~NavTC7Y7
z67hi7qW`(QAXn>{(37{2!jBiSZFy&*`L+YPdZb}+eMk2B;(}4J8eFb1M11zOvru{A
zyL`IVJ(fEhr4=cmJZ`9*>d*prv}_XZX%mg<-AMj512~h@s4*doQs{zUrF===_0=D^
zX%++dqckwZA`ADPnIWk5Dn_NdnwY6%#*e$HQjK89I3kdp)=ZMP1+SnawjCQvS=t_B
zles`Bq7vUlpwP1j9+}-2d>)4IpOo?3x}sU!7ym&V6me5%3D3l>$6~lH?g_lHzX6eV
zPm+yKZ#;2Lja*&}c)miDSKrRVwDB53@pDDlis@N!=VS?FOmn0QyPr{-!Xi|9|A2Rt
z2jb>+x1h@XCKUR&LF$%xT%tUR&PLi$yr-0F%W9;Y<ezkU;T`yS<v!gXvYToGs_E9_
z5_l+aAFqB1#{qZt(P@j{^tyvBG+sy)_jDH^q~JKdZaRRiIe@V{!tvzPSF(z%EGjn8
z!-l@+=&MhzATx@BZ@&{cf9h+vx1^aiHs7Z9UDePQXR*AlBX)h_0`G5{;M&-^cu7MQ
zm(I|}8?|xbv88|q6Qn!E^C`H(cQViG)e6^)49IWkW6+8JOA|+IqF!PQrx#mt!O;Vx
zn0kmpWf`E>)Q>xQDv`a}HaNRikE1)R#2w8VIJN1sXmw^MPBH!kE5_X+hhuj5Apa=!
zj#TF#&6%ips0Un{yc;$=S3;FfZFKpDAH8YL1=r0jLd%Fg=;AAfX`{NM_PiUAs}@er
zU)bWbFA8j0`9c`rwFEu)jA8@#&(Kb<g@T@p=Sz}Py|7&$`1b5QgsY)gbw3vNz6j^q
zy(zN1%KrGsUKd`)9>C?r;bQjNC@4Fq%`QK3;J$@BTCB~6-UcqH+^~y|H=g5`6={^O
z(2d<1rirHOa?wn`J+*yM#lbB%$X34-rC9H#0TK4Hh<^K^F1Q(H-Eqa*v@p=P(-Chz
z-AC)XghA4$>tN4kg`08fz;N~)A%64_UVr-})Xcph`Qa^b%58Jp)Gk2WrC^E^TWV?2
zlNh!<nGPns`*78@?-2T}42KTf4@cA1L(6<cQTqP#4Xa}mX)z7gz6yjk$)g!Melm2d
z*(p!@TnOh@xJwzx&Dbrhgu4|l2M4{eJZ4Qh@j~ReN>Ax56SJ#YH0yPYzUM090pmaP
zMzfpbz+H}-R=*(AJq*tsJOc{Pa%i@`GhLjT%iXSCB4;~Io;Lh7`LAh*MlbE~MfH1#
zsg<J`5Gt|Uo>N7w73|%)j;#mH<h+01WfQv|gd1VTq<pL=b$zCcc}+QdE+ZLLq<Pu!
zmJ#@FxDMX;Ylne%dvdhb8*0re<OiLjgcbUhTtD7U>PZ>l`WtUZdwU>%PEV8DSUzRe
zm@4j)yxH$PH_)hMe*{jhz*56XF>0O&D;!RuHDz)6iEofp(1GX6syJ=y5i%&6z%kZR
zXIg)jc=Y}%G2DJIPujnW%}VXDsP-$obNPu6uN4U48>)qhzyQu%{~lV8PRIL;?qK%^
zi?PXMH}vhj4qg>(6TX}%lH5+oRNCstrTUWZETaj&`AajwSbwbewS$^o0M1*j$;tCV
zu&%=f@HrO1m8q)Y`ynxGx<@)|c2>vVzmCG>PEqi5rsVAQdPl#_O}XCCj9uRXmz_UL
z&}2<FZc4e^8Lz}ux8lH6m`G{TJtm<jhIYE8(DvFknrNcWN0lRSgTrHCV^lZXW#Esa
z<AS+%gb(G;_zM%aAEmH&UJ?`LHVn#<7^RuVDX%yjA6Z=zSti5s#YZsubVnTev?D%!
z^Z>^8>cy7=Bp*=jUSYL@9@q|jA($>q6(3Jd;)o%Ks5&En7yF;0k?#$JY@fdTdys0y
z)&fA~cfrD|+_8B1dN9ZBbKw^4dVVp<RoG@;KzH4;@LX0hJSZQDKfHp$ZB)KcwtpCY
z=-m^w+8GF^y0ri#tFnepF7NvDgUUB(^U|C?RDG~X_@I@5D&5y{@6FF3?$BZ?S!cit
z9IJ&o-JPVHGfoKB3FMd2bEN*<Mw;%qQdHV6d50g2;@Gdqzm<Up`OEn9UrpYjUrmY0
z0uHLZOYWWpuz92g&bG{>s~_#qc-B_<8g&q!bUr|D5|4r5<#<tlw=-NTX@h(EyCH0W
zCNvM7K~HtVQGL)1?yu?%e#iUsX@5CK=q=@I*UdQlwjnouE0?c#nhdd5n&}_7bD~-?
zX?~aS*bfeza_A|zYo>95${~7s@iU=cJpGMHhDi&TU}BS%a14@Qb%-P6R9zJldz#=Z
zgY#nIt9BBD@+SF~Es%WP1~93&K71K=TddpPMh9Ze1<S%2|GWRgvxtch*s_QNww)6y
zeq@V<*Vc=UJv!h3Oapb7alBu;n_V~@486z|Z)6O>%~7XeseO`gNczF}7Z`|hryGI)
z!S{k`;1kZZwdcOwE%?jp{o*yJSJ>rIgW$K|AFR;oh9zxvLht%*c>nMPn0)cQ#D!kS
zm*4E-$Ndk$`gR#?a)HonQZ#DqmC?1M7MS|@4V_j3{=V`d*!)NqI#}0;r~4N|=&EF{
z+_8)DOp<u~ka~LQ*@c%>50&+8?ZJzaK7jSTLJE3UC!5;Q2(m9LvuXKHvFUYt_TN)0
zM&IiSwqfn){D>GK{bd)<_>sZi-qwm+Qi`eVh8-$pPsHLG-SPEI6_`ED0q>4kMMsiL
zz+(P%zSC_Wf6E=g%O@p)Ok)yO_uY$ojn<-Sp9oI<uwT~IrjEOPmGQjXQn+HJ!5vbp
z(C&;WSTEl$+)vw1<7Z{T^=`k!S&n6}XGT4gDW$?mX&-Ysup62U-i`n2*W=uMWiU9*
z6n%_J=(A%21h0Dr2h{yw>=R@7HzbL6Cf9&#?^3XBNkez9xl)HT49|A2c7ND739)jA
zFiqO~>)Qr_)r<AyyLAyc%^nEJPZ<n{oR^=TUJZe-pOE6=<Fw^gAVs~?WJQNO$oQ$m
zyRYmLd>?qj)uiQ=JK!ih*KUui<o7AdItQW;9fYp?T4>wUg%~z*AB|n{k@`EiQHG`^
zYt&moy6OkPHO`!7jqb~1uD9n5-SPBj?``O~whX^IxXaJF%*DEEs%&?&7fx~ahVs8D
z_@W}3rVg<alRKTGiq&dt7xji#cd!SyV&buRy=e5zsXWcjm^+nThEM%oiOZKKq3z5*
z5R#INpFKM9?5(ZTJjRrVT8`k9L1s|jFcGZ`Z_xYXJ*YnK12qH`2nW`9<HzDY+_|R}
zo~rr`*-<-Sk;F#ww~xZ!dNWx$eha?sbwEfC>53NzF2Vlxm&GA_Z8>~!C-Sx*${%f}
z3+gvCX>+m}uV{;h)^k?eJ8vPL`=!BV`!mJYi+1y4ndCICGT=Vf)X}NKCOBnhg)dz^
zX>;Bxh`Do>)-*@R9waEs{&6*JEp@@Z3w!dCjc$;?E|*rzYPrLWX%uRo$xR>Qr2Vcg
zJXrCVGOz1!-6h1#53&4o&v|$?=QI6!>w!T#Q^n=Ir5X00D%w9>pEJLB(~t&J9zHDy
zA7u98Fx`RV*kHl+-d*{7LN8G%{Tljr=tFxG7USxfM*LH94(>hX%dZxw;uf7Z;=$%n
zj635(*H1S|e4tW39NrCFQjBQH#fw7slz6GjQ~@TAnQ*so9Ne!z0vT$3F}~#hInVn7
z-klUU+U%lyh)W)RZ*|3;sd5~2R+Fkrf5OL)M1MoHalY0#w7;_lO^>yKVUru??3bZ(
zeHY$t^-FlS=qm-7_haWVTPUaC7<8HRoNNVEZkSZyF5e-|@@5YdXBQ+2(MSH!+lnGy
zd3+ehUp@^-Jg!iF{6-Xdji(oF5$?UJyYhog&ZzmM6WFT9@Rj#k^du>Y7o==wuk9mn
zz2Q%iM=#>Be^aSC$Bmqm&2i8wJ>(4sux@q)(~d{NIqOv6*3}?{Z=b}7H`_6Ad=`I@
zNIDrW;`qTv$%(DEpU<W>g6irdl6&}oqWus-dv+nrURg!I{+Q6?Q8nTMmugw|Y7dU<
zcArM~m?7@)tfuqhZbFmMTNt`#IopiW5x&j;O&1N?L+`(-;JB_8oVT8UA>(r3tl~K^
zC^`nZ!3x~J?t-jKq!};T(?SRP*a+RaNIq(PU+fXD3T^-5FuZC8eNqU7sr|>XMRpem
zaN3UxFHEDnWg)os!yKN}rG`eExbm@$eQ;I(jlAOdSpE={L(gu1BZu`_5I0~KDsCCb
zu_crE=8ZIARmW_ecfb>~+=IE)Zxr@RDx?zC3hXqi1oz2Q(cWMqJ=tRfPJ@j2D;wd*
zL(w?r-3&?}mI6yBSvu>#Jq3orpM_7G6qy81{_!c3FIsitZSAsQ_`@V{YtRAf;WoJA
zOE*gE=Lj2(7E`F!FHtw;2DP7)L8jkKabVyKR!nFWti4r1&mos00!*><{&49m<svNE
zv<(Lin2m8|%OJ+|8I&&R!ed^@C}?67+ih%u@Z`l@{BNhAprtQ_HBG=`Z8b~`xdS=_
zT$wwb<&}3`cx{m6QoL%%m-MwgRD755^#!5WuG$AY?XuA3^Eq<#Rl&7|W}I}}gW=0M
z_!E;(d#kHxzMi6Z!BrnW-jKt3_ieZdzd>63DNvgjN}nV~?)BZ(;%x0<FheOuj#92-
z>C{V<;JQ=F=cI$}j@!Ij-3Qg5<U-F%YcVf*E~@W-AvV-phr8OJz#}pei)?-gG49V{
zhh8V#W$}v?ug2i6*RlM@NDCB}cwp7)DOA13*yE#qtayCeNYIg(SQhthQm;pspdooN
z?D;m5k3RoSH>_L4*>W{pP`?1<pNzq<n|k;pVubK@Vj9fSO|Hy6JeXGmC5e}(wb9Sj
zCYXf&IAUZeZIkx@@9v*shsp#T{;-CQmdB!TRR_KzeijclXhE`3GT34fhMK*kj_O9#
zcXtZUU%rZ;F6c(tUEWgJF{MhUvoel;lLHya{=Dja49v6|BJo<>X@IO4%!4J5_)0g4
z9~?}ZyzbGw>z&x;{v<&;<EU#>z7LfiPonq{U7(NmBT|||;^Mk?JUpWlB{V4UfUf5O
zrG3VtU&{E<Z8Fxj+X-@Mrkk<YoK@b-IngXhZlQQrAX~}pe#HYn+eeT%P-1foNWlHu
zGvSwXSKJ>L0h_fh@V0-NeCyeEvhlZpxTlMSOnyYNjVfeNa-7PqnxNu18+5wxj0UK!
z<prZUaanu=ANVnntZwNGd*)elwsIo=)7?r_{upCj^;P;YF;+M=doa(G*TXNG$vI&m
zH1<OWHiKa4j%|*9tCtD47g%A&rhK^ESORlkJ%;MF(cnfybg2Kqje)OuZeL*4AHQhr
zs6KdTcOw27{73YCyP8MXgox`R=kir66^!gJeb!z-pbTRJG2Fw1{Y>KovxX>Cnz|gC
zl0(Gb^9`_N&kgomD)HnRCBC)f^mKB12sKlm)0dBpytE<$-}}v`3y;pjp0-lZ37AG2
zpGLFEkc+|}U*LDjzT)oHi}`VFtgu!1NJ%D-;kws3Vc>^Iu=TK^Jpt0(<@QrS)8is_
zGO3rBUtP@_UZphG-jv-3Zx$bS$N^{nDPZ}vGb-P*ljRRJL%n`F{CI6LzH2ZN51c=U
z2j4fq(a$R}=yfTng~Y?1P$#zVO5$(X39Q<)f?t;mkT@$S+#RtP&y{-OVsQrU8~R>0
z&skjvw$JCTj~ZnY5+AZl?_1PK${N})`6SjBEyAfMXQ6}gN3ywVh*Bw?-ksS_e$$4*
zw_blJ%y%|hJ6O={zl}U^+%B==V-Rb5I#G;m7FfD%0j;41uyvA|#BiJi%HL0s_&^_r
z)ZZ1<-c6)Ep}oZ#-#ehLF_~r!D#ZI~{umUJM*WxEfzEqO>BHyu)KzYSN6dmixn(7{
z+oFd0RbJe6`%ug)>nxvfdN58jn#CIVe?-gqYw5_K1S*O(ru6)!lzk?a&kXJ$sAW6Q
z)YJ@Yp5{j1?ybV0;T<J#+*f+>)(E%HcIBe3p7_So68(nhL+#{YQeW{S4U%%KFYctW
z@sbX>qOCi>Fg$~0!f?nM-W^=`j0bs=09O|mLWd2%1q-*!5TiGa^Bx+2ZM&WHWJMty
zk=fGi1w9c~9HHFEUh>sG8%gXk=YR7*4puW_YeyX+;t-(9ekW4vcNbHARoLXf7|d^R
z!Po1u;HB|-_*hwrM=NL3!~zZ6eqINiEeG=T-}`8y?iZmfxI5pOrb1r2Uij{5zNpdb
zDww=dmzcl#G&<XXO&j)--;nmyW3m^gJ=%>YR@VpvjV6+hmkd=ixA3;?o8+iJo+qC6
zk#Ab!$l<LEKq=>s?CAJ4EKXZXuZL}-aXa_%B#CF@{Mv~--u(&&4K}D)pn!e1T!M&;
z{^T`cCXd`Cxi-3nimBi3LCeWZRI+?26wFBk<sI+n+Of41I(rju+<ua7WZALHjRTAy
zx1uax#%j|Mm6w}iOxYM#PR*2sj(I>v;wElC=^)%YW6IkfZ-8IAozTAel{hxk3XPAw
zl((Ldf#ci$aA$CT{5yXGonDzL%?mElrRh=pu4WrWPcxT0r{>WX_i6a%jkXw6@<2$Q
z=`QbYSP4tcyV9+c3y|kyOY`OF<o=^0OuzzReuEv(Fn&){tka?U>!W-@;+}sldrt+&
z*YJ;=7*JX_mlwy@in3B8X?A^wrsyOK_UR`{=gbc5wPZhi1|3`!bcvSQP2$pL58>}Q
z0fYUcQR#Ia*8KK{J{g*PP<C8+a%4Rynhk@(+52F{W?NSL5Wrf+Gr8BI)3A5p8afx^
zjmIblG|zQFBg^Y><!7*P)%TpR>A+|{JGnDP{c8d{jac-ktPzLLO%;16JY?;_{;YC;
z1O(sA6_Xp+qwdhlLR`P&;J8!|3x-v|rQ?9<>mCTL&Zc~K?L3U%){d7e&w;m=i5OH>
z!rNmb@PL68YYp81FY>jhuI;{1>fnSAw)G}!<$1XC(;&E?tHXYmBVmidQ<$!;#j#m2
zVgiljc^6gAZ2pzP!tP0=nzo+nBb9MPqdC6a(NX-gFB0E|r=T~)(KD?dpz|tU==V%t
z3||p|k1cLPu6ru1PW$0z(b%4ATFp_~uk*0KeK1*`B8>CdP3sLiW0!H-{9NOb`;7rh
zc+OCLUKt}7C#B3`)7%`?UYtmWJpaPdsRPh$ZMK*<#T0{s<J|ANwa41Pt+c53Hgeh8
zBy{W+$j(&@!6U)~RWG-ShhBBYmb24^V2Kg2ZT30JE{X)N(iF0vs3@j?OT?CBZ5};Y
z`mSk?h1p^!R-5Gyn~Kzs$`XZ|)A87(+70W)Cb;#fBN|-lDY)9a6OY-BVJou&7~yf7
z)_vWDYt7AZR{x=J&vHEp8*bCH&BwX0p$wMX+|B>x|NqPX|Cj&&FaQ5v{{O%H|9|=a
V|MLI;<^TW7|Noc&|G$&}|9>SCF8Kfe

literal 0
HcmV?d00001

diff --git a/lib/src/phy/phch/test/pmch_file_test.c b/lib/src/phy/phch/test/pmch_file_test.c
new file mode 100644
index 000000000..4ab99b560
--- /dev/null
+++ b/lib/src/phy/phch/test/pmch_file_test.c
@@ -0,0 +1,204 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+#include <unistd.h>
+
+#include "srslte/srslte.h"
+
+char *input_file_name = NULL;
+
+srslte_cell_t cell = {
+  100,            // nof_prb
+  1,            // nof_ports
+  1,            // cell_id
+  SRSLTE_CP_EXT,       // cyclic prefix
+  SRSLTE_PHICH_R_1,          // PHICH resources      
+  SRSLTE_PHICH_NORM    // PHICH length
+};
+
+int flen;
+
+uint32_t cfi = 2;
+uint16_t rnti = SRSLTE_SIRNTI;
+
+int max_frames = 150;
+uint32_t sf_idx = 1;
+
+uint8_t non_mbsfn_region = 2;
+int mbsfn_area_id = 1;
+
+srslte_dci_format_t dci_format = SRSLTE_DCI_FORMAT1A;
+srslte_filesource_t fsrc;
+srslte_ue_dl_t ue_dl;
+cf_t *input_buffer[SRSLTE_MAX_PORTS];
+
+void usage(char *prog) {
+  printf("Usage: %s [rovfcenmps] -i input_file\n", prog);
+  printf("\t-o DCI format [Default %s]\n", srslte_dci_format_string(dci_format));
+  printf("\t-c cell.id [Default %d]\n", cell.id);
+  printf("\t-s Start subframe_idx [Default %d]\n", sf_idx);
+  printf("\t-f cfi [Default %d]\n", cfi);
+  printf("\t-r rnti [Default 0x%x]\n",rnti);
+  printf("\t-p cell.nof_ports [Default %d]\n", cell.nof_ports);
+  printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-M mbsfn_area_id [Default %d]\n", mbsfn_area_id);
+  printf("\t-e Set extended prefix [Default Normal]\n");
+  printf("\t-v [set srslte_verbose to debug, default none]\n");
+}
+
+void parse_args(int argc, char **argv) {
+  int opt;
+  while ((opt = getopt(argc, argv, "irovfcenmps")) != -1) {
+    switch(opt) {
+    case 'i':
+      input_file_name = argv[optind];
+      break;
+    case 'c':
+      cell.id = atoi(argv[optind]);
+      break;
+    case 's':
+      sf_idx = atoi(argv[optind]);
+      break;
+    case 'r':
+      rnti = strtoul(argv[optind], NULL, 0);
+      break;
+    case 'f':
+      cfi = atoi(argv[optind]);
+      break;
+    case 'n':
+      cell.nof_prb = atoi(argv[optind]);
+      break;
+    case 'p':
+      cell.nof_ports = atoi(argv[optind]);
+      break;
+    case 'M':
+      mbsfn_area_id = atoi(argv[optind]);
+      break;
+    case 'o':
+      dci_format = srslte_dci_format_from_string(argv[optind]);
+      if (dci_format == SRSLTE_DCI_NOF_FORMATS) {
+        fprintf(stderr, "Error unsupported format %s\n", argv[optind]);
+        exit(-1);
+      }
+      break;
+    case 'v':
+      srslte_verbose++;
+      break;
+    case 'e':
+      cell.cp = SRSLTE_CP_EXT;
+      break;
+    default:
+      usage(argv[0]);
+      exit(-1);
+    }
+  }
+  if (!input_file_name) {
+    usage(argv[0]);
+    exit(-1);
+  }
+}
+
+int base_init() {
+
+  if (srslte_filesource_init(&fsrc, input_file_name, SRSLTE_COMPLEX_FLOAT_BIN)) {
+    fprintf(stderr, "Error opening file %s\n", input_file_name);
+    exit(-1);
+  }
+
+  flen = 2 * (SRSLTE_SLOT_LEN(srslte_symbol_sz(cell.nof_prb)));
+
+  input_buffer[0] = malloc(flen * sizeof(cf_t));
+  if (!input_buffer[0]) {
+    perror("malloc");
+    exit(-1);
+  }
+
+  if (srslte_ue_dl_init(&ue_dl, cell.nof_prb, 1)) {
+    fprintf(stderr, "Error initializing UE DL\n");
+    return -1;
+  }
+  
+
+  if (srslte_ue_dl_set_cell(&ue_dl, cell)) {
+    fprintf(stderr, "Error initializing UE DL\n");
+    return -1;
+  }
+
+  srslte_ue_dl_set_rnti(&ue_dl, rnti); 
+
+  srslte_ue_dl_set_mbsfn_area_id(&ue_dl, mbsfn_area_id);
+  srslte_ue_dl_set_non_mbsfn_region(&ue_dl, non_mbsfn_region);
+  
+  
+  DEBUG("Memory init OK\n",0);
+  return 0;
+}
+
+void base_free() {
+  srslte_filesource_free(&fsrc);
+  srslte_ue_dl_free(&ue_dl);
+  free(input_buffer[0]);
+}
+
+int main(int argc, char **argv) {
+  int ret;
+
+  if (argc < 3) {
+    usage(argv[0]);
+    exit(-1);
+  }
+  parse_args(argc,argv);
+
+  if (base_init()) {
+    fprintf(stderr, "Error initializing memory\n");
+    exit(-1);
+  }
+
+  uint8_t *data[] = {malloc(100000)};
+
+  ret = -1;
+
+  srslte_filesource_read(&fsrc, input_buffer[0], flen);
+  INFO("Reading %d samples sub-frame %d\n", flen, sf_idx);
+  ret = srslte_ue_dl_decode_mbsfn(&ue_dl, input_buffer, data[0], sf_idx);
+  if(ret > 0) {
+    printf("PMCH Decoded OK!\n");       
+  }  else if (ret < 0) {
+    printf("Error decoding PMCH\n");
+  }
+
+  base_free();
+  free(data[0]);
+  if (ret > 0) {
+    exit(0);
+  } else {
+    exit(-1); 
+  }
+}
diff --git a/lib/src/phy/phch/test/pmch_test.c b/lib/src/phy/phch/test/pmch_test.c
new file mode 100644
index 000000000..de7948852
--- /dev/null
+++ b/lib/src/phy/phch/test/pmch_test.c
@@ -0,0 +1,475 @@
+/**
+ *
+ * \section COPYRIGHT
+ *
+ * Copyright 2013-2015 Software Radio Systems Limited
+ *
+ * \section LICENSE
+ *
+ * This file is part of the srsLTE library.
+ *
+ * srsLTE is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as
+ * published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ *
+ * srsLTE is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Affero General Public License for more details.
+ *
+ * A copy of the GNU Affero General Public License can be found in
+ * the LICENSE file in the top-level directory of this distribution
+ * and at http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <srslte/phy/phch/ra.h>
+
+#include "srslte/srslte.h"
+
+// Enable to measure execution time
+#define DO_OFDM
+
+#ifdef DO_OFDM
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_PRB(cell.nof_prb)
+#else
+#define NOF_CE_SYMBOLS SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp)
+#endif
+
+srslte_cell_t cell = {
+  100,            // nof_prb
+  1,            // nof_ports
+  1,            // cell_id
+  SRSLTE_CP_EXT,       // cyclic prefix
+  SRSLTE_PHICH_NORM,    // PHICH length
+  SRSLTE_PHICH_R_1_6    // PHICH resources
+};
+
+char mimo_type_str [32] = "single";
+srslte_mimo_type_t mimo_type = SRSLTE_MIMO_TYPE_SINGLE_ANTENNA;
+uint32_t cfi = 2;
+uint32_t mcs_idx = 2;
+uint32_t subframe = 1;
+int rv_idx[SRSLTE_MAX_CODEWORDS] = {0, 1};
+uint16_t rnti = 1234;
+uint32_t nof_rx_antennas = 1;
+uint32_t pmi = 0;
+char *input_file = NULL; 
+uint32_t mbsfn_area_id = 1;
+uint32_t non_mbsfn_region = 2;
+void usage(char *prog) {
+  printf("Usage: %s [fmMcsrtRFpnwav] \n", prog);
+  printf("\t-f read signal from file [Default generate it with pdsch_encode()]\n");
+  printf("\t-m MCS [Default %d]\n", mcs_idx);
+  printf("\t-M mbsfn area id [Default %d]\n", mbsfn_area_id);
+  printf("\t-N non mbsfn region [Default %d]\n", non_mbsfn_region);
+  printf("\t-c cell id [Default %d]\n", cell.id);
+  printf("\t-s subframe [Default %d]\n", subframe);
+  printf("\t-r rv_idx [Default %d]\n", rv_idx[0]);
+  printf("\t-R rnti [Default %d]\n", rnti);
+  printf("\t-F cfi [Default %d]\n", cfi);
+  printf("\t-n cell.nof_prb [Default %d]\n", cell.nof_prb);
+  printf("\t-a nof_rx_antennas [Default %d]\n", nof_rx_antennas);
+  printf("\t-v [set srslte_verbose to debug, default none]\n");
+}
+
+void parse_args(int argc, char **argv) {
+  int opt;
+  while ((opt = getopt(argc, argv, "fmMcsrtRFpnavx")) != -1) {
+    switch(opt) {
+    case 'f':
+      input_file = argv[optind];
+      break;
+    case 'm':
+      mcs_idx = (uint32_t) atoi(argv[optind]);
+      break;
+    case 's':
+      subframe = atoi(argv[optind]);
+      break;
+    case 'r':
+      rv_idx[0] = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'R':
+      rnti = atoi(argv[optind]);
+      break;
+    case 'F':
+      cfi = atoi(argv[optind]);
+      break;
+    case 'x':
+      strncpy(mimo_type_str, argv[optind], 32);
+      break;
+    case 'p':
+      pmi = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'n':
+      cell.nof_prb = atoi(argv[optind]);
+      break;
+    case 'c':
+      cell.id = atoi(argv[optind]);
+      break;
+    case 'a':
+      nof_rx_antennas = (uint32_t) atoi(argv[optind]);
+      break;
+    case 'v':
+      srslte_verbose++;
+      break;
+    default:
+      usage(argv[0]);
+      exit(-1);
+    }
+  }
+}
+
+static uint8_t *data_tx[SRSLTE_MAX_CODEWORDS] = {NULL};
+static uint8_t *data_rx[SRSLTE_MAX_CODEWORDS] = {NULL};
+cf_t *ce[SRSLTE_MAX_PORTS][SRSLTE_MAX_PORTS];
+srslte_softbuffer_rx_t *softbuffers_rx[SRSLTE_MAX_CODEWORDS];
+srslte_ra_dl_grant_t grant; 
+#ifdef DO_OFDM
+cf_t *tx_sf_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_sf_symbols[SRSLTE_MAX_PORTS];
+#endif /* DO_OFDM */
+cf_t *tx_slot_symbols[SRSLTE_MAX_PORTS];
+cf_t *rx_slot_symbols[SRSLTE_MAX_PORTS];
+srslte_pmch_t pmch_tx, pmch_rx;
+srslte_pdsch_cfg_t  pmch_cfg;
+srslte_ofdm_t ifft_mbsfn, fft_mbsfn; 
+
+int main(int argc, char **argv) {
+  uint32_t i, j, k;
+  int ret = -1;
+  struct timeval t[3];
+  srslte_softbuffer_tx_t *softbuffers_tx[SRSLTE_MAX_CODEWORDS];
+  int M=1;
+  
+  parse_args(argc,argv);
+
+  /* Initialise to zeros */
+  bzero(&pmch_tx, sizeof(srslte_pdsch_t));
+  bzero(&pmch_rx, sizeof(srslte_pdsch_t));
+  bzero(&pmch_cfg, sizeof(srslte_pdsch_cfg_t));
+  bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+  bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
+
+
+  cell.nof_ports = 1;
+
+  srslte_ra_dl_dci_t dci;
+  bzero(&dci, sizeof(srslte_ra_dl_dci_t));
+  dci.type0_alloc.rbg_bitmask = 0xffffffff;
+  
+
+  /* If transport block 0 is enabled */
+    grant.tb_en[0] = true;
+    grant.tb_en[1] = false;
+    grant.nof_tb = 1;
+    grant.mcs[0].idx = mcs_idx;
+   
+    grant.nof_prb = cell.nof_prb;
+    grant.sf_type = SRSLTE_SF_MBSFN;
+    
+    srslte_dl_fill_ra_mcs(&grant.mcs[0], cell.nof_prb);
+    grant.Qm[0] = srslte_mod_bits_x_symbol(grant.mcs[0].mod);
+    
+    for(int i = 0; i < 2; i++){
+      for(int j = 0; j < grant.nof_prb; j++){
+        grant.prb_idx[i][j] = true;
+      }
+    }
+
+  
+
+#ifdef DO_OFDM
+
+ if (srslte_ofdm_tx_init_mbsfn(&ifft_mbsfn, SRSLTE_CP_EXT, cell.nof_prb)) {
+    fprintf(stderr, "Error creating iFFT object\n");
+    exit(-1);
+  }
+  if (srslte_ofdm_rx_init_mbsfn(&fft_mbsfn, SRSLTE_CP_EXT, cell.nof_prb)) {
+    fprintf(stderr, "Error creating iFFT object\n");
+    exit(-1);
+  }
+  
+  
+  srslte_ofdm_set_non_mbsfn_region(&ifft_mbsfn, non_mbsfn_region);
+  srslte_ofdm_set_non_mbsfn_region(&fft_mbsfn, non_mbsfn_region);
+  srslte_ofdm_set_normalize(&ifft_mbsfn, true);
+  srslte_ofdm_set_normalize(&fft_mbsfn, true);
+  
+
+  for (i = 0; i < cell.nof_ports; i++) {
+    tx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+
+  for (i = 0; i < nof_rx_antennas; i++) {
+    rx_sf_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_PRB(cell.nof_prb));
+  }
+#endif /* DO_OFDM */
+
+  /* Configure PDSCH */
+  
+  if (srslte_pmch_cfg(&pmch_cfg, cell, &grant, cfi, subframe)) {
+    fprintf(stderr, "Error configuring PMCH\n");
+    exit(-1);
+  }
+  
+
+  /* init memory */
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      ce[i][j] = srslte_vec_malloc(sizeof(cf_t) * NOF_CE_SYMBOLS);
+      if (!ce[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+        ce[i][j][k] = (i == j) ? 1.0f : 0.0f;
+      }
+    }
+    rx_slot_symbols[i] = srslte_vec_malloc(sizeof(cf_t) * SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp));
+    if (!rx_slot_symbols[i]) {
+      perror("srslte_vec_malloc");
+      goto quit;
+    }
+  }
+
+
+  for (int i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant.tb_en[i]) {
+      data_tx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_tx[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data_tx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
+      data_rx[i] = srslte_vec_malloc(sizeof(uint8_t) * grant.mcs[i].tbs);
+      if (!data_rx[i]) {
+        perror("srslte_vec_malloc");
+        goto quit;
+      }
+      bzero(data_rx[i], sizeof(uint8_t) * grant.mcs[i].tbs);
+
+    }
+  }
+
+
+  
+  
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    softbuffers_rx[i] = calloc(sizeof(srslte_softbuffer_rx_t), 1);
+    if (!softbuffers_rx[i]) {
+      fprintf(stderr, "Error allocating RX soft buffer\n");
+      goto quit;
+    }
+
+    if (srslte_softbuffer_rx_init(softbuffers_rx[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating RX soft buffer\n");
+      goto quit;
+    }
+  }
+  
+  if (srslte_pmch_cfg(&pmch_cfg, cell, &grant, cfi, subframe)) {
+      fprintf(stderr, "Error configuring PMCH\n");
+      exit(-1);
+    }
+    
+
+  INFO(" Global:\n");
+  INFO("         nof_prb=%d\n", cell.nof_prb);
+  INFO("       nof_ports=%d\n", cell.nof_ports);
+  INFO("              id=%d\n", cell.id);
+  INFO("              cp=%s\n", srslte_cp_string(cell.cp));
+  INFO("    phich_length=%d\n", (int) cell.phich_length);
+  INFO(" phich_resources=%d\n", (int) cell.phich_resources);
+  INFO("         nof_prb=%d\n", pmch_cfg.grant.nof_prb);
+  INFO("          sf_idx=%d\n", pmch_cfg.sf_idx);
+  INFO("       mimo_type=%s\n", srslte_mimotype2str(pmch_cfg.mimo_type));
+  INFO("      nof_layers=%d\n", pmch_cfg.nof_layers);
+  INFO("          nof_tb=%d\n", SRSLTE_RA_DL_GRANT_NOF_TB(&pmch_cfg.grant));  
+
+  INFO("              Qm=%d\n", pmch_cfg.grant.Qm[0]);
+  INFO("         mcs.idx=0x%X\n", pmch_cfg.grant.mcs[0].idx);
+  INFO("         mcs.tbs=%d\n", pmch_cfg.grant.mcs[0].tbs);
+  INFO("         mcs.mod=%s\n", srslte_mod_string(pmch_cfg.grant.mcs[0].mod));
+  INFO("              rv=%d\n", pmch_cfg.rv[0]);
+  INFO("          lstart=%d\n", pmch_cfg.nbits[0].lstart);
+  INFO("        nof_bits=%d\n", pmch_cfg.nbits[0].nof_bits);
+  INFO("          nof_re=%d\n", pmch_cfg.nbits[0].nof_re);
+  INFO("        nof_symb=%d\n", pmch_cfg.nbits[0].nof_symb);
+
+
+
+  if (srslte_pmch_init(&pmch_tx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PMCH object\n");
+  }
+  srslte_pmch_set_area_id(&pmch_tx, mbsfn_area_id);
+
+  if (srslte_pmch_init(&pmch_rx, cell.nof_prb)) {
+    fprintf(stderr, "Error creating PMCH object\n");
+  }
+  srslte_pmch_set_area_id(&pmch_rx, mbsfn_area_id);
+
+
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    softbuffers_tx[i] = calloc(sizeof(srslte_softbuffer_tx_t), 1);
+    if (!softbuffers_tx[i]) {
+      fprintf(stderr, "Error allocating TX soft buffer\n");
+    }
+
+    if (srslte_softbuffer_tx_init(softbuffers_tx[i], cell.nof_prb)) {
+      fprintf(stderr, "Error initiating TX soft buffer\n");
+      goto quit;
+    }
+  }
+
+  for (i = 0; i < cell.nof_ports; i++) {
+    tx_slot_symbols[i] = calloc(SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp), sizeof(cf_t));
+    if (!tx_slot_symbols[i]) {
+      perror("srslte_vec_malloc");
+      goto quit;
+    }
+  }
+
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        data_tx[tb][byte] = (uint8_t) (rand() % 256);
+      }
+    }
+  }
+
+  if (srslte_pmch_encode(&pmch_tx, &pmch_cfg, softbuffers_tx[0], data_tx[0], mbsfn_area_id, tx_slot_symbols)) {
+    fprintf(stderr, "Error encoding PDSCH\n");
+    exit(-1);
+  }
+  gettimeofday(&t[2], NULL);
+  get_time_interval(t);
+  printf("ENCODED in %.2f (PHY bitrate=%.2f Mbps. Processing bitrate=%.2f Mbps)\n",
+         (float) t[0].tv_usec/M, (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)/1000.0f,
+         (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
+
+#ifdef DO_OFDM
+  for (i = 0; i < cell.nof_ports; i++) {
+    /* For each Tx antenna modulate OFDM */
+    srslte_ofdm_tx_sf(&ifft_mbsfn, tx_slot_symbols[i], tx_sf_symbols[i]);
+  }
+
+
+  /* combine outputs */
+  for (j = 0; j < nof_rx_antennas; j++) {
+    for (k = 0; k < NOF_CE_SYMBOLS; k++) {
+      rx_sf_symbols[j][k] = 0.0f;
+      for (i = 0; i < cell.nof_ports; i++) {
+        rx_sf_symbols[j][k] += tx_sf_symbols[i][k] * ce[i][j][k];
+      }
+    }
+  }
+    
+  #else
+    /* combine outputs */
+    for (j = 0; j < nof_rx_antennas; j++) {
+      for (k = 0; k < SRSLTE_SF_LEN_RE(cell.nof_prb, cell.cp); k++) {
+        rx_slot_symbols[j][k] = 0.0f;
+        for (i = 0; i < cell.nof_ports; i++) {
+          rx_slot_symbols[j][k] += tx_slot_symbols[i][k] * ce[i][j][k];
+        }
+      }
+    }
+  #endif
+
+
+  
+  int r=0;
+  gettimeofday(&t[1], NULL);
+
+#ifdef DO_OFDM
+    /* For each Rx antenna demodulate OFDM */
+    for (i = 0; i < nof_rx_antennas; i++) {
+      srslte_ofdm_rx_sf(&fft_mbsfn, tx_sf_symbols[i], rx_slot_symbols[i]);
+    }
+#endif
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    if (grant.tb_en[i]) {
+      srslte_softbuffer_rx_reset_tbs(softbuffers_rx[i], (uint32_t) grant.mcs[i].tbs);
+    }
+  }
+
+  r = srslte_pmch_decode(&pmch_rx, &pmch_cfg, softbuffers_rx[0],rx_slot_symbols[0], ce[0],0,mbsfn_area_id, data_rx[0]);
+  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[0].tbs + grant.mcs[1].tbs)/1000.0f,
+         (float) (grant.mcs[0].tbs + grant.mcs[1].tbs)*M/t[0].tv_usec);
+
+  /* If there is an error in PDSCH decode */
+  if (r) {
+    ret = -1;
+    goto quit;
+  }
+
+  /* Check Tx and Rx bytes */
+  for (int tb = 0; tb < SRSLTE_MAX_CODEWORDS; tb++) {
+    if (grant.tb_en[tb]) {
+      for (int byte = 0; byte < grant.mcs[tb].tbs / 8; byte++) {
+        if (data_tx[tb][byte] != data_rx[tb][byte]) {
+          ERROR("Found BYTE error in TB %d (%02X != %02X), quiting...", tb, data_tx[tb][byte], data_rx[tb][byte]);
+          ret = SRSLTE_ERROR;
+          goto quit;
+        }
+      }
+    }
+  }
+
+  ret = SRSLTE_SUCCESS;
+
+quit:
+  srslte_pmch_free(&pmch_tx);
+  srslte_pmch_free(&pmch_rx);
+  for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
+    srslte_softbuffer_tx_free(softbuffers_tx[i]);
+    if (softbuffers_tx[i]) {
+      free(softbuffers_tx[i]);
+    }
+
+    srslte_softbuffer_rx_free(softbuffers_rx[i]);
+    if (softbuffers_rx[i]) {
+      free(softbuffers_rx[i]);
+    }
+
+    if (data_tx[i]) {
+      free(data_tx[i]);
+    }
+
+    if (data_rx[i]) {
+      free(data_rx[i]);
+    }
+  }
+
+  for (i=0;i<SRSLTE_MAX_PORTS;i++) {
+    for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
+      if (ce[i][j]) {
+        free(ce[i][j]);
+      }
+    }
+    if (tx_slot_symbols[i]) {
+      free(tx_slot_symbols[i]);
+    }
+    if (rx_slot_symbols[i]) {
+      free(rx_slot_symbols[i]);
+    }
+  }
+  if (ret) {
+    printf("Error\n");
+  } else {
+    printf("Ok\n");
+  }
+  exit(ret);
+}

From 7c081c695a299244443cdd505c7c6cbd33b965ab Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 10:08:01 +0200
Subject: [PATCH 246/269] Missing call to c_str in printf

---
 srsenb/src/upper/gtpu.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/src/upper/gtpu.cc b/srsenb/src/upper/gtpu.cc
index e01be1ad0..d8f53d662 100644
--- a/srsenb/src/upper/gtpu.cc
+++ b/srsenb/src/upper/gtpu.cc
@@ -87,7 +87,7 @@ bool gtpu::init(std::string gtp_bind_addr_, std::string mme_addr_, srsenb::pdcp_
   bindaddr.sin_port        = htons(GTPU_PORT);
 
   if (bind(src_fd, (struct sockaddr *)&bindaddr, sizeof(struct sockaddr_in))) {
-    gtpu_log->error("Failed to bind on address %s, port %d\n", gtp_bind_addr, GTPU_PORT);
+    gtpu_log->error("Failed to bind on address %s, port %d\n", gtp_bind_addr.c_str(), GTPU_PORT);
     return false;
   }
 

From a043b8a1678944b1306b8a60dcb9945069eac67d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 10:15:10 +0200
Subject: [PATCH 247/269] buffer overflow in pmch_test bzero

---
 lib/src/phy/phch/test/pmch_test.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/phy/phch/test/pmch_test.c b/lib/src/phy/phch/test/pmch_test.c
index de7948852..88e9fd296 100644
--- a/lib/src/phy/phch/test/pmch_test.c
+++ b/lib/src/phy/phch/test/pmch_test.c
@@ -151,8 +151,8 @@ int main(int argc, char **argv) {
   parse_args(argc,argv);
 
   /* Initialise to zeros */
-  bzero(&pmch_tx, sizeof(srslte_pdsch_t));
-  bzero(&pmch_rx, sizeof(srslte_pdsch_t));
+  bzero(&pmch_tx, sizeof(srslte_pmch_t));
+  bzero(&pmch_rx, sizeof(srslte_pmch_t));
   bzero(&pmch_cfg, sizeof(srslte_pdsch_cfg_t));
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);

From 983bd0060c863972f68ce1486de09d446d8e108c Mon Sep 17 00:00:00 2001
From: yagoda <tallonj@tcd.ie>
Date: Wed, 4 Oct 2017 09:53:35 +0100
Subject: [PATCH 248/269] fixing incorrect type in bzero in pmch test

---
 CMakeLists.txt                    |  2 +-
 lib/src/phy/phch/test/pmch_test.c | 10 ++--------
 2 files changed, 3 insertions(+), 9 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a63a77016..d72bb5fef 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,7 +50,7 @@ configure_file(
     IMMEDIATE @ONLY)
 
 if(NOT CMAKE_BUILD_TYPE)
-   set(CMAKE_BUILD_TYPE Debug)
+   set(CMAKE_BUILD_TYPE Release)
    message(STATUS "Build type not specified: defaulting to Release.")
 endif(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "")
diff --git a/lib/src/phy/phch/test/pmch_test.c b/lib/src/phy/phch/test/pmch_test.c
index de7948852..aea1b50c0 100644
--- a/lib/src/phy/phch/test/pmch_test.c
+++ b/lib/src/phy/phch/test/pmch_test.c
@@ -149,16 +149,14 @@ int main(int argc, char **argv) {
   int M=1;
   
   parse_args(argc,argv);
-
   /* Initialise to zeros */
-  bzero(&pmch_tx, sizeof(srslte_pdsch_t));
-  bzero(&pmch_rx, sizeof(srslte_pdsch_t));
+  bzero(&pmch_tx, sizeof(srslte_pmch_t));
+  bzero(&pmch_rx, sizeof(srslte_pmch_t));
   bzero(&pmch_cfg, sizeof(srslte_pdsch_cfg_t));
   bzero(ce, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(tx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
   bzero(rx_slot_symbols, sizeof(cf_t*)*SRSLTE_MAX_PORTS);
 
-
   cell.nof_ports = 1;
 
   srslte_ra_dl_dci_t dci;
@@ -177,7 +175,6 @@ int main(int argc, char **argv) {
     
     srslte_dl_fill_ra_mcs(&grant.mcs[0], cell.nof_prb);
     grant.Qm[0] = srslte_mod_bits_x_symbol(grant.mcs[0].mod);
-    
     for(int i = 0; i < 2; i++){
       for(int j = 0; j < grant.nof_prb; j++){
         grant.prb_idx[i][j] = true;
@@ -197,7 +194,6 @@ int main(int argc, char **argv) {
     exit(-1);
   }
   
-  
   srslte_ofdm_set_non_mbsfn_region(&ifft_mbsfn, non_mbsfn_region);
   srslte_ofdm_set_non_mbsfn_region(&fft_mbsfn, non_mbsfn_region);
   srslte_ofdm_set_normalize(&ifft_mbsfn, true);
@@ -220,7 +216,6 @@ int main(int argc, char **argv) {
     exit(-1);
   }
   
-
   /* init memory */
   for (i=0;i<SRSLTE_MAX_PORTS;i++) {
     for (j = 0; j < SRSLTE_MAX_PORTS; j++) {
@@ -262,7 +257,6 @@ int main(int argc, char **argv) {
 
 
   
-  
   for (i = 0; i < SRSLTE_MAX_CODEWORDS; i++) {
     softbuffers_rx[i] = calloc(sizeof(srslte_softbuffer_rx_t), 1);
     if (!softbuffers_rx[i]) {

From cc6546db18c67eae4f2c26171c18de9e66c9d4e0 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 11:50:15 +0200
Subject: [PATCH 249/269] Fixed incorrect scheduling with PUCCH

---
 srsenb/hdr/mac/mac.h               |  5 +----
 srsenb/hdr/mac/scheduler.h         |  2 +-
 srsenb/hdr/mac/scheduler_metric.h  |  2 +-
 srsenb/hdr/mac/scheduler_ue.h      |  3 +--
 srsenb/src/mac/mac.cc              | 24 +++---------------------
 srsenb/src/mac/scheduler.cc        | 22 +++++++++-------------
 srsenb/src/mac/scheduler_metric.cc |  8 +++-----
 srsenb/src/mac/scheduler_ue.cc     | 25 +++++++++----------------
 srsenb/src/upper/rrc.cc            | 10 ++++++----
 9 files changed, 34 insertions(+), 67 deletions(-)

diff --git a/srsenb/hdr/mac/mac.h b/srsenb/hdr/mac/mac.h
index 4620c9629..d71245e89 100644
--- a/srsenb/hdr/mac/mac.h
+++ b/srsenb/hdr/mac/mac.h
@@ -113,10 +113,7 @@ public:
 
 private:  
 
-  void log_step_ul(uint32_t tti);
-  void log_step_dl(uint32_t tti);
-  
-  static const int MAX_LOCATIONS = 20; 
+  static const int MAX_LOCATIONS = 20;
   static const uint32_t cfi = 3; 
   srslte_dci_location_t locations[MAX_LOCATIONS];
   
diff --git a/srsenb/hdr/mac/scheduler.h b/srsenb/hdr/mac/scheduler.h
index c70ee4247..231239285 100644
--- a/srsenb/hdr/mac/scheduler.h
+++ b/srsenb/hdr/mac/scheduler.h
@@ -68,7 +68,7 @@ public:
     /* Virtual methods for user metric calculation */
     virtual void           new_tti(std::map<uint16_t,sched_ue> &ue_db, uint32_t nof_rb, uint32_t tti) = 0;
     virtual ul_harq_proc*  get_user_allocation(sched_ue *user) = 0; 
-    virtual void           update_allocation(ul_harq_proc::ul_alloc_t alloc) = 0; 
+    virtual void           update_allocation(ul_harq_proc::ul_alloc_t alloc) = 0;
   };
 
   
diff --git a/srsenb/hdr/mac/scheduler_metric.h b/srsenb/hdr/mac/scheduler_metric.h
index b9d515ade..eda0b31ed 100644
--- a/srsenb/hdr/mac/scheduler_metric.h
+++ b/srsenb/hdr/mac/scheduler_metric.h
@@ -65,7 +65,7 @@ class ul_metric_rr : public sched::metric_ul
 public:
   void           new_tti(std::map<uint16_t,sched_ue> &ue_db, uint32_t nof_rb, uint32_t tti);
   ul_harq_proc*  get_user_allocation(sched_ue *user); 
-  void           update_allocation(ul_harq_proc::ul_alloc_t alloc); 
+  void           update_allocation(ul_harq_proc::ul_alloc_t alloc);
 private:
   
   const static int MAX_PRB = 100; 
diff --git a/srsenb/hdr/mac/scheduler_ue.h b/srsenb/hdr/mac/scheduler_ue.h
index b59461140..3653ff65c 100644
--- a/srsenb/hdr/mac/scheduler_ue.h
+++ b/srsenb/hdr/mac/scheduler_ue.h
@@ -42,7 +42,6 @@ public:
   // used by sched_metric
   uint32_t ue_idx;
 
-  bool has_pusch;
   bool has_pucch;
   
   typedef struct {
@@ -116,7 +115,7 @@ public:
   bool       needs_cqi(uint32_t tti, bool will_send = false); 
   uint32_t   get_max_retx(); 
   
-  bool       get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32_t *L);
+  bool       get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2]);
   bool       pucch_sr_collision(uint32_t current_tti, uint32_t n_cce);
 
   uint32_t   get_pending_ul_old_data();
diff --git a/srsenb/src/mac/mac.cc b/srsenb/src/mac/mac.cc
index 7cb30cc65..6c8458540 100644
--- a/srsenb/src/mac/mac.cc
+++ b/srsenb/src/mac/mac.cc
@@ -403,7 +403,7 @@ int mac::rach_detected(uint32_t tti, uint32_t preamble_idx, uint32_t time_adv)
 
 int mac::get_dl_sched(uint32_t tti, dl_sched_t *dl_sched_res)
 {
-  log_step_dl(tti);
+  log_h->step(tti);
 
   if (!started) {
     return 0; 
@@ -544,8 +544,8 @@ uint8_t* mac::assemble_si(uint32_t index)
 
 int mac::get_ul_sched(uint32_t tti, ul_sched_t *ul_sched_res) 
 {
-  
-  log_step_ul(tti);
+
+  log_h->step(tti);
   
   if (!started) {
     return 0; 
@@ -602,24 +602,6 @@ int mac::get_ul_sched(uint32_t tti, ul_sched_t *ul_sched_res)
   return SRSLTE_SUCCESS; 
 }
 
-void mac::log_step_ul(uint32_t tti) 
-{
-  int tti_ul = tti-8;
-  if (tti_ul < 0) {
-    tti_ul += 10240;
-  }
-  log_h->step(tti_ul);
-}
-
-void mac::log_step_dl(uint32_t tti) 
-{
-  int tti_dl = tti-4;
-  if (tti_dl < 0) {
-    tti_dl += 10240;
-  }
-  log_h->step(tti_dl);
-}
-
 void mac::tti_clock()
 {
   timers_thread.tti_clock();
diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 79cf3f476..539b7725f 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -700,7 +700,6 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
     sched_ue *user = (sched_ue*) &iter->second;
     uint16_t rnti  = (uint16_t) iter->first; 
 
-    user->has_pusch = false;
     user->has_pucch = false;
 
     ul_harq_proc *h = user->get_ul_harq(current_tti);
@@ -726,15 +725,12 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
     sched_ue *user = (sched_ue*) &iter->second;
     uint16_t rnti  = (uint16_t) iter->first; 
     uint32_t prb_idx[2] = {0, 0}; 
-    uint32_t L = 0; 
-    if (user->get_pucch_sched(current_tti, prb_idx, &L)) {
+    if (user->get_pucch_sched(current_tti, prb_idx)) {
       user->has_pucch = true;
-      // allocate PUCCH if no PUSCH for user
-      if (!user->has_pusch) {
-        for (int i=0;i<2;i++) {
-          ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], L};
-          ul_metric->update_allocation(pucch);
-        }
+      // allocate PUCCH
+      for (int i=0;i<2;i++) {
+        ul_harq_proc::ul_alloc_t pucch = {prb_idx[i], 1};
+        ul_metric->update_allocation(pucch);
       }
     }
   }
@@ -807,22 +803,22 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
             user->unset_sr();
           }
 
-          log_h->info("SCHED: %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=%d,%d, n_rtx=%d, tbs=%d, bsr=%d (%d-%d)\n",
+          log_h->info("SCHED: %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=(%d,%d), n_rtx=%d, tbs=%d, bsr=%d (%d-%d)\n",
                       is_rar?"RAR":"UL",
                       is_newtx?"tx":"retx",                
                       rnti, h->get_id(), 
                       sched_result->pusch[nof_dci_elems].dci_location.L, sched_result->pusch[nof_dci_elems].dci_location.ncce,
-                      alloc.RB_start, alloc.L, h->nof_retx(), sched_result->pusch[nof_dci_elems].tbs, 
+                      alloc.RB_start, alloc.RB_start+alloc.L, h->nof_retx(), sched_result->pusch[nof_dci_elems].tbs,
                       user->get_pending_ul_new_data(current_tti),pending_data_before, user->get_pending_ul_old_data());
 
           nof_dci_elems++;          
         } else {
-          log_h->warning("SCHED: Error %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=%d,%d, tbs=%d, bsr=%d\n", 
+          log_h->warning("SCHED: Error %s %s rnti=0x%x, pid=%d, dci=%d,%d, grant=(%d,%d), tbs=%d, bsr=%d\n",
                       is_rar?"RAR":"UL",
                       is_newtx?"tx":"retx",                
                       rnti, h->get_id(), 
                       sched_result->pusch[nof_dci_elems].dci_location.L, sched_result->pusch[nof_dci_elems].dci_location.ncce,
-                      alloc.RB_start, alloc.L, sched_result->pusch[nof_dci_elems].tbs, 
+                      alloc.RB_start, alloc.RB_start+alloc.L, sched_result->pusch[nof_dci_elems].tbs,
                       user->get_pending_ul_new_data(current_tti));
         }   
       }
diff --git a/srsenb/src/mac/scheduler_metric.cc b/srsenb/src/mac/scheduler_metric.cc
index 309eed45a..708ab2dd8 100644
--- a/srsenb/src/mac/scheduler_metric.cc
+++ b/srsenb/src/mac/scheduler_metric.cc
@@ -25,8 +25,7 @@
  */
 
 #include <string.h>
-
-#include "srslte/srslte.h"
+#include "mac/scheduler_harq.h"
 #include "mac/scheduler_metric.h"
 
 #define Error(fmt, ...)   log_h->error_line(__FILE__, __LINE__, fmt, ##__VA_ARGS__)
@@ -215,8 +214,7 @@ void ul_metric_rr::new_tti(std::map<uint16_t,sched_ue> &ue_db, uint32_t nof_rb_,
     sched_ue *user      = (sched_ue*) &iter->second;
     if (user->get_pending_ul_new_data(current_tti) || !user->get_ul_harq(current_tti)->is_empty()) {
       user->ue_idx    = nof_users_with_data;
-      user->has_pusch = true;
-      nof_users_with_data++; 
+      nof_users_with_data++;
     }
   }
 
@@ -275,7 +273,7 @@ void ul_metric_rr::update_allocation(ul_harq_proc::ul_alloc_t alloc)
     return; 
   }
   for (uint32_t n=alloc.RB_start;n<alloc.RB_start+alloc.L;n++) {
-    used_rb[n] = true; 
+    used_rb[n] = true;
   }
   available_rb -= alloc.L; 
 }
diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 1534d12ef..f0627bb4b 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -28,6 +28,7 @@
 #include <boost/concept_check.hpp>
 #include <srslte/interfaces/sched_interface.h>
 #include <srslte/phy/phch/pucch.h>
+#include <srslte/srslte.h>
 
 #include "srslte/srslte.h"
 #include "srslte/common/pdu.h"
@@ -232,7 +233,7 @@ bool sched_ue::pucch_sr_collision(uint32_t current_tti, uint32_t n_cce)
   }
 }
 
-bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32_t *L)
+bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2])
 {
   if (!phy_config_dedicated_enabled) {
     return false; 
@@ -241,7 +242,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
   pucch_sched.sps_enabled = false;
   pucch_sched.n_pucch_sr = cfg.sr_N_pucch;
   pucch_sched.n_pucch_2  = cfg.n_pucch_cqi;
-  pucch_sched.N_pucch_1  = cfg.pucch_cfg.n1_pucch_an; 
+  pucch_sched.N_pucch_1  = cfg.pucch_cfg.n1_pucch_an;
   
   bool has_sr = cfg.sr_enabled && srslte_ue_ul_sr_send_tti(cfg.sr_I, current_tti);    
   
@@ -251,13 +252,11 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
       uint32_t n_pucch = srslte_pucch_get_npucch(dl_harq[i].get_n_cce(), SRSLTE_PUCCH_FORMAT_1A, has_sr, &pucch_sched);
       if (prb_idx) {
         for (int i=0;i<2;i++) {
-          prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_1A, n_pucch, cell.nof_prb, cell.cp, i); 
-        }      
-      }
-      if (L) {
-        *L = 1; 
+          prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_1A, n_pucch, cell.nof_prb, cell.cp, i);
+        }
       }
-      Debug("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], n_pucch);
+      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, ncce=%d, has_sr=%d, n_pucch_1=%d\n",
+           rnti, prb_idx[0], prb_idx[1], n_pucch, dl_harq[i].get_n_cce(), has_sr, pucch_sched.N_pucch_1);
       return true; 
     }
   }
@@ -268,10 +267,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
         prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_1, cfg.sr_N_pucch, cell.nof_prb, cell.cp, i); 
       }
     }
-    if (L) {
-      *L = 1; 
-    }
-    Debug("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
+    Info("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
     return true; 
   }
   // Finally check Format2 (periodic CQI)
@@ -281,10 +277,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2], uint32
         prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_2, cfg.cqi_pucch, cell.nof_prb, cell.cp, i);
       }
     }
-    if(L) {
-      *L = 2;
-    }
-    Debug("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
+    Info("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
          rnti, prb_idx[0], prb_idx[1], cfg.cqi_pucch, cfg.cqi_idx);
     return true;
   }
diff --git a/srsenb/src/upper/rrc.cc b/srsenb/src/upper/rrc.cc
index b1280cf55..4a432b394 100644
--- a/srsenb/src/upper/rrc.cc
+++ b/srsenb/src/upper/rrc.cc
@@ -24,9 +24,10 @@
  *
  */
 
-#include <srslte/interfaces/sched_interface.h>
-#include <srslte/asn1/liblte_rrc.h>
-#include <upper/rrc.h>
+#include "srslte/interfaces/sched_interface.h"
+#include "srslte/asn1/liblte_rrc.h"
+#include "upper/rrc.h"
+#include "srslte/srslte.h"
 #include "srslte/asn1/liblte_mme.h"
 #include "upper/rrc.h"
 
@@ -1177,7 +1178,8 @@ void rrc::ue::send_connection_setup(bool is_setup)
   sched_cfg.pucch_cfg.delta_pucch_shift  = liblte_rrc_delta_pucch_shift_num[parent->sib2.rr_config_common_sib.pucch_cnfg.delta_pucch_shift%LIBLTE_RRC_DELTA_PUCCH_SHIFT_N_ITEMS];
   sched_cfg.pucch_cfg.N_cs               = parent->sib2.rr_config_common_sib.pucch_cnfg.n_cs_an;
   sched_cfg.pucch_cfg.n_rb_2             = parent->sib2.rr_config_common_sib.pucch_cnfg.n_rb_cqi;
-  
+  sched_cfg.pucch_cfg.n1_pucch_an        = parent->sib2.rr_config_common_sib.pucch_cnfg.n1_pucch_an;
+
   // Configure MAC 
   parent->mac->ue_cfg(rnti, &sched_cfg);
     

From b155ba4c2865e1f8d0aa4dceef41e352e7c0e2fc Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 12:00:31 +0200
Subject: [PATCH 250/269] Fixed segfault in pdsch_ue --help command

---
 lib/examples/pdsch_ue.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/examples/pdsch_ue.c b/lib/examples/pdsch_ue.c
index 38113dbcf..97fd71f50 100644
--- a/lib/examples/pdsch_ue.c
+++ b/lib/examples/pdsch_ue.c
@@ -171,8 +171,8 @@ void usage(prog_args_t *args, char *prog) {
   printf("\t-S remote UDP address to send input signal [Default %s]\n", args->net_address_signal);
   printf("\t-u remote TCP port to send data (-1 does nothing with it) [Default %d]\n", args->net_port);
   printf("\t-U remote TCP address to send data [Default %s]\n", args->net_address);
-  printf("\t-M MBSFN area id [Default %s]\n", args->mbsfn_area_id);
-  printf("\t-N Non-MBSFN region [Default %s]\n", args->non_mbsfn_region);
+  printf("\t-M MBSFN area id [Default %d]\n", args->mbsfn_area_id);
+  printf("\t-N Non-MBSFN region [Default %d]\n", args->non_mbsfn_region);
   printf("\t-v [set srslte_verbose to debug, default none]\n");
 }
 

From 189dcfa1d23f0bac69b3ff301ecc6be63d7e2516 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 13:35:18 +0200
Subject: [PATCH 251/269] Updated TX/RX calibrated values for x300

---
 lib/src/radio/radio.cc | 22 +++-------------------
 1 file changed, 3 insertions(+), 19 deletions(-)

diff --git a/lib/src/radio/radio.cc b/lib/src/radio/radio.cc
index c0e828f71..b393a3ff4 100644
--- a/lib/src/radio/radio.cc
+++ b/lib/src/radio/radio.cc
@@ -357,25 +357,9 @@ void radio::set_tx_srate(double srate)
         nsamples = cur_tx_srate*(uhd_default_tx_adv_samples * (1/cur_tx_srate) + uhd_default_tx_adv_offset_sec);        
       }                
     } else if (!strcmp(srslte_rf_name(&rf_device), "uhd_x300")) {
-      
-      double srate_khz = round(cur_tx_srate/1e3);
-      if (srate_khz == 1.92e3) {
-        nsamples = 50;
-      } else if (srate_khz == 3.84e3) {
-        nsamples = 65;
-      } else if (srate_khz == 5.76e3) {
-        nsamples = 75;
-      } else if (srate_khz == 11.52e3) {
-        nsamples = 89;
-      } else if (srate_khz == 15.36e3) {
-        nsamples = 86;
-      } else if (srate_khz == 23.04e3) {
-        nsamples = 110;
-      } else {
-        /* Interpolate from known values */
-        printf("\nWarning TX/RX time offset for sampling rate %.0f KHz not calibrated. Using interpolated value\n\n", cur_tx_srate);
-        nsamples = cur_tx_srate*(uhd_default_tx_adv_samples * (1/cur_tx_srate) + uhd_default_tx_adv_offset_sec);        
-      }                
+
+      // In X300 TX/RX offset is independent of sampling rate
+      nsamples = 45;
     } else if (!strcmp(srslte_rf_name(&rf_device), "bladerf")) {
       
       double srate_khz = round(cur_tx_srate/1e3);

From 582c87e86d1c487a4967b07d7484dcfabcf83425 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Wed, 4 Oct 2017 17:45:17 +0200
Subject: [PATCH 252/269] Removed PUCCH reserved logs

---
 srsenb/src/mac/scheduler_ue.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index f0627bb4b..3eab6b33d 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -255,7 +255,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2])
           prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_1A, n_pucch, cell.nof_prb, cell.cp, i);
         }
       }
-      Info("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, ncce=%d, has_sr=%d, n_pucch_1=%d\n",
+      Debug("SCHED: Reserved Format1A PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, ncce=%d, has_sr=%d, n_pucch_1=%d\n",
            rnti, prb_idx[0], prb_idx[1], n_pucch, dl_harq[i].get_n_cce(), has_sr, pucch_sched.N_pucch_1);
       return true; 
     }
@@ -267,7 +267,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2])
         prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_1, cfg.sr_N_pucch, cell.nof_prb, cell.cp, i); 
       }
     }
-    Info("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
+    Debug("SCHED: Reserved Format1 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d\n", rnti, prb_idx[0], prb_idx[1], cfg.sr_N_pucch);
     return true; 
   }
   // Finally check Format2 (periodic CQI)
@@ -277,7 +277,7 @@ bool sched_ue::get_pucch_sched(uint32_t current_tti, uint32_t prb_idx[2])
         prb_idx[i] = srslte_pucch_n_prb(&cfg.pucch_cfg, SRSLTE_PUCCH_FORMAT_2, cfg.cqi_pucch, cell.nof_prb, cell.cp, i);
       }
     }
-    Info("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
+    Debug("SCHED: Reserved Format2 PUCCH for rnti=0x%x, n_prb=%d,%d, n_pucch=%d, pmi_idx=%d\n",
          rnti, prb_idx[0], prb_idx[1], cfg.cqi_pucch, cfg.cqi_idx);
     return true;
   }

From b353ed03ddc9ae2df9fa217f8192a24da6866562 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Thu, 5 Oct 2017 12:26:48 +0200
Subject: [PATCH 253/269] cleaned stdout metric

---
 srsenb/src/metrics_stdout.cc | 8 ++++----
 srsue/src/metrics_stdout.cc  | 4 ++--
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/srsenb/src/metrics_stdout.cc b/srsenb/src/metrics_stdout.cc
index 6c294b96d..ec55b0dcf 100644
--- a/srsenb/src/metrics_stdout.cc
+++ b/srsenb/src/metrics_stdout.cc
@@ -130,9 +130,9 @@ void metrics_stdout::print_metrics()
         cout << float_to_string(0, 2);                
       }
       if (metrics.mac[i].tx_pkts > 0 && metrics.mac[i].tx_errors) {
-        cout << float_to_string((float) 100*metrics.mac[i].tx_errors/metrics.mac[i].tx_pkts, 2) << "%";
+        cout << float_to_string((float) 100*metrics.mac[i].tx_errors/metrics.mac[i].tx_pkts, 1) << "%";
       } else {
-        cout << float_to_string(0, 2) << "%";
+        cout << float_to_string(0, 1) << "%";
       }
       cout << float_to_string(metrics.phy[i].ul.sinr, 2);
       cout << float_to_string(metrics.mac[i].phr, 2);
@@ -143,9 +143,9 @@ void metrics_stdout::print_metrics()
         cout << float_to_string(0, 2);        
       }
       if (metrics.mac[i].rx_pkts > 0 && metrics.mac[i].rx_errors > 0) {
-        cout << float_to_string((float) 100*metrics.mac[i].rx_errors/metrics.mac[i].rx_pkts, 2) << "%";
+        cout << float_to_string((float) 100*metrics.mac[i].rx_errors/metrics.mac[i].rx_pkts, 1) << "%";
       } else {
-        cout << float_to_string(0, 2) << "%";
+        cout << float_to_string(0, 1) << "%";
       }
       cout << float_to_eng_string(metrics.mac[i].ul_buffer, 2);
       cout << endl;
diff --git a/srsue/src/metrics_stdout.cc b/srsue/src/metrics_stdout.cc
index 6828c912f..048532c8f 100644
--- a/srsue/src/metrics_stdout.cc
+++ b/srsue/src/metrics_stdout.cc
@@ -92,7 +92,7 @@ void metrics_stdout::set_metrics(ue_metrics_t &metrics, float metrics_report_per
   if (metrics.mac.rx_pkts > 0) {
     cout << float_to_string((float) 100*metrics.mac.rx_errors/metrics.mac.rx_pkts, 1) << "%";
   } else {
-    cout << float_to_string(0, 2) << "%";
+    cout << float_to_string(0, 1) << "%";
   }
   cout << float_to_string(metrics.phy.ul.mcs, 2);
   cout << float_to_eng_string((float) metrics.mac.ul_buffer, 2);
@@ -100,7 +100,7 @@ void metrics_stdout::set_metrics(ue_metrics_t &metrics, float metrics_report_per
   if (metrics.mac.tx_pkts > 0) {
     cout << float_to_string((float) 100*metrics.mac.tx_errors/metrics.mac.tx_pkts, 1) << "%";
   } else {
-    cout << float_to_string(0, 2) << "%";
+    cout << float_to_string(0, 1) << "%";
   }
   cout << endl;
 

From 81b143715b0b0bb5e7f3f22b31f0f0c5bd43e411 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Wed, 4 Oct 2017 16:17:44 +0200
Subject: [PATCH 254/269] fix formating

---
 lib/src/phy/rf/rf_soapy_imp.c | 95 +++++++++++++++++------------------
 1 file changed, 45 insertions(+), 50 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 31649af69..084308116 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -88,12 +88,12 @@ void rf_soapy_register_error_handler(void *notused, srslte_rf_error_handler_t ne
 }
 
 
-
 char* rf_soapy_devname(void* h)
 {
   return "soapy";
 }
 
+
 bool rf_soapy_rx_wait_lo_locked(void *h)
 {
   printf("TODO: implement rf_soapy_rx_wait_lo_locked()\n");
@@ -155,7 +155,6 @@ int rf_soapy_stop_tx_stream(void *h)
   if(SoapySDRDevice_deactivateStream(handler->device, handler->txStream, 0, 0) != 0)
     return SRSLTE_ERROR;
 
-
   handler->tx_stream_active = false;
   return SRSLTE_SUCCESS;
 }
@@ -199,9 +198,8 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
   }
   
   for (size_t i = 0; i < length; i++) {
-    printf("Soapy Has Found device #%d: ", (int)i);
-    for (size_t j = 0; j < soapy_args[i].size; j++)
-    {
+    printf("Soapy has Found device #%d: ", (int)i);
+    for (size_t j = 0; j < soapy_args[i].size; j++) {
       printf("%s=%s, ", soapy_args[i].keys[j], soapy_args[i].vals[j]);
     }
     printf("\n");
@@ -221,7 +219,6 @@ int rf_soapy_open_multi(char *args, void **h, uint32_t nof_rx_antennas)
   handler->tx_stream_active = false;
   handler->rx_stream_active = false;
 
-
   if(SoapySDRDevice_getNumChannels(handler->device,SOAPY_SDR_RX) > 0){     
     printf("setting up RX stream\n");
     if(SoapySDRDevice_setupStream(handler->device, &(handler->rxStream), SOAPY_SDR_RX, SOAPY_SDR_CF32, NULL, 0, NULL) != 0) {
@@ -364,7 +361,8 @@ double rf_soapy_set_tx_freq(void *h, double freq)
 }
 
 
-void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs) {
+void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs)
+{
 
 }
 
@@ -430,49 +428,46 @@ int rf_soapy_recv_with_time(void *h,
 
 
 int rf_soapy_send_timed(void *h,
-                     void *data,
-                     int nsamples,
-                     time_t secs,
-                     double frac_secs,                      
-                     bool has_time_spec,
-                     bool blocking,
-                     bool is_start_of_burst,
-                     bool is_end_of_burst) 
-{
-    
-    int flags;
-    long long timeNs;
-    int trials = 0;
-    int ret = 0;
-    rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-    timeNs = secs * 1000000000;
-    timeNs = timeNs + (frac_secs * 1000000000);
-    int n = 0;
-
-    if(!handler->tx_stream_active){
-      rf_soapy_start_tx_stream(h);
+                        void *data,
+                        int nsamples,
+                        time_t secs,
+                        double frac_secs,
+                        bool has_time_spec,
+                        bool blocking,
+                        bool is_start_of_burst,
+                        bool is_end_of_burst)
+{
+  int flags;
+  long long timeNs;
+  int trials = 0;
+  int ret = 0;
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t *) h;
+  timeNs = secs * 1000000000;
+  timeNs = timeNs + (frac_secs * 1000000000);
+  int n = 0;
+
+  if (!handler->tx_stream_active) {
+    rf_soapy_start_tx_stream(h);
+  }
+  
+  cf_t *data_c = (cf_t *) data;
+  do {
+    size_t tx_samples = nsamples;
+    if (tx_samples > nsamples - n) {
+      tx_samples = nsamples - n;
     }
-    
-    
-    cf_t *data_c = (cf_t*) data;
-    do{
-      size_t tx_samples = nsamples;
-      if (tx_samples > nsamples - n) {
-          tx_samples = nsamples - n; 
-      }
-      void *buff = (void*) &data_c[n];
-      const void *buffs_ptr[1] = {buff};
-      ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs,  10000);
-      if(ret < 0)
-         return SRSLTE_ERROR;
-       
-       n += ret;
-       trials++;
-     }while (n < nsamples && trials < 100);
-
-    if(ret != nsamples)
-        return SRSLTE_ERROR;
-    
-    return ret;
+    void *buff = (void *) &data_c[n];
+    const void *buffs_ptr[1] = {buff};
+    ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs, 10000);
+    if (ret < 0)
+      return SRSLTE_ERROR;
+
+    n += ret;
+    trials++;
+  } while (n < nsamples && trials < 100);
+
+  if (ret != nsamples)
+    return SRSLTE_ERROR;
 
+  return ret;
 }

From df2bbd40871be545f151cf3e1a382f691d56c3bf Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Thu, 5 Oct 2017 16:26:23 +0200
Subject: [PATCH 255/269] fix multi channel tx support in soapy

---
 lib/src/phy/rf/rf_dev.h       |   2 +-
 lib/src/phy/rf/rf_soapy_imp.c | 129 ++++++++++++++++++++++++++++------
 lib/src/phy/rf/rf_soapy_imp.h |  11 ++-
 3 files changed, 117 insertions(+), 25 deletions(-)

diff --git a/lib/src/phy/rf/rf_dev.h b/lib/src/phy/rf/rf_dev.h
index 00f157b6b..d41adbeed 100644
--- a/lib/src/phy/rf/rf_dev.h
+++ b/lib/src/phy/rf/rf_dev.h
@@ -177,7 +177,7 @@ static rf_dev_t dev_soapy = {
   rf_soapy_recv_with_time,
   rf_soapy_recv_with_time_multi,
   rf_soapy_send_timed,
-  .srslte_rf_send_timed_multi = /* FIXME: Implement srslte_rf_send_timed_multi for Soapy SDR */ NULL,
+  .srslte_rf_send_timed_multi = rf_soapy_send_timed_multi,
   rf_soapy_set_tx_cal,
   rf_soapy_set_rx_cal
 };
diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 084308116..0eaf34c32 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -248,12 +248,12 @@ int rf_soapy_open(char *args, void **h)
 int rf_soapy_close(void *h)
 {
   rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
-  if (handler->txStream) {
+  if (handler->tx_stream_active) {
     rf_soapy_stop_tx_stream(handler);
     SoapySDRDevice_closeStream(handler->device, handler->txStream);
   }
   
-  if (handler->rxStream) {
+  if (handler->rx_stream_active) {
     rf_soapy_stop_rx_stream(handler);
     SoapySDRDevice_closeStream(handler->device, handler->rxStream);
   }
@@ -285,6 +285,12 @@ double rf_soapy_set_rx_srate(void *h, double rate)
     printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
+
+  if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
+    printf("setBandwidth failed: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+
   return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_RX,0);
 }
 
@@ -295,6 +301,12 @@ double rf_soapy_set_tx_srate(void *h, double rate)
     printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
+
+  if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
+    printf("setBandwidth failed: %s\n", SoapySDRDevice_lastError());
+    return SRSLTE_ERROR;
+  }
+
   return SoapySDRDevice_getSampleRate(handler->device, SOAPY_SDR_TX,0);
 }
 
@@ -345,7 +357,15 @@ double rf_soapy_set_rx_freq(void *h, double freq)
     printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
-  
+
+  // Todo: expose antenna setting
+  if (SoapySDRDevice_setAntenna(handler->device, SOAPY_SDR_RX, 0, "LNAH") != 0) {
+    fprintf(stderr, "Failed to set Rx antenna.\n");
+  }
+
+  char *ant = SoapySDRDevice_getAntenna(handler->device, SOAPY_SDR_RX, 0);
+  printf("Rx antenna set to %s\n", ant);
+
   return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_RX, 0);
 }
 
@@ -357,6 +377,16 @@ double rf_soapy_set_tx_freq(void *h, double freq)
     printf("setFrequency fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
+
+  // Todo: expose antenna name in arguments
+  if (SoapySDRDevice_setAntenna(handler->device, SOAPY_SDR_TX, 0, "BAND1") != 0) {
+    fprintf(stderr, "Failed to set Tx antenna.\n");
+  }
+
+
+  char *ant = SoapySDRDevice_getAntenna(handler->device, SOAPY_SDR_TX, 0);
+  printf("Tx antenna set to %s\n", ant);
+
   return SoapySDRDevice_getFrequency(handler->device, SOAPY_SDR_TX, 0);
 }
 
@@ -366,6 +396,7 @@ void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs)
 
 }
 
+
 //TODO: add multi-channel support
 int  rf_soapy_recv_with_time_multi(void *h,
                                    void **data,
@@ -405,17 +436,22 @@ int  rf_soapy_recv_with_time_multi(void *h,
       }
     }
 
+#if 0
+    *secs = timeNs / 1000000000;
+    *frac_secs = (timeNs % 1000000000)/1000000000;
+    printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
+#endif
+
     n += ret;
     trials++;
   } while (n < nsamples && trials < 100);
   
   
-  //*secs = timeNs / 1000000000;
-  //*frac_secs = (timeNs % 1000000000)/1000000000;
-  // printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
+
   return n;
 }
 
+
 int rf_soapy_recv_with_time(void *h,
                             void *data,
                             uint32_t nsamples,
@@ -428,19 +464,37 @@ int rf_soapy_recv_with_time(void *h,
 
 
 int rf_soapy_send_timed(void *h,
-                        void *data,
-                        int nsamples,
-                        time_t secs,
-                        double frac_secs,
-                        bool has_time_spec,
-                        bool blocking,
-                        bool is_start_of_burst,
-                        bool is_end_of_burst)
-{
-  int flags;
+                      void *data,
+                      int nsamples,
+                      time_t secs,
+                      double frac_secs,
+                      bool has_time_spec,
+                      bool blocking,
+                      bool is_start_of_burst,
+                      bool is_end_of_burst)
+{
+  void *_data[SRSLTE_MAX_PORTS]= {data, zero_mem, zero_mem, zero_mem};
+  return rf_soapy_send_timed_multi(h, _data, nsamples, secs, frac_secs, has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
+}
+
+
+// Todo: Check correct handling of flags, use RF metrics API, fix timed transmissions
+int rf_soapy_send_timed_multi(void *h,
+                            void *data[SRSLTE_MAX_PORTS],
+                            int nsamples,
+                            time_t secs,
+                            double frac_secs,
+                            bool has_time_spec,
+                            bool blocking,
+                            bool is_start_of_burst,
+                            bool is_end_of_burst)
+{
+  int flags = 0;
+  const long timeoutUs = 2000; // arbitrarily chosen
   long long timeNs;
   int trials = 0;
   int ret = 0;
+
   rf_soapy_handler_t *handler = (rf_soapy_handler_t *) h;
   timeNs = secs * 1000000000;
   timeNs = timeNs + (frac_secs * 1000000000);
@@ -449,25 +503,54 @@ int rf_soapy_send_timed(void *h,
   if (!handler->tx_stream_active) {
     rf_soapy_start_tx_stream(h);
   }
-  
-  cf_t *data_c = (cf_t *) data;
+
+  //printf("send_timed_multi(): time_spec=%d blocking=%d, is_start_of_burst=%d is_end_of_burst=%d\n", has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
+
+  if (is_start_of_burst && is_end_of_burst) {
+    flags |= SOAPY_SDR_ONE_PACKET;
+  }
+
+  if (is_end_of_burst) {
+    flags |= SOAPY_SDR_END_BURST;
+  }
+
+  if (has_time_spec) {
+    flags |= SOAPY_SDR_HAS_TIME;
+  }
+
   do {
     size_t tx_samples = nsamples;
     if (tx_samples > nsamples - n) {
       tx_samples = nsamples - n;
     }
-    void *buff = (void *) &data_c[n];
-    const void *buffs_ptr[1] = {buff};
-    ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, buffs_ptr, tx_samples, &flags, timeNs, 10000);
-    if (ret < 0)
+
+    ret = SoapySDRDevice_writeStream(handler->device, handler->txStream, (const void *)data, tx_samples, &flags, timeNs, timeoutUs);
+    if (ret == SOAPY_SDR_TIMEOUT) {
+      printf("L");
+      continue;
+    }
+    if (ret == SOAPY_SDR_OVERFLOW) {
+      printf("O");
+      continue;
+    }
+    if (ret == SOAPY_SDR_UNDERFLOW) {
+      printf("U");
+      continue;
+    }
+    if (ret < 0) {
+      fprintf(stderr, "Error during writeStream\n");
+      exit(-1);
       return SRSLTE_ERROR;
+    }
 
     n += ret;
     trials++;
   } while (n < nsamples && trials < 100);
 
-  if (ret != nsamples)
+  if (n != nsamples) {
+    fprintf(stderr, "Couldn't write all samples.\n");
     return SRSLTE_ERROR;
+  }
 
   return ret;
 }
diff --git a/lib/src/phy/rf/rf_soapy_imp.h b/lib/src/phy/rf/rf_soapy_imp.h
index 23b59a8b3..19de4536c 100644
--- a/lib/src/phy/rf/rf_soapy_imp.h
+++ b/lib/src/phy/rf/rf_soapy_imp.h
@@ -106,7 +106,7 @@ SRSLTE_API void rf_soapy_get_time(void *h,
                               time_t *secs, 
                               double *frac_secs); 
 
-SRSLTE_API int  rf_soapy_send_timed(void *h, 
+SRSLTE_API int rf_soapy_send_timed(void *h,
                                   void *data, 
                                   int nsamples,
                                   time_t secs, 
@@ -116,3 +116,12 @@ SRSLTE_API int  rf_soapy_send_timed(void *h,
                                   bool is_start_of_burst, 
                                   bool is_end_of_burst);
 
+int rf_soapy_send_timed_multi(void *h,
+                              void *data[4],
+                              int nsamples,
+                              time_t secs,
+                              double frac_secs,
+                              bool has_time_spec,
+                              bool blocking,
+                              bool is_start_of_burst,
+                              bool is_end_of_burst);

From 5e9750f2f6e565ceb784eeb593f6697f6c69784b Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Thu, 5 Oct 2017 21:17:17 +0200
Subject: [PATCH 256/269] remove clutter from enb.conf.example

---
 srsenb/enb.conf.example | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsenb/enb.conf.example b/srsenb/enb.conf.example
index 391eb8af6..d88b63bb7 100644
--- a/srsenb/enb.conf.example
+++ b/srsenb/enb.conf.example
@@ -100,7 +100,7 @@ filename = /tmp/enb.pcap
 #
 # filename: File path to use for log output. Can be set to stdout
 #           to print logs to standard output
-git c#####################################################################
+#####################################################################
 [log]
 all_level = info
 all_hex_limit = 32

From ba91a38da49422e025d5d2518b56515f51f0a21d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 10:11:43 +0200
Subject: [PATCH 257/269] Allow PDCCH scheduling for 6 PRB

---
 srsenb/src/mac/scheduler_ue.cc | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/srsenb/src/mac/scheduler_ue.cc b/srsenb/src/mac/scheduler_ue.cc
index 3eab6b33d..38058415f 100644
--- a/srsenb/src/mac/scheduler_ue.cc
+++ b/srsenb/src/mac/scheduler_ue.cc
@@ -29,6 +29,7 @@
 #include <srslte/interfaces/sched_interface.h>
 #include <srslte/phy/phch/pucch.h>
 #include <srslte/srslte.h>
+#include <srslte/phy/common/phy_common.h>
 
 #include "srslte/srslte.h"
 #include "srslte/common/pdu.h"
@@ -695,10 +696,16 @@ uint32_t sched_ue::get_aggr_level(uint32_t nof_bits)
   uint32_t l=0;
   float max_coderate = srslte_cqi_to_coderate(dl_cqi);
   float coderate = 99;
+  float factor=1.5;
+  uint32_t l_max = 3;
+  if (cell.nof_prb == 6) {
+    factor = 1.0;
+    l_max  = 2;
+  }
   do {
     coderate = srslte_pdcch_coderate(nof_bits, l);
     l++;
-  } while(l<3 && 1.5*coderate > max_coderate);
+  } while(l<l_max && factor*coderate > max_coderate);
   Debug("SCHED: CQI=%d, l=%d, nof_bits=%d, coderate=%.2f, max_coderate=%.2f\n", dl_cqi, l, nof_bits, coderate, max_coderate);
   return l; 
 }

From c2c637e06a2c958557b9481f645a5d742a1ac5ff Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 10:12:02 +0200
Subject: [PATCH 258/269] Fixed out-of-bounds checking in rlc_um resegmentation

---
 lib/src/upper/rlc_um.cc | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/lib/src/upper/rlc_um.cc b/lib/src/upper/rlc_um.cc
index d44df6348..b2697178c 100644
--- a/lib/src/upper/rlc_um.cc
+++ b/lib/src/upper/rlc_um.cc
@@ -513,10 +513,10 @@ void rlc_um::reassemble_rx_sdus()
     }
     
     // Handle last segment
-    // Handle last segment
-    if (rx_sdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES                 ||
-        rx_window[vr_ur].buf->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES   ||
-        rx_window[vr_ur].buf->N_bytes + rx_sdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES) {
+    if (rx_sdu->N_bytes                                 < SRSLTE_MAX_BUFFER_SIZE_BYTES   &&
+        rx_window[vr_ur].buf->N_bytes                   < SRSLTE_MAX_BUFFER_SIZE_BYTES   &&
+        rx_window[vr_ur].buf->N_bytes + rx_sdu->N_bytes < SRSLTE_MAX_BUFFER_SIZE_BYTES)
+    {
 
       memcpy(&rx_sdu->msg[rx_sdu->N_bytes], rx_window[vr_ur].buf->msg, rx_window[vr_ur].buf->N_bytes);
       rx_sdu->N_bytes += rx_window[vr_ur].buf->N_bytes;

From 5bd92fb6581d5294a65c87803adcacff6e533b27 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 10:30:58 +0200
Subject: [PATCH 259/269] fix prach example and close rf at exit

---
 lib/src/phy/phch/test/prach_test_usrp.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/lib/src/phy/phch/test/prach_test_usrp.c b/lib/src/phy/phch/test/prach_test_usrp.c
index 2defdec4f..c0fae365c 100644
--- a/lib/src/phy/phch/test/prach_test_usrp.c
+++ b/lib/src/phy/phch/test/prach_test_usrp.c
@@ -231,7 +231,8 @@ int main(int argc, char **argv) {
   }
   
   srslte_vec_save_file(output_filename,buffer,11*flen*sizeof(cf_t));
-  
+
+  srslte_rf_close(&rf);
   srslte_prach_free(p);
   free(p);
 

From f619f53cc460df6230b69e2598550b190720f1d6 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 10:40:59 +0200
Subject: [PATCH 260/269] fix soapy support

---
 lib/src/phy/rf/rf_soapy_imp.c | 36 +++++++++++++++++------------------
 1 file changed, 18 insertions(+), 18 deletions(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index 0eaf34c32..e1b451412 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -174,7 +174,7 @@ void rf_soapy_flush_buffer(void *h)
 
 bool rf_soapy_has_rssi(void *h)
 {
-  printf("TODO: implement rf_soapy_has_rssi()\n");
+  // TODO: implement rf_soapy_has_rssi()
   return false;
 }
 
@@ -282,12 +282,12 @@ double rf_soapy_set_rx_srate(void *h, double rate)
 {
   rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
   if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
-    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    printf("setSampleRate Rx fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
 
   if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_RX, 0, rate) != 0) {
-    printf("setBandwidth failed: %s\n", SoapySDRDevice_lastError());
+    printf("setBandwidth Rx failed: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
 
@@ -298,12 +298,12 @@ double rf_soapy_set_tx_srate(void *h, double rate)
 {
   rf_soapy_handler_t *handler = (rf_soapy_handler_t*) h;
   if (SoapySDRDevice_setSampleRate(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
-    printf("setSampleRate fail: %s\n", SoapySDRDevice_lastError());
+    printf("setSampleRate Tx fail: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
 
   if (SoapySDRDevice_setBandwidth(handler->device, SOAPY_SDR_TX, 0, rate) != 0) {
-    printf("setBandwidth failed: %s\n", SoapySDRDevice_lastError());
+    printf("setBandwidth Tx failed: %s\n", SoapySDRDevice_lastError());
     return SRSLTE_ERROR;
   }
 
@@ -393,7 +393,7 @@ double rf_soapy_set_tx_freq(void *h, double freq)
 
 void rf_soapy_get_time(void *h, time_t *secs, double *frac_secs)
 {
-
+  printf("Todo: implement rf_soapy_get_time()\n");
 }
 
 
@@ -424,7 +424,7 @@ int  rf_soapy_recv_with_time_multi(void *h,
       cf_t *data_c = (cf_t*) data[i];
       buffs_ptr[i] = &data_c[n];
     }
-    ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr , rx_samples, &flags, &timeNs, 1000000);
+    ret = SoapySDRDevice_readStream(handler->device, handler->rxStream, buffs_ptr, rx_samples, &flags, &timeNs, 10000);
     if(ret < 0) {
       // continue when getting overflows
       if (ret == SOAPY_SDR_OVERFLOW) {
@@ -436,11 +436,12 @@ int  rf_soapy_recv_with_time_multi(void *h,
       }
     }
 
-#if 0
-    *secs = timeNs / 1000000000;
-    *frac_secs = (timeNs % 1000000000)/1000000000;
-    printf("ret=%d, flags=%d, timeNs=%lld\n", ret, flags, timeNs);
-#endif
+    // update rx time
+    if (secs != NULL && frac_secs != NULL) {
+      *secs = timeNs / 1e9;
+      *frac_secs = (timeNs % 1000000000)/1e9;
+      //printf("rx_time: secs=%d, frac_secs=%lf timeNs=%lld\n", *secs, *frac_secs, timeNs);
+    }
 
     n += ret;
     trials++;
@@ -489,23 +490,19 @@ int rf_soapy_send_timed_multi(void *h,
                             bool is_start_of_burst,
                             bool is_end_of_burst)
 {
+  rf_soapy_handler_t *handler = (rf_soapy_handler_t *) h;
   int flags = 0;
   const long timeoutUs = 2000; // arbitrarily chosen
   long long timeNs;
   int trials = 0;
   int ret = 0;
-
-  rf_soapy_handler_t *handler = (rf_soapy_handler_t *) h;
-  timeNs = secs * 1000000000;
-  timeNs = timeNs + (frac_secs * 1000000000);
   int n = 0;
 
+
   if (!handler->tx_stream_active) {
     rf_soapy_start_tx_stream(h);
   }
 
-  //printf("send_timed_multi(): time_spec=%d blocking=%d, is_start_of_burst=%d is_end_of_burst=%d\n", has_time_spec, blocking, is_start_of_burst, is_end_of_burst);
-
   if (is_start_of_burst && is_end_of_burst) {
     flags |= SOAPY_SDR_ONE_PACKET;
   }
@@ -516,6 +513,9 @@ int rf_soapy_send_timed_multi(void *h,
 
   if (has_time_spec) {
     flags |= SOAPY_SDR_HAS_TIME;
+    timeNs = secs * 1000000000;
+    timeNs = timeNs + (frac_secs * 1000000000);
+    //printf("time_spec: secs=%d, frac_secs=%lf timeNs=%lld\n", secs, frac_secs, timeNs);
   }
 
   do {

From 0c3d4e9ad313110552bff2cf4d08555f48e2472d Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 14:04:14 +0200
Subject: [PATCH 261/269] Removed log

---
 srsue/src/mac/mac.cc | 1 -
 1 file changed, 1 deletion(-)

diff --git a/srsue/src/mac/mac.cc b/srsue/src/mac/mac.cc
index 60327fcd4..b54ea5be6 100644
--- a/srsue/src/mac/mac.cc
+++ b/srsue/src/mac/mac.cc
@@ -307,7 +307,6 @@ void mac::new_grant_ul(mac_interface_phy::mac_grant_t grant, mac_interface_phy::
 
 void mac::new_grant_ul_ack(mac_interface_phy::mac_grant_t grant, bool ack, mac_interface_phy::tb_action_ul_t* action)
 {
-  log_h->info("new_grant_ul_ack\n");
   int tbs = ul_harq.get_current_tbs(tti);
   ul_harq.new_grant_ul_ack(grant, ack, action);
   if (!ack) {

From 404971703c1ea92e4bf31b8f6759dc04025f93b1 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 14:04:33 +0200
Subject: [PATCH 262/269] PLMN update only on PLMN search state

---
 srsue/src/upper/rrc.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/srsue/src/upper/rrc.cc b/srsue/src/upper/rrc.cc
index 3bb3e0017..1d614aed4 100644
--- a/srsue/src/upper/rrc.cc
+++ b/srsue/src/upper/rrc.cc
@@ -427,7 +427,7 @@ void rrc::cell_found(uint32_t earfcn, srslte_cell_t phy_cell, float rsrp) {
 
       if (!known_cells[i].has_valid_sib1) {
         si_acquire_state = SI_ACQUIRE_SIB1;
-      } else {
+      } else if (state == RRC_STATE_PLMN_SELECTION) {
         for (uint32_t i = 0; i < current_cell->sib1.N_plmn_ids; i++) {
           nas->plmn_found(current_cell->sib1.plmn_id[i].id, current_cell->sib1.tracking_area_code);
         }

From eb07dacfe94cf6844fceab65f5b656c53aab9b95 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 14:33:22 +0200
Subject: [PATCH 263/269] Cleaning logging format

---
 lib/src/common/pdu_queue.cc  |  4 ++--
 srsue/src/phy/phch_worker.cc | 32 ++++++++++++++++----------------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/lib/src/common/pdu_queue.cc b/lib/src/common/pdu_queue.cc
index a1bf7cd59..6b4c8bfd9 100644
--- a/lib/src/common/pdu_queue.cc
+++ b/lib/src/common/pdu_queue.cc
@@ -66,7 +66,7 @@ uint8_t* pdu_queue::request(uint32_t len)
 void pdu_queue::deallocate(uint8_t* pdu)
 {
   if (!pool.deallocate((pdu_t*) pdu)) {
-    log_h->warning("Error deallocating from buffer pool: buffer not created in this pool.\n");
+    log_h->warning("Error deallocating from buffer pool in deallocate(): buffer not created in this pool.\n");
   }
 }
 
@@ -92,7 +92,7 @@ bool pdu_queue::process_pdus()
       callback->process_pdu(pdu->ptr, pdu->len, pdu->tstamp);
     }
     if (!pool.deallocate(pdu)) {
-      log_h->warning("Error deallocating from buffer pool: buffer not created in this pool.\n");
+      log_h->warning("Error deallocating from buffer pool in process_pdus(): buffer not created in this pool.\n");
     }
     cnt++;
     have_data = true;
diff --git a/srsue/src/phy/phch_worker.cc b/srsue/src/phy/phch_worker.cc
index 1c9abc1ca..c0fec2ba2 100644
--- a/srsue/src/phy/phch_worker.cc
+++ b/srsue/src/phy/phch_worker.cc
@@ -915,16 +915,16 @@ void phch_worker::encode_pusch(srslte_ra_ul_grant_t *grant, uint8_t *payload, ui
 #ifdef LOG_EXECTIME
   gettimeofday(&logtime_start[2], NULL);
   get_time_interval(logtime_start);
-  snprintf(timestr, 64, ", total_time=%4d us", (int) logtime_start[0].tv_usec);
+  snprintf(timestr, 64, ", tot_time=%4d us", (int) logtime_start[0].tv_usec);
 #endif
 
-  Info("PUSCH: tti_tx=%d, n_prb=%d, rb_start=%d, tbs=%d, mod=%d, mcs=%d, rv_idx=%d, ack=%s, ri=%s, cfo=%.1f Hz%s\n",
+  Info("PUSCH: tti_tx=%d, alloc=(%d,%d), tbs=%d, mcs=%d, rv=%d, ack=%s, ri=%s, cfo=%.1f KHz%s\n",
          (tti+4)%10240,
-         grant->L_prb, grant->n_prb[0], 
-         grant->mcs.tbs/8, grant->mcs.mod, grant->mcs.idx, rv,
+         grant->n_prb[0], grant->n_prb[0]+grant->L_prb,
+         grant->mcs.tbs/8, grant->mcs.idx, rv,
          uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
          uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
-         cfo*15000, timestr);
+         cfo*15, timestr);
 
   // Store metrics
   ul_metrics.mcs   = grant->mcs.idx;
@@ -959,22 +959,22 @@ void phch_worker::encode_pucch()
   memcpy(&t[2], &logtime_start[2], sizeof(struct timeval));
   get_time_interval(logtime_start);
   get_time_interval(t);
-  snprintf(timestr, 64, ", enc_time=%d, total_time=%d us", (int) t[0].tv_usec, (int) logtime_start[0].tv_usec);
+  snprintf(timestr, 64, ", tot_time=%d us", (int) logtime_start[0].tv_usec);
 #endif
 
   float tx_power = srslte_ue_ul_pucch_power(&ue_ul, phy->pathloss, ue_ul.last_pucch_format, uci_data.uci_cqi_len, uci_data.uci_ack_len);
   float gain = set_power(tx_power);  
   
-  Info("PUCCH: tti_tx=%d, n_cce=%3d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f Hz%s\n",
+  Info("PUCCH: tti_tx=%d, n_pucch=%d, n_prb=%d, ack=%s%s, ri=%s, pmi=%s%s, sr=%s, cfo=%.1f KHz%s\n",
          (tti+4)%10240,
-         last_dl_pdcch_ncce, ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb, 
-       uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
-       uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
-       uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
-       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[1]?"1":"0"):"no",
-       uci_data.uci_pmi_len>0?(uci_data.uci_pmi[0]?"1":"0"):"",
-       uci_data.scheduling_request?"yes":"no",
-         cfo*15000, timestr);        
+         ue_ul.pucch.last_n_pucch, ue_ul.pucch.last_n_prb,
+         uci_data.uci_ack_len>0?(uci_data.uci_ack?"1":"0"):"no",
+         uci_data.uci_ack_len>1?(uci_data.uci_ack_2?"1":"0"):"",
+         uci_data.uci_ri_len>0?(uci_data.uci_ri?"1":"0"):"no",
+         uci_data.uci_pmi_len>0?(uci_data.uci_pmi[1]?"1":"0"):"no",
+         uci_data.uci_pmi_len>0?(uci_data.uci_pmi[0]?"1":"0"):"",
+         uci_data.scheduling_request?"yes":"no",
+         cfo*15, timestr);
   }   
   
   if (uci_data.scheduling_request) {
@@ -995,7 +995,7 @@ void phch_worker::encode_srs()
 #ifdef LOG_EXECTIME
   gettimeofday(&logtime_start[2], NULL);
   get_time_interval(logtime_start);
-  snprintf(timestr, 64, ", total_time=%4d us", (int) logtime_start[0].tv_usec);
+  snprintf(timestr, 64, ", tot_time=%4d us", (int) logtime_start[0].tv_usec);
 #endif
   
   float tx_power = srslte_ue_ul_srs_power(&ue_ul, phy->pathloss);  

From 0c263b123fbc8c9d5db88c297d7349e80e290e93 Mon Sep 17 00:00:00 2001
From: Ismael Gomez <ismagom@gmail.com>
Date: Fri, 6 Oct 2017 14:56:25 +0200
Subject: [PATCH 264/269] Reset HARQ process if TB can't be scheduled

---
 srsenb/src/mac/scheduler.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/srsenb/src/mac/scheduler.cc b/srsenb/src/mac/scheduler.cc
index 539b7725f..801b68d38 100644
--- a/srsenb/src/mac/scheduler.cc
+++ b/srsenb/src/mac/scheduler.cc
@@ -619,6 +619,7 @@ int sched::dl_sched_data(dl_sched_data_t data[MAX_DATA_LIST])
                       tbs, user->get_pending_dl_new_data(current_tti));          
         }      
       } else {
+        h->reset();
         Warning("SCHED: Could not schedule DL DCI for rnti=0x%x, pid=%d\n", rnti, h->get_id());              
       }      
     }    
@@ -782,6 +783,7 @@ int sched::ul_sched(uint32_t tti, srsenb::sched_interface::ul_sched_res_t* sched
             user->get_locations(current_cfi, sf_idx), 
             aggr_level)) 
         {
+          h->reset();
           log_h->warning("SCHED: Could not schedule UL DCI rnti=0x%x, pid=%d, L=%d\n", 
                           rnti, h->get_id(), aggr_level);          
           sched_result->pusch[nof_dci_elems].needs_pdcch = false; 

From 5d5e8167b7e4b5dd7ea08a952d37fd77f314a8f8 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 15:04:17 +0200
Subject: [PATCH 265/269] stop radio after radio error

---
 lib/src/phy/rf/rf_soapy_imp.c | 1 -
 srsue/src/phy/phch_recv.cc    | 1 +
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index e1b451412..ee646c2c2 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -383,7 +383,6 @@ double rf_soapy_set_tx_freq(void *h, double freq)
     fprintf(stderr, "Failed to set Tx antenna.\n");
   }
 
-
   char *ant = SoapySDRDevice_getAntenna(handler->device, SOAPY_SDR_TX, 0);
   printf("Tx antenna set to %s\n", ant);
 
diff --git a/srsue/src/phy/phch_recv.cc b/srsue/src/phy/phch_recv.cc
index dad2c82b8..718b15b81 100644
--- a/srsue/src/phy/phch_recv.cc
+++ b/srsue/src/phy/phch_recv.cc
@@ -171,6 +171,7 @@ void phch_recv::radio_error() {
 
   // Need to find a method to effectively reset radio, reloading the driver does not work
   //radio_h->reset();
+  radio_h->stop();
 
   fprintf(stdout, "Error while receiving samples. Restart srsUE\n");
   exit(-1);

From f4e13c4a562727ebba2943eebf2b66fc97ebe0d5 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 15:05:47 +0200
Subject: [PATCH 266/269] update readme

---
 README.md | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index a6eada615..e588ea1fd 100644
--- a/README.md
+++ b/README.md
@@ -15,11 +15,12 @@ srsLTE is released under the AGPLv3 license and uses software from the OpenLTE p
 Common Features
 ---------------
 
- * LTE Release 8 compliant
+ * LTE Release 8 compliant (with selected features of Release 9)
  * FDD configuration
  * Tested bandwidths: 1.4, 3, 5, 10, 15 and 20 MHz
- * Transmission mode 1 (single antenna) and 2 (transmit diversity) 
+ * Transmission mode 1 (single antenna), 2 (transmit diversity), 3 (transmit diversity/CCD) and 4 (closed-loop spatial multiplexing)
  * Frequency-based ZF and MMSE equalizer
+ * Evolved multimedia broadcast and multicast service (eMBMS)
  * Highly optimized Turbo Decoder available in Intel SSE4.1/AVX (+100 Mbps) and standard C (+25 Mbps)
  * MAC, RLC, PDCP, RRC, NAS, S1AP and GW layers
  * Detailed log system with per-layer log levels and hex dumps
@@ -33,6 +34,7 @@ srsUE Features
  * Cell search and synchronization procedure for the UE
  * Soft USIM supporting Milenage and XOR authentication 
  * Virtual network interface *tun_srsue* created upon network attach
+ * >100 Mbps DL in 20 MHz MIMO TM4 configuration in i7 Quad-Core CPU.
  * 75 Mbps DL in 20 MHz SISO configuration in i7 Quad-Core CPU.
  * 36 Mbps DL in 10 MHz SISO configuration in i5 Dual-Core CPU.
 
@@ -65,7 +67,7 @@ We have tested the following hardware:
  * USRP B210
  * USRP X300
  * bladeRF
- * limeSDR
+ * limeSDR (currently, only the PHY-layer examples, i.e., pdsch_enodeb/ue are supported)
 
 Build Instructions
 ------------------

From b2a4d93fb89f765a82c9fcf8aad1369e8199155a Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 15:07:19 +0200
Subject: [PATCH 267/269] update readme

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index e588ea1fd..c891ab219 100644
--- a/README.md
+++ b/README.md
@@ -34,7 +34,7 @@ srsUE Features
  * Cell search and synchronization procedure for the UE
  * Soft USIM supporting Milenage and XOR authentication 
  * Virtual network interface *tun_srsue* created upon network attach
- * >100 Mbps DL in 20 MHz MIMO TM4 configuration in i7 Quad-Core CPU.
+ * +100 Mbps DL in 20 MHz MIMO TM4 configuration in i7 Quad-Core CPU.
  * 75 Mbps DL in 20 MHz SISO configuration in i7 Quad-Core CPU.
  * 36 Mbps DL in 10 MHz SISO configuration in i5 Dual-Core CPU.
 

From cfc9432ee20b63b377fd0a7b2fa835fd4e592d4e Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 16:14:16 +0200
Subject: [PATCH 268/269] fix warning

---
 lib/src/phy/rf/rf_soapy_imp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/phy/rf/rf_soapy_imp.c b/lib/src/phy/rf/rf_soapy_imp.c
index ee646c2c2..fef533ffc 100644
--- a/lib/src/phy/rf/rf_soapy_imp.c
+++ b/lib/src/phy/rf/rf_soapy_imp.c
@@ -492,7 +492,7 @@ int rf_soapy_send_timed_multi(void *h,
   rf_soapy_handler_t *handler = (rf_soapy_handler_t *) h;
   int flags = 0;
   const long timeoutUs = 2000; // arbitrarily chosen
-  long long timeNs;
+  long long timeNs = 0;
   int trials = 0;
   int ret = 0;
   int n = 0;

From 270a15fe1af8de1944eab1639a8db0188ef51499 Mon Sep 17 00:00:00 2001
From: Andre Puschmann <andre@softwareradiosystems.com>
Date: Fri, 6 Oct 2017 16:14:58 +0200
Subject: [PATCH 269/269] update version

---
 cmake/modules/SRSLTEVersion.cmake | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/cmake/modules/SRSLTEVersion.cmake b/cmake/modules/SRSLTEVersion.cmake
index d2ab204f5..b52e47fe5 100644
--- a/cmake/modules/SRSLTEVersion.cmake
+++ b/cmake/modules/SRSLTEVersion.cmake
@@ -18,7 +18,7 @@
 # and at http://www.gnu.org/licenses/.
 #
 
-SET(SRSLTE_VERSION_MAJOR 002)
-SET(SRSLTE_VERSION_MINOR 000)
-SET(SRSLTE_VERSION_PATCH 000)
+SET(SRSLTE_VERSION_MAJOR 17)
+SET(SRSLTE_VERSION_MINOR 9)
+SET(SRSLTE_VERSION_PATCH 0)
 SET(SRSLTE_VERSION_STRING "${SRSLTE_VERSION_MAJOR}.${SRSLTE_VERSION_MINOR}.${SRSLTE_VERSION_PATCH}")