diff --git a/common/utils/itti/intertask_interface.c b/common/utils/itti/intertask_interface.c
index 6c9a95732bccddc1d96f19f6529e4f2d980554fe..66b5e9aedcd41461ebd840f7dd678113a78c8da4 100644
--- a/common/utils/itti/intertask_interface.c
+++ b/common/utils/itti/intertask_interface.c
@@ -63,6 +63,9 @@
 
 #include "signals.h"
 #include "timer.h"
+#ifdef UE_EXPANSION
+#include "log.h"
+#endif
 
 #ifdef RTAI
 # include <rtai.h>
@@ -455,7 +458,21 @@ int itti_send_msg_to_task(task_id_t destination_task_id, instance_t instance, Me
 
       /* Enqueue message in destination task queue */
       if (lfds611_queue_enqueue(itti_desc.tasks[destination_task_id].message_queue, new) == 0) {
+#ifdef UE_EXPANSION
+        LOG_I(UDP_, " Assertion Message %s(id:%d), number %lu with priority %d can not be sent from (%u:%s) to queue (%u:%s). discarding...\n",
+                      itti_desc.messages_info[message_id].name,
+                      message_id,
+                      message_number,
+                      priority,
+                      origin_task_id,
+                      itti_get_task_name(origin_task_id),
+                      destination_task_id,
+                      itti_get_task_name(destination_task_id));
+        int result = itti_free(origin_task_id, message);
+        AssertFatal( result == EXIT_SUCCESS, "Failed to free memory (%d)!\n", result);
+#else
         AssertFatal(0, "Error: lfds611_queue_enqueue returns 0, queue is full, exiting\n");
+#endif
       }
 
 #if defined(OAI_EMU) || defined(RTAI)
@@ -625,7 +642,14 @@ static inline void itti_receive_msg_internal_event_fd(task_id_t task_id, uint8_t
 
       if (lfds611_queue_dequeue (itti_desc.tasks[task_id].message_queue, (void **) &message) == 0) {
         /* No element in list -> this should not happen */
+#ifdef UE_EXPANSION
+        LOG_I(UDP_, "Assertion No message in queue for task %d while there are %d events and some for the messages queue!\n", task_id, epoll_ret);
+        /* Mark that the event has been processed */
+        itti_desc.threads[thread_id].events[i].events &= ~EPOLLIN;
+        return;
+#else
         AssertFatal (0, "No message in queue for task %d while there are %d events and some for the messages queue!\n", task_id, epoll_ret);
+#endif
       }
 
       AssertFatal(message != NULL, "Message from message queue is NULL!\n");
diff --git a/openair1/PHY/LTE_TRANSPORT/dci_tools.c b/openair1/PHY/LTE_TRANSPORT/dci_tools.c
index e6fce0ab6ae932d56fdcc516415c41c0a27848a2..6fce45ab1ee720e6a67a30171d91cce395275f9a 100644
--- a/openair1/PHY/LTE_TRANSPORT/dci_tools.c
+++ b/openair1/PHY/LTE_TRANSPORT/dci_tools.c
@@ -2639,8 +2639,15 @@ void fill_ulsch(PHY_VARS_eNB *eNB,int UE_id,nfapi_ul_config_ulsch_pdu *ulsch_pdu
     ulsch->harq_processes[harq_pid]->Or1           = 0;
     ulsch->harq_processes[harq_pid]->Or2           = 0;
   }
+#ifndef UE_EXPANSION
   else  ulsch->harq_processes[harq_pid]->round++;
-
+#else
+  else {
+      ulsch->harq_processes[harq_pid]->round++;
+      ulsch->harq_processes[harq_pid]->TBS           = ulsch_pdu->ulsch_pdu_rel8.size<<3;
+      ulsch->harq_processes[harq_pid]->Msc_initial   = 12*ulsch_pdu->ulsch_pdu_rel8.number_of_resource_blocks;
+  }
+#endif
   ulsch->rnti = ulsch_pdu->ulsch_pdu_rel8.rnti;
   LOG_D(PHY,"Filling ULSCH %x (UE_id %d) (new_ulsch %d) for Frame %d, Subframe %d : harq_pid %d, first_rb %d, nb_rb %d, rvidx %d, Qm %d, TBS %d, round %d \n",
 	ulsch->rnti,
diff --git a/openair1/PHY/LTE_TRANSPORT/ulsch_decoding.c b/openair1/PHY/LTE_TRANSPORT/ulsch_decoding.c
index f885bf8eb379d0beaa3ff96023c14452a9633a13..defe7b56483018389de9709611e738ee24770e28 100644
--- a/openair1/PHY/LTE_TRANSPORT/ulsch_decoding.c
+++ b/openair1/PHY/LTE_TRANSPORT/ulsch_decoding.c
@@ -916,8 +916,9 @@ unsigned int  ulsch_decoding(PHY_VARS_eNB *eNB,eNB_rxtx_proc_t *proc,
 	G);
 	
   //#endif
-
+#ifndef UE_EXPANSION
   if (ulsch_harq->round == 0) {
+#endif
     // This is a new packet, so compute quantities regarding segmentation
     ulsch_harq->B = A+24;
     lte_segmentation(NULL,
@@ -930,8 +931,9 @@ unsigned int  ulsch_decoding(PHY_VARS_eNB *eNB,eNB_rxtx_proc_t *proc,
                      &ulsch_harq->Kminus,
                      &ulsch_harq->F);
     //  CLEAR LLR's HERE for first packet in process
+#ifndef UE_EXPANSION
   }
-
+#endif
   //  printf("after segmentation c[%d] = %p\n",0,ulsch_harq->c[0]);
 
   sumKr = 0;
diff --git a/openair2/LAYER2/MAC/eNB_scheduler.c b/openair2/LAYER2/MAC/eNB_scheduler.c
index 2c3b88306a821b435d7d31da6364ee710395f6ee..80032d6b99f07574d127506a4e790def63315825 100644
--- a/openair2/LAYER2/MAC/eNB_scheduler.c
+++ b/openair2/LAYER2/MAC/eNB_scheduler.c
@@ -549,7 +549,7 @@ check_ul_failure(module_id_t module_idP, int CC_id, int UE_id,
 
 	UE_list->UE_sched_ctrl[UE_id].ul_failure_timer++;
 	// check threshold
-	if (UE_list->UE_sched_ctrl[UE_id].ul_failure_timer > 200) {
+	if (UE_list->UE_sched_ctrl[UE_id].ul_failure_timer > 20000) {
 	    // inform RRC of failure and clear timer
 	    LOG_I(MAC,
 		  "UE %d rnti %x: UL Failure after repeated PDCCH orders: Triggering RRC \n",
@@ -724,7 +724,9 @@ eNB_dlsch_ulsch_scheduler(module_id_t module_idP, frame_t frameP,
     }
 
 #endif
-
+#ifdef UE_EXPANSION
+  memset(dlsch_ue_select, 0, sizeof(dlsch_ue_select));
+#endif
     // This schedules MIB
     if ((subframeP == 0) && (frameP & 3) == 0)
 	schedule_mib(module_idP, frameP, subframeP);
diff --git a/openair2/LAYER2/MAC/eNB_scheduler_RA.c b/openair2/LAYER2/MAC/eNB_scheduler_RA.c
index 327f5faafe16182cceb24ed5736661b47b0f186b..c79dee327db19d746dfc86e66131a2caab8d9738 100644
--- a/openair2/LAYER2/MAC/eNB_scheduler_RA.c
+++ b/openair2/LAYER2/MAC/eNB_scheduler_RA.c
@@ -507,6 +507,13 @@ generate_Msg2(module_id_t module_idP, int CC_idP, frame_t frameP,
 		TX_req->segments[0].segment_data =
 		    cc[CC_idP].RAR_pdu.payload;
 		mac->TX_req[CC_idP].tx_request_body.number_of_pdus++;
+#ifdef UE_EXPANSION
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].ue_priority = SCH_DL_MSG2;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].nb_rb = 4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].UE_id = -1;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].rnti = ra->rnti;
+        dlsch_ue_select[CC_idP].ue_num++;
+#endif
 	    }
 	}
 
@@ -634,6 +641,13 @@ generate_Msg2(module_id_t module_idP, int CC_idP, frame_t frameP,
 		TX_req->segments[0].segment_data =
 		    cc[CC_idP].RAR_pdu.payload;
 		mac->TX_req[CC_idP].tx_request_body.number_of_pdus++;
+#ifdef UE_EXPANSION
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].ue_priority = SCH_DL_MSG2;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].nb_rb = 4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].UE_id = -1;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].rnti = ra->rnti;
+        dlsch_ue_select[CC_idP].ue_num++;
+#endif
 	    }			// PDCCH CCE allocation is feasible
 	}			// Msg2 frame/subframe condition
     }				// else BL/CE
@@ -1092,6 +1106,13 @@ generate_Msg4(module_id_t module_idP, int CC_idP, frame_t frameP,
 							      UE_id),
 			  rrc_sdu_length);
 		}
+#ifdef UE_EXPANSION
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].ue_priority = SCH_DL_MSG4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].nb_rb = 4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].UE_id = UE_id;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].rnti = ra->rnti;
+        dlsch_ue_select[CC_idP].ue_num++;
+#endif
 	    }			// Msg4 frame/subframe
 	}			// msg4_mpdcch_repetition_count
     }				// rach_resource_type > 0 
@@ -1293,7 +1314,13 @@ generate_Msg4(module_id_t module_idP, int CC_idP, frame_t frameP,
 							      UE_id),
 			  rrc_sdu_length);
 		}
-
+#ifdef UE_EXPANSION
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].ue_priority = SCH_DL_MSG4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].nb_rb = 4;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].UE_id = UE_id;
+        dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].rnti = ra->rnti;
+        dlsch_ue_select[CC_idP].ue_num++;
+#endif
 	    }			// CCE Allocation feasible
 	}			// msg4 frame/subframe
     }				// else rach_resource_type
@@ -1431,6 +1458,13 @@ check_Msg4_retransmission(module_id_t module_idP, int CC_idP,
 					    (cc->p_eNB == 1) ? 1 : 2,	// transmission mode
 					    1,	// num_bf_prb_per_subband
 					    1);	// num_bf_vector
+#ifdef UE_EXPANSION
+          dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].ue_priority = SCH_DL_MSG4;
+          dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].nb_rb = 4;
+          dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].UE_id = UE_id;
+          dlsch_ue_select[CC_idP].list[dlsch_ue_select[CC_idP].ue_num].rnti = ra->rnti;
+          dlsch_ue_select[CC_idP].ue_num++;
+#endif
 		} else
 		    LOG_D(MAC,
 			  "msg4 retransmission for rnti %x (round %d) fsf %d/%d CCE allocation failed!\n",
diff --git a/openair2/LAYER2/MAC/eNB_scheduler_dlsch.c b/openair2/LAYER2/MAC/eNB_scheduler_dlsch.c
index 292a018543a365f36e4607a3b117d41b33411d76..6fa7ce8ad8ef40faea15699afee9a091fb0f0690 100644
--- a/openair2/LAYER2/MAC/eNB_scheduler_dlsch.c
+++ b/openair2/LAYER2/MAC/eNB_scheduler_dlsch.c
@@ -511,10 +511,6 @@ schedule_ue_spec(module_id_t module_idP,
 	  }
     }
 
-#ifdef UE_EXPANSION
-    DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs];
-    memset(dlsch_ue_select, 0, sizeof(dlsch_ue_select));
-#endif
     //weight = get_ue_weight(module_idP,UE_id);
     aggregation = 2;
     for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
@@ -546,11 +542,7 @@ schedule_ue_spec(module_id_t module_idP,
                                 frameP,
                                 subframeP,
                                 N_RBG,
-                                mbsfn_flag
-#ifdef UE_EXPANSION
-                                , dlsch_ue_select
-#endif
-);
+                                mbsfn_flag);
   stop_meas(&eNB->schedule_dlsch_preprocessor);
   VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_DLSCH_PREPROCESSOR,VCD_FUNCTION_OUT);
 
@@ -564,11 +556,52 @@ schedule_ue_spec(module_id_t module_idP,
 
 #ifdef UE_EXPANSION
     for (i = 0; i < dlsch_ue_select[CC_id].ue_num; i++) {
+      if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG2){
+        continue;
+      }
+      if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG4){
+        continue;
+      }
       UE_id = dlsch_ue_select[CC_id].list[i].UE_id;
+      rnti = UE_RNTI(module_idP,UE_id);
+      if (rnti==NOT_A_RNTI) {
+        LOG_E(MAC,"Cannot find rnti for UE_id %d (num_UEs %d)\n",UE_id,UE_list->num_UEs);
+        continue;
+      }
+
+      eNB_UE_stats = &UE_list->eNB_UE_stats[CC_id][UE_id];
+      ue_sched_ctl = &UE_list->UE_sched_ctrl[UE_id];
+
+      switch(get_tmode(module_idP,CC_id,UE_id)){
+      case 1:
+      case 2:
+      case 7:
+        aggregation = get_aggregation(get_bw_index(module_idP,CC_id),
+                                      ue_sched_ctl->dl_cqi[CC_id],
+                                      format1);
+        break;
+      case 3:
+        aggregation = get_aggregation(get_bw_index(module_idP,CC_id),
+                                      ue_sched_ctl->dl_cqi[CC_id],
+                                      format2A);
+        break;
+      default:
+        LOG_W(MAC,"Unsupported transmission mode %d\n", get_tmode(module_idP,CC_id,UE_id));
+        aggregation = 2;
+        break;
+      }
 
+      if (cc[CC_id].tdd_Config != NULL) { //TDD
+        set_ue_dai (subframeP,
+                    UE_id,
+                    CC_id,
+  		    cc[CC_id].tdd_Config->subframeAssignment,
+                    UE_list);
+        // update UL DAI after DLSCH scheduling
+        set_ul_DAI(module_idP,UE_id,CC_id,frameP,subframeP);
+      }
 #else
     for (UE_id=UE_list->head; UE_id>=0; UE_id=UE_list->next[UE_id]) {
-#endif
 
       continue_flag=0; // reset the flag to allow allocation for the remaining UEs
       rnti = UE_RNTI(module_idP,UE_id);
@@ -632,6 +665,7 @@ schedule_ue_spec(module_id_t module_idP,
 				  CC_id, UE_id, subframeP, S_DL_NONE);
 		continue;
 	    }
+#endif
 #warning RK->CR This old API call has to be revisited for FAPI, or logic must be changed
 #if 0
 	    /* add "fake" DCI to have CCE_allocation_infeasible work properly for next allocations */
@@ -1415,7 +1449,9 @@ schedule_ue_spec(module_id_t module_idP,
 
 	    eNB->pdu_index[CC_id]++;
 	    program_dlsch_acknak(module_idP,CC_id,UE_id,frameP,subframeP,dl_config_pdu->dci_dl_pdu.dci_dl_pdu_rel8.cce_idx);
-
+#ifdef UE_EXPANSION
+        last_dlsch_ue_id[CC_id] = UE_id;
+#endif
 	  }
 	  else {
 	    LOG_W(MAC,"Frame %d, Subframe %d: Dropping DLSCH allocation for UE %d/%x, infeasible CCE allocations\n",
@@ -1434,11 +1470,7 @@ schedule_ue_spec(module_id_t module_idP,
   }  // CC_id loop
 
 
-#ifndef UE_EXPANSION
   fill_DLSCH_dci(module_idP,frameP,subframeP,mbsfn_flag);
-#else
-  fill_DLSCH_dci(module_idP,frameP,subframeP,mbsfn_flag,dlsch_ue_select);
-#endif
   stop_meas(&eNB->schedule_dlsch);
   VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME(VCD_SIGNAL_DUMPER_FUNCTIONS_SCHEDULE_DLSCH,VCD_FUNCTION_OUT);
 
@@ -1451,11 +1483,7 @@ fill_DLSCH_dci(
 	       module_id_t module_idP,
 	       frame_t frameP,
 	       sub_frame_t subframeP,
-	       int* mbsfn_flagP
-#ifdef UE_EXPANSION
-	       , DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs]
-#endif  
-	       )
+	       int* mbsfn_flagP)
 //------------------------------------------------------------------------------
 {
 
@@ -1495,7 +1523,13 @@ fill_DLSCH_dci(
     // UE specific DCIs
 #ifdef UE_EXPANSION
     for (j = 0; j < dlsch_ue_select[CC_id].ue_num; j++) {
-      UE_id = dlsch_ue_select[CC_id].list[j].UE_id;
+        if(dlsch_ue_select[CC_id].list[j].ue_priority == SCH_DL_MSG2){
+          continue;
+        }
+        if(dlsch_ue_select[CC_id].list[j].ue_priority == SCH_DL_MSG4){
+          continue;
+        }
+        UE_id = dlsch_ue_select[CC_id].list[j].UE_id;
 #else
     for (UE_id=UE_list->head; UE_id>=0; UE_id=UE_list->next[UE_id]) {
 #endif
diff --git a/openair2/LAYER2/MAC/eNB_scheduler_ulsch.c b/openair2/LAYER2/MAC/eNB_scheduler_ulsch.c
index c2817d3ac7a6dfa42694f8770978b52013c86d52..87b5f9b8375ef36f73d0433a57bb64cc3362507b 100644
--- a/openair2/LAYER2/MAC/eNB_scheduler_ulsch.c
+++ b/openair2/LAYER2/MAC/eNB_scheduler_ulsch.c
@@ -933,10 +933,12 @@ schedule_ulsch(module_id_t module_idP, frame_t frameP,
   int CC_id;
   eNB_MAC_INST *mac = RC.mac[module_idP];
   COMMON_channels_t *cc;
+  int sched_frame=frameP;
 
   start_meas(&mac->schedule_ulsch);
 
   int sched_subframe = (subframeP+4)%10;
+  if (sched_subframe < subframeP) sched_frame++;
 
   cc = &mac->common_channels[0];
   int tdd_sfa;
@@ -1051,7 +1053,7 @@ schedule_ulsch(module_id_t module_idP, frame_t frameP,
     }
 
     //PRACH
-    if (is_prach_subframe(frame_parms,frameP,subframeP)==1) {
+    if (is_prach_subframe(frame_parms,sched_frame,sched_subframe)==1) {
       ulsch_ue_select[CC_id].list[ulsch_ue_select[CC_id].ue_num].ue_priority = SCH_UL_PRACH;
       ulsch_ue_select[CC_id].list[ulsch_ue_select[CC_id].ue_num].start_rb = get_prach_prb_offset(
     		                                                                 frame_parms,
diff --git a/openair2/LAYER2/MAC/extern.h b/openair2/LAYER2/MAC/extern.h
index bde0e4d15764a4e61ce7bf27e3a0a0804bca4812..58c801e8dd785b0f72aaaf5ba05e0d415a9bd190 100644
--- a/openair2/LAYER2/MAC/extern.h
+++ b/openair2/LAYER2/MAC/extern.h
@@ -102,6 +102,7 @@ extern DCI2_5MHz_2A_TDD_t DLSCH_alloc_pdu2;
 extern DCI1E_5MHz_2A_M10PRB_TDD_t DLSCH_alloc_pdu1E;
 
 #ifdef UE_EXPANSION
+extern DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs];
 extern int last_dlsch_ue_id[MAX_NUM_CCs];
 extern int last_ulsch_ue_id[MAX_NUM_CCs];
 #endif
diff --git a/openair2/LAYER2/MAC/pre_processor.c b/openair2/LAYER2/MAC/pre_processor.c
index a9e4e516648d1548982851f45d4855cae797d889..6277c1c69fb39e81fd4211fcee22821f184c7d66 100644
--- a/openair2/LAYER2/MAC/pre_processor.c
+++ b/openair2/LAYER2/MAC/pre_processor.c
@@ -117,13 +117,6 @@ store_dlsch_buffer(module_id_t Mod_id, frame_t frameP,
 	UE_template->dl_buffer_total = 0;
 	UE_template->dl_pdus_total = 0;
 
-	for (i = 0; i < MAX_NUM_LCID; i++) {
-	    UE_template->dl_buffer_info[i] = 0;
-	    UE_template->dl_pdus_in_buffer[i] = 0;
-	    UE_template->dl_buffer_head_sdu_creation_time[i] = 0;
-	    UE_template->dl_buffer_head_sdu_remaining_size_to_send[i] = 0;
-	}
-
 	rnti = UE_RNTI(Mod_id, UE_id);
 
 	for (i = 0; i < MAX_NUM_LCID; i++) {	// loop over all the logical channels
@@ -551,6 +544,18 @@ void sort_UEs(module_id_t Mod_idP, int frameP, sub_frame_t subframeP)
 }
 
 #ifdef UE_EXPANSION
+int cc_id_end(uint8_t *cc_id_flag )
+{
+  int end_flag = 1;
+  for (int CC_id=0;CC_id<MAX_NUM_CCs;CC_id++) {
+    if (cc_id_flag[CC_id]==0) {
+      end_flag = 0;
+      break;
+    }
+  }
+  return end_flag;
+}
+
 void dlsch_scheduler_pre_ue_select(
     module_id_t     module_idP,
     frame_t         frameP,
@@ -559,43 +564,31 @@ void dlsch_scheduler_pre_ue_select(
     uint16_t        nb_rbs_required[MAX_NUM_CCs][NUMBER_OF_UE_MAX],
     DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs])
 {
-  uint8_t                        CC_id;
-  int                            UE_id;
-//  uint16_t                       nb_rb;
-  unsigned char                  round             = 0;
-  unsigned char                  harq_pid          = 0;
   eNB_MAC_INST                   *eNB      = RC.mac[module_idP];
   COMMON_channels_t              *cc       = eNB->common_channels;
   UE_list_t                      *UE_list  = &eNB->UE_list;
   UE_sched_ctrl                  *ue_sched_ctl;
+  uint8_t                        CC_id;
+  int                            UE_id;
+  unsigned char                  round             = 0;
+  unsigned char                  harq_pid          = 0;
   rnti_t                         rnti;
+  uint16_t                       i;
   unsigned char                  aggregation;
- // int                            i;
- // int                            N_RB_DL[MAX_NUM_CCs];
-  uint16_t                       used_dlsch_rbs_num[MAX_NUM_CCs] = {0};
-  uint16_t                       available_dlsch_rbs_num[MAX_NUM_CCs] = {0};
-  uint16_t                       dl_ue_max_num[MAX_NUM_CCs] = {0};
-  uint16_t                       dlsch_ue_max_num[MAX_NUM_CCs] = {0};
   int                            format_flag;
   nfapi_dl_config_request_body_t *DL_req;
   nfapi_dl_config_request_pdu_t  *dl_config_pdu;
+  uint16_t                       dlsch_ue_max_num[MAX_NUM_CCs] = {0};
   uint16_t                       saved_dlsch_dci[MAX_NUM_CCs] = {0};
-  int                            continue_flag = 0;
-  int                            old_last_dlsch_ue_id[MAX_NUM_CCs];
+  uint8_t                        end_flag[MAX_NUM_CCs] = {0};
+
   // Initialization
   for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
-    // get maximum UE number of DLSCH
-    dl_ue_max_num[CC_id] = NUMBER_OF_UE_MAX;
     dlsch_ue_max_num[CC_id] = (uint16_t)RC.rrc[module_idP]->configuration.ue_multiple_max[CC_id];
 
     // save origin DL PDU number
     DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
     saved_dlsch_dci[CC_id] = DL_req->number_pdu;
-    // set available_dlsch_rbs
-    available_dlsch_rbs_num[CC_id] = eNB->eNB_stats[CC_id].available_prbs;
-
-    // get last DLSCH ue_id
-    old_last_dlsch_ue_id[CC_id] = last_dlsch_ue_id[CC_id];
   }
 
   // Insert DLSCH(retransmission) UE into selected UE list
@@ -605,7 +598,7 @@ void dlsch_scheduler_pre_ue_select(
     }
 
     DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
-    for (UE_id = 0; UE_id < dl_ue_max_num[CC_id]  /*NUMBER_OF_UE_MAX*/; UE_id++) {
+    for (UE_id = 0; UE_id < NUMBER_OF_UE_MAX; UE_id++) {
       if (UE_list->active[UE_id] == FALSE) {
         continue;
       }
@@ -620,15 +613,14 @@ void dlsch_scheduler_pre_ue_select(
         continue;
       }
 
-      if (nb_rbs_required[CC_id][UE_id] == 0) {
-        continue;
-      }
-
       if (cc[CC_id].tdd_Config) harq_pid = ((frameP*10)+subframeP)%10;
       else harq_pid = ((frameP*10)+subframeP)&7;
 
       round = ue_sched_ctl->round[CC_id][harq_pid];
       if (round != 8) {  // retransmission
+        if(UE_list->UE_template[CC_id][UE_id].nb_rb[harq_pid] == 0){
+          continue;
+        }
         switch (get_tmode(module_idP, CC_id, UE_id)) {
           case 1:
           case 2:
@@ -667,12 +659,8 @@ void dlsch_scheduler_pre_ue_select(
           dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].rnti = rnti;
           dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].nb_rb = nb_rbs_required[CC_id][UE_id];
           dlsch_ue_select[CC_id].ue_num++;
-          used_dlsch_rbs_num[CC_id] += nb_rbs_required[CC_id][UE_id];
           if (dlsch_ue_select[CC_id].ue_num == dlsch_ue_max_num[CC_id]) {
-            break;
-          }
-          if (used_dlsch_rbs_num[CC_id] > available_dlsch_rbs_num[CC_id]) {
-            dlsch_ue_select[CC_id].ue_num--;  // roll back
+            end_flag[CC_id] = 1;
             break;
           }
         } else {
@@ -691,22 +679,29 @@ void dlsch_scheduler_pre_ue_select(
                             UE_id,
                             subframeP,
                             S_DL_NONE);
+          end_flag[CC_id] = 1;
+          break;
         }
       }
     }
   }
+  if(cc_id_end(end_flag) == 1){
+    for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
+      DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
+      DL_req->number_pdu = saved_dlsch_dci[CC_id];
+    }
+    return;
+  }
 
   // Insert DLSCH(first transmission) UE into selected UE list (UE_id > last_dlsch_ue_id[CC_id])
   for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
-    if (mbsfn_flag[CC_id]>0) {
-      continue;
-    }
+      if (mbsfn_flag[CC_id]>0) {
+        continue;
+      }
 
-    if (dlsch_ue_select[CC_id].ue_num < dlsch_ue_max_num[CC_id] &&
-        used_dlsch_rbs_num[CC_id] < available_dlsch_rbs_num[CC_id]) {
       DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
-      for (UE_id = (old_last_dlsch_ue_id[CC_id]+1); UE_id < dl_ue_max_num[CC_id]; UE_id++) {
-        if (dlsch_ue_select[CC_id].ue_num >= dlsch_ue_max_num[CC_id]) {
+      for (UE_id = (last_dlsch_ue_id[CC_id]+1); UE_id <NUMBER_OF_UE_MAX; UE_id++) {
+        if(end_flag[CC_id] == 1){
           break;
         }
 
@@ -723,16 +718,22 @@ void dlsch_scheduler_pre_ue_select(
           continue;
         }
 
-        continue_flag = 0;
-        if (nb_rbs_required[CC_id][UE_id] == 0) {
-          continue_flag = 1;
-        } else {
-          if (cc[CC_id].tdd_Config) harq_pid = ((frameP*10)+subframeP)%10;
-          else harq_pid = ((frameP*10)+subframeP)&7;
+       for(i = 0;i<dlsch_ue_select[CC_id].ue_num;i++){
+          if(dlsch_ue_select[CC_id].list[i].UE_id == UE_id){
+           break;
+          }
+        }
+        if(i < dlsch_ue_select[CC_id].ue_num)
+          continue;
 
-          round = ue_sched_ctl->round[CC_id][harq_pid];
-          if (round == 8) {
-            ue_sched_ctl = &UE_list->UE_sched_ctrl[UE_id];
+        if (cc[CC_id].tdd_Config) harq_pid = ((frameP*10)+subframeP)%10;
+        else harq_pid = ((frameP*10)+subframeP)&7;
+
+        round = ue_sched_ctl->round[CC_id][harq_pid];
+        if (round == 8) {
+            if (nb_rbs_required[CC_id][UE_id] == 0) {
+              continue;
+            }
             switch (get_tmode(module_idP, CC_id, UE_id)) {
               case 1:
               case 2:
@@ -749,6 +750,7 @@ void dlsch_scheduler_pre_ue_select(
               default:
                 LOG_W(MAC,"Unsupported transmission mode %d\n", get_tmode(module_idP,CC_id,UE_id));
                 aggregation = 2;
+                break;
             }
             format_flag = 1;
             if (!CCE_allocation_infeasible(module_idP,
@@ -770,45 +772,49 @@ void dlsch_scheduler_pre_ue_select(
               dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].UE_id = UE_id;
               dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].rnti = rnti;
               dlsch_ue_select[CC_id].ue_num++;
-              last_dlsch_ue_id[CC_id] = UE_id;
-            } else {
-              continue_flag = 1;
-            }
-          }
-        }
-
-        if (continue_flag == 1) {
-          if (cc[CC_id].tdd_Config != NULL) { //TDD
-            set_ue_dai (subframeP,
-                         UE_id,
-                         CC_id,
-            cc[CC_id].tdd_Config->subframeAssignment,
-                         UE_list);
-            // update UL DAI after DLSCH scheduling
-            set_ul_DAI(module_idP,UE_id,CC_id,frameP,subframeP);
-          }
 
-          add_ue_dlsch_info(module_idP,
+          if (dlsch_ue_select[CC_id].ue_num == dlsch_ue_max_num[CC_id]) {
+                  end_flag[CC_id] = 1;
+                  break;
+              }
+          }else {
+            if (cc[CC_id].tdd_Config != NULL) { //TDD
+              set_ue_dai (subframeP,
+                           UE_id,
+                           CC_id,
+              cc[CC_id].tdd_Config->subframeAssignment,
+                           UE_list);
+              // update UL DAI after DLSCH scheduling
+              set_ul_DAI(module_idP,UE_id,CC_id,frameP,subframeP);
+            }
+           add_ue_dlsch_info(module_idP,
                             CC_id,
                             UE_id,
                             subframeP,
                             S_DL_NONE);
-        }
+            end_flag[CC_id] = 1;
+            break;
+          }
       }
     }
   }
+  if(cc_id_end(end_flag) == 1){
+    for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
+      DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
+      DL_req->number_pdu = saved_dlsch_dci[CC_id];
+    }
+    return;
+  }
 
   // Insert DLSCH(first transmission) UE into selected UE list (UE_id <= last_dlsch_ue_id[CC_id])
   for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
-    if (mbsfn_flag[CC_id]>0) {
-      continue;
-    }
+      if (mbsfn_flag[CC_id]>0) {
+        continue;
+      }
 
-    if (dlsch_ue_select[CC_id].ue_num < dlsch_ue_max_num[CC_id] &&
-        used_dlsch_rbs_num[CC_id] < available_dlsch_rbs_num[CC_id]) {
       DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
-      for (UE_id = 0; UE_id <= old_last_dlsch_ue_id[CC_id]; UE_id++) {
-        if (dlsch_ue_select[CC_id].ue_num >= dlsch_ue_max_num[CC_id]) {
+      for (UE_id = 0; UE_id <= last_dlsch_ue_id[CC_id]; UE_id++) {
+        if(end_flag[CC_id] == 1){
           break;
         }
 
@@ -825,16 +831,22 @@ void dlsch_scheduler_pre_ue_select(
           continue;
         }
 
-        continue_flag = 0;
-        if (nb_rbs_required[CC_id][UE_id] == 0) {
-          continue_flag = 1;
-        } else {
-          if (cc[CC_id].tdd_Config) harq_pid = ((frameP*10)+subframeP)%10;
-          else harq_pid = ((frameP*10)+subframeP)&7;
+        for(i = 0;i<dlsch_ue_select[CC_id].ue_num;i++){
+          if(dlsch_ue_select[CC_id].list[i].UE_id == UE_id){
+           break;
+          }
+        }
+        if(i < dlsch_ue_select[CC_id].ue_num)
+          continue;
 
-          round = ue_sched_ctl->round[CC_id][harq_pid];
-          if (round == 8) {
-            ue_sched_ctl = &UE_list->UE_sched_ctrl[UE_id];
+        if (cc[CC_id].tdd_Config) harq_pid = ((frameP*10)+subframeP)%10;
+        else harq_pid = ((frameP*10)+subframeP)&7;
+
+        round = ue_sched_ctl->round[CC_id][harq_pid];
+        if (round == 8) {
+            if (nb_rbs_required[CC_id][UE_id] == 0) {
+                continue;
+             }
             switch (get_tmode(module_idP, CC_id, UE_id)) {
               case 1:
               case 2:
@@ -851,6 +863,7 @@ void dlsch_scheduler_pre_ue_select(
               default:
                 LOG_W(MAC,"Unsupported transmission mode %d\n", get_tmode(module_idP,CC_id,UE_id));
                 aggregation = 2;
+                break;
             }
             format_flag = 1;
             if (!CCE_allocation_infeasible(module_idP,
@@ -872,30 +885,29 @@ void dlsch_scheduler_pre_ue_select(
               dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].UE_id = UE_id;
               dlsch_ue_select[CC_id].list[dlsch_ue_select[CC_id].ue_num].rnti = rnti;
               dlsch_ue_select[CC_id].ue_num++;
-              last_dlsch_ue_id[CC_id] = UE_id;
-            } else {
-              continue_flag = 1;
-            }
-          }
-        }
-
-        if (continue_flag == 1) {
-          if (cc[CC_id].tdd_Config != NULL) { //TDD
-            set_ue_dai (subframeP,
-                         UE_id,
-                         CC_id,
-            cc[CC_id].tdd_Config->subframeAssignment,
-                         UE_list);
-            // update UL DAI after DLSCH scheduling
-            set_ul_DAI(module_idP,UE_id,CC_id,frameP,subframeP);
-          }
 
+              if (dlsch_ue_select[CC_id].ue_num == dlsch_ue_max_num[CC_id]) {
+                end_flag[CC_id] = 1;
+                break;
+              }
+            } else {
+              if (cc[CC_id].tdd_Config != NULL) { //TDD
+                set_ue_dai (subframeP,
+                            UE_id,
+                            CC_id,
+                            cc[CC_id].tdd_Config->subframeAssignment,
+                            UE_list);
+              // update UL DAI after DLSCH scheduling
+                set_ul_DAI(module_idP,UE_id,CC_id,frameP,subframeP);
+              }
           add_ue_dlsch_info(module_idP,
                             CC_id,
                             UE_id,
                             subframeP,
                             S_DL_NONE);
-        }
+              end_flag[CC_id] = 1;
+              break;
+            }
       }
     }
   }
@@ -904,6 +916,7 @@ void dlsch_scheduler_pre_ue_select(
     DL_req          = &eNB->DL_req[CC_id].dl_config_request_body;
     DL_req->number_pdu = saved_dlsch_dci[CC_id];
   }
+  return;
 }
 
 
@@ -916,11 +929,7 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
                                     frame_t       frameP,
                                     sub_frame_t   subframeP,
                                     int           N_RBG[MAX_NUM_CCs],
-                                    int           *mbsfn_flag
-#ifdef UE_EXPANSION
-                                    , DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs]
-#endif
-)
+                                    int           *mbsfn_flag)
 {
 
 #ifndef UE_EXPANSION
@@ -931,6 +940,8 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
 //  int rrc_status           = RRC_IDLE;
 #else
   unsigned char rballoc_sub[MAX_NUM_CCs][N_RBG_MAX],harq_pid=0,Round=0;
+  uint16_t                temp_total_rbs_count;
+  unsigned char           temp_total_ue_count;
 #endif 
   unsigned char MIMO_mode_indicator[MAX_NUM_CCs][N_RBG_MAX];
   int                     UE_id, i; 
@@ -962,11 +973,6 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
     UE_sched_ctrl *ue_sched_ctl1, *ue_sched_ctl2;
 #endif
 
-#ifdef UE_EXPANSION
-  uint16_t                temp_total_rbs_count;
-  unsigned char           temp_total_ue_count;
-#endif
-
   for (CC_id=0; CC_id<MAX_NUM_CCs; CC_id++) {
 
 	if (mbsfn_flag[CC_id] > 0)	// If this CC is allocated for MBSFN skip it here
@@ -1031,14 +1037,23 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
     temp_total_ue_count = dlsch_ue_select[CC_id].ue_num;
 
     for (i = 0; i < dlsch_ue_select[CC_id].ue_num; i++) {
+      if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG2){
+          temp_total_ue_count--;
+          continue;
+      }
+      if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG4){
+          temp_total_ue_count--;
+          continue;
+      }
       UE_id = dlsch_ue_select[CC_id].list[i].UE_id;
       nb_rbs_required[CC_id][UE_id] = dlsch_ue_select[CC_id].list[i].nb_rb;
 
-      if( (min_rb_unit[CC_id] * temp_total_ue_count) <= temp_total_rbs_count ) {
-        //average_rbs_per_user[CC_id] = ((uint16_t)round((double)temp_total_rbs_count/(double)(temp_total_ue_count*min_rb_unit[CC_id])))*min_rb_unit[CC_id];
-        average_rbs_per_user[CC_id] = (uint16_t)round((double)temp_total_rbs_count/(double)temp_total_ue_count);
-      } else {
-        average_rbs_per_user[CC_id] = min_rb_unit[CC_id]; // consider the total number of use that can be scheduled UE
+      average_rbs_per_user[CC_id] = (uint16_t)round((double)temp_total_rbs_count/(double)temp_total_ue_count);
+      if( average_rbs_per_user[CC_id] < min_rb_unit[CC_id] ){
+        temp_total_ue_count--;
+        dlsch_ue_select[CC_id].ue_num--;
+        i--;
+        continue;
       }
 
       rnti = dlsch_ue_select[CC_id].list[i].rnti;
@@ -1069,17 +1084,15 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
                                               nb_rbs_required_remaining,
                                               rballoc_sub,
                                               MIMO_mode_indicator);
+      temp_total_rbs_count -= ue_sched_ctl->pre_nb_available_rbs[CC_id];
+      temp_total_ue_count--;
+
       if (ue_sched_ctl->pre_nb_available_rbs[CC_id] == 0) {
-        last_dlsch_ue_id[CC_id] = dlsch_ue_select[CC_id].list[i-1].UE_id;
         dlsch_ue_select[CC_id].ue_num = i;
         break;
       }
 
-      temp_total_rbs_count -= ue_sched_ctl->pre_nb_available_rbs[CC_id];
-      temp_total_ue_count--;
-
       if (temp_total_rbs_count == 0) {
-        last_dlsch_ue_id[CC_id] = UE_id;
         dlsch_ue_select[CC_id].ue_num = i+1;
         break;
       }
@@ -1466,6 +1479,12 @@ void dlsch_scheduler_pre_processor (module_id_t   Mod_id,
 #ifdef UE_EXPANSION
   for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
       for (i = 0; i < dlsch_ue_select[CC_id].ue_num; i++) {
+        if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG2){
+            continue;
+        }
+        if(dlsch_ue_select[CC_id].list[i].ue_priority == SCH_DL_MSG4){
+            continue;
+        }
         UE_id = dlsch_ue_select[CC_id].list[i].UE_id;
         ue_sched_ctl = &RC.mac[Mod_id]->UE_list.UE_sched_ctrl[UE_id];
 #else
@@ -2299,20 +2318,7 @@ void sort_ue_ul(module_id_t module_idP, int frameP, sub_frame_t subframeP)
     }
 #endif
 }
-
 #ifdef UE_EXPANSION
-int ulsch_cc_id_end(uint8_t *cc_id_flag )
-{
-  int end_flag = 1;
-  for (int CC_id=0;CC_id<MAX_NUM_CCs;CC_id++) {
-    if (cc_id_flag[CC_id]==0) {
-      end_flag = 0;
-      break;
-    }
-  }
-  return end_flag;
-}
-
 void ulsch_scheduler_pre_ue_select(
     module_id_t       module_idP,
     frame_t           frameP,
@@ -2323,6 +2329,7 @@ void ulsch_scheduler_pre_ue_select(
   COMMON_channels_t *cc;
   int CC_id,UE_id;
   int ret;
+  uint16_t i;
   uint8_t ue_first_num[MAX_NUM_CCs];
   uint8_t first_ue_total[MAX_NUM_CCs][20];
   uint8_t first_ue_id[MAX_NUM_CCs][20];
@@ -2373,7 +2380,7 @@ void ulsch_scheduler_pre_ue_select(
       if ( (ulsch_ue_select[CC_id].ue_num >= ulsch_ue_max_num[CC_id]) || (cc_id_flag[CC_id] == 1) ) {
         cc_id_flag[CC_id] = 1;
         HI_DCI0_req->number_of_dci = saved_ulsch_dci[CC_id];
-        ret = ulsch_cc_id_end(cc_id_flag);
+        ret = cc_id_end(cc_id_flag);
         if ( ret == 0 ) {
             continue;
         }
@@ -2479,7 +2486,7 @@ void ulsch_scheduler_pre_ue_select(
     if ( (ulsch_ue_select[CC_id].ue_num >= ulsch_ue_max_num[CC_id]) || (cc_id_flag[CC_id] == 1) ) {
         cc_id_flag[CC_id] = 1;
         HI_DCI0_req->number_of_dci = saved_ulsch_dci[CC_id];
-        ret = ulsch_cc_id_end(cc_id_flag);
+        ret = cc_id_end(cc_id_flag);
         if ( ret == 0 ) {
             continue;
         }
@@ -2491,6 +2498,14 @@ void ulsch_scheduler_pre_ue_select(
     if (UE_id > last_ulsch_ue_id[CC_id])
         continue;
 
+    for(i = 0;i<ulsch_ue_select[CC_id].ue_num;i++){
+      if(ulsch_ue_select[CC_id].list[i].UE_id == UE_id){
+       break;
+      }
+    }
+    if(i < ulsch_ue_select[CC_id].ue_num)
+      continue;
+
     HI_DCI0_req   = &eNB->HI_DCI0_req[CC_id].hi_dci0_request_body;
     //SR BSR
     if ( (UE_list->UE_template[CC_id][UE_id].ul_total_buffer > 0) || (UE_list->UE_template[CC_id][UE_id].ul_SR > 0) ) {
diff --git a/openair2/LAYER2/MAC/proto.h b/openair2/LAYER2/MAC/proto.h
index f47d17949ebd4eef3d133d5564c20f1cb99e9bdc..a2c489035681eca0f071c2e121250ba8141f320c 100644
--- a/openair2/LAYER2/MAC/proto.h
+++ b/openair2/LAYER2/MAC/proto.h
@@ -118,11 +118,7 @@ void schedule_ulsch_rnti(module_id_t module_idP, frame_t frameP, sub_frame_t sub
 @param subframe Index of subframe
 @param mbsfn_flag Indicates that this subframe is for MCH/MCCH
 */
-#ifndef UE_EXPANSION
 void fill_DLSCH_dci(module_id_t module_idP,frame_t frameP,sub_frame_t subframe,int *mbsfn_flag);
-#else
-void fill_DLSCH_dci(module_id_t module_idP,frame_t frameP,sub_frame_t subframe,int *mbsfn_flag, DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs]);
-#endif
 
 /** \brief UE specific DLSCH scheduling. Retrieves next ue to be schduled from round-robin scheduler and gets the appropriate harq_pid for the subframe from PHY. If the process is active and requires a retransmission, it schedules the retransmission with the same PRB count and MCS as the first transmission. Otherwise it consults RLC for DCCH/DTCH SDUs (status with maximum number of available PRBS), builds the MAC header (timing advance sent by default) and copies
 @param Mod_id Instance ID of eNB
@@ -216,11 +212,7 @@ void dlsch_scheduler_pre_processor (module_id_t module_idP,
                                     frame_t frameP,
                                     sub_frame_t subframe,
                                     int N_RBG[MAX_NUM_CCs],
-                                    int *mbsfn_flag
-#ifdef UE_EXPANSION
-                                    , DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs]
-#endif
-);
+                                    int *mbsfn_flag);
 
 
 void dlsch_scheduler_pre_processor_allocate (module_id_t   Mod_id,
diff --git a/openair2/LAYER2/MAC/rar_tools.c b/openair2/LAYER2/MAC/rar_tools.c
index 8d9e5bb3e14d88664d06a3f621f25cb07c033188..2fa979b8b769c803e9d4c5ba876b302f62108d8d 100644
--- a/openair2/LAYER2/MAC/rar_tools.c
+++ b/openair2/LAYER2/MAC/rar_tools.c
@@ -71,7 +71,7 @@ fill_rar(const module_id_t module_idP,
     ra->timing_offset /= 16;	//T_A = N_TA/16, where N_TA should be on a 30.72Msps
     rar[0] = (uint8_t) (ra->timing_offset >> (2 + 4));	// 7 MSBs of timing advance + divide by 4
     rar[1] = (uint8_t) (ra->timing_offset << (4 - 2)) & 0xf0;	// 4 LSBs of timing advance + divide by 4
-    ra->msg3_first_rb = 6;
+    ra->msg3_first_rb = 1;
     ra->msg3_nb_rb = 1;
     uint16_t rballoc = mac_computeRIV(N_RB_UL, ra->msg3_first_rb, ra->msg3_nb_rb);	// first PRB only for UL Grant
     rar[1] |= (rballoc >> 7) & 7;	// Hopping = 0 (bit 3), 3 MSBs of rballoc
diff --git a/openair2/LAYER2/MAC/vars.h b/openair2/LAYER2/MAC/vars.h
index 7fcc5147017a3d2b9f0d93200854e95eb6318030..9882bf75457a6a832554a7745e78d41d4e45ce73 100644
--- a/openair2/LAYER2/MAC/vars.h
+++ b/openair2/LAYER2/MAC/vars.h
@@ -146,6 +146,7 @@ DCI2_5MHz_2A_TDD_t DLSCH_alloc_pdu2;
 DCI1E_5MHz_2A_M10PRB_TDD_t DLSCH_alloc_pdu1E;
 
 #ifdef UE_EXPANSION
+DLSCH_UE_SELECT dlsch_ue_select[MAX_NUM_CCs];
 int last_dlsch_ue_id[MAX_NUM_CCs] = {-1};
 int last_ulsch_ue_id[MAX_NUM_CCs] = {-1};
 #endif
diff --git a/openair2/RRC/LITE/defs.h b/openair2/RRC/LITE/defs.h
index 9d7f7fdf2d169d4104de8c5ff5662d51aa2bfc85..5ab41db75f78150d0b4d80662d7829468450f344 100644
--- a/openair2/RRC/LITE/defs.h
+++ b/openair2/RRC/LITE/defs.h
@@ -438,6 +438,14 @@ typedef struct eNB_RRC_UE_s {
   uint32_t                           ul_failure_timer;
   uint32_t                           ue_release_timer;
   uint32_t                           ue_release_timer_thres;
+#ifdef UE_EXPANSION
+  uint32_t                           ue_release_timer_s1;
+  uint32_t                           ue_release_timer_thres_s1;
+  uint32_t                           ue_release_timer_rrc;
+  uint32_t                           ue_release_timer_thres_rrc;
+  uint32_t                           ue_reestablishment_timer;
+  uint32_t                           ue_reestablishment_timer_thres;
+#endif
 } eNB_RRC_UE_t;
 
 typedef uid_t ue_uid_t;
diff --git a/openair2/RRC/LITE/rrc_common.c b/openair2/RRC/LITE/rrc_common.c
index 0e836f56778df02cca1a0b0bb47792cbe00adec7..47f67a137380d505e99d0e2ed2020aaec7a42c94 100644
--- a/openair2/RRC/LITE/rrc_common.c
+++ b/openair2/RRC/LITE/rrc_common.c
@@ -333,26 +333,62 @@ rrc_rx_tx(
     RB_FOREACH(ue_context_p, rrc_ue_tree_s, &(RC.rrc[ctxt_pP->module_id]->rrc_ue_head)) {
       if ((ctxt_pP->frame == 0) && (ctxt_pP->subframe==0)) {
 	if (ue_context_p->ue_context.Initialue_identity_s_TMSI.presence == TRUE) {
-	  LOG_I(RRC,"UE rnti %x:S-TMSI %x failure timer %d/20000\n",
+	  LOG_I(RRC,"UE rnti %x:S-TMSI %x failure timer %d/8\n",
 		ue_context_p->ue_context.rnti,
 		ue_context_p->ue_context.Initialue_identity_s_TMSI.m_tmsi,
 		ue_context_p->ue_context.ul_failure_timer);
 	}
 	else {
-	  LOG_I(RRC,"UE rnti %x failure timer %d/20000\n",
+	  LOG_I(RRC,"UE rnti %x failure timer %d/8\n",
 		ue_context_p->ue_context.rnti,
 		ue_context_p->ue_context.ul_failure_timer);
 	}
       }
       if (ue_context_p->ue_context.ul_failure_timer>0) {
 	ue_context_p->ue_context.ul_failure_timer++;
-	if (ue_context_p->ue_context.ul_failure_timer >= 20000) {
+	if (ue_context_p->ue_context.ul_failure_timer >= 8) {
 	  // remove UE after 20 seconds after MAC has indicated UL failure
 	  LOG_I(RRC,"Removing UE %x instance\n",ue_context_p->ue_context.rnti);
 	  ue_to_be_removed = ue_context_p;
 	  break;
 	}
       }
+
+#ifdef UE_EXPANSION
+      if (ue_context_p->ue_context.ue_release_timer_s1>0) {
+        ue_context_p->ue_context.ue_release_timer_s1++;
+        if (ue_context_p->ue_context.ue_release_timer_s1 >=
+            ue_context_p->ue_context.ue_release_timer_thres_s1) {
+          LOG_I(RRC,"Removing UE %x instance Because of UE_CONTEXT_RELEASE_COMMAND not received after %d ms from sending request\n",
+        		  ue_context_p->ue_context.rnti, ue_context_p->ue_context.ue_release_timer_thres_s1);
+          ue_to_be_removed = ue_context_p;
+          break;
+        }
+      }
+
+      if (ue_context_p->ue_context.ue_release_timer_rrc>0) {
+        ue_context_p->ue_context.ue_release_timer_rrc++;
+        if (ue_context_p->ue_context.ue_release_timer_rrc >=
+          ue_context_p->ue_context.ue_release_timer_thres_rrc) {
+          LOG_I(RRC,"Removing UE %x instance After UE_CONTEXT_RELEASE_Complete\n", ue_context_p->ue_context.rnti);
+          ue_to_be_removed = ue_context_p;
+          break;
+        }
+      }
+
+      if (ue_context_p->ue_context.ue_reestablishment_timer>0) {
+        ue_context_p->ue_context.ue_reestablishment_timer++;
+        if (ue_context_p->ue_context.ue_reestablishment_timer >=
+            ue_context_p->ue_context.ue_reestablishment_timer_thres) {
+          LOG_I(RRC,"UE %d reestablishment_timer max\n",ue_context_p->ue_context.rnti);
+          ue_context_p->ue_context.ul_failure_timer = 20000;
+          ue_to_be_removed = ue_context_p;
+          ue_context_p->ue_context.ue_reestablishment_timer = 0;
+          break;
+        }
+      }
+#endif
+
       if (ue_context_p->ue_context.ue_release_timer>0) {
 	ue_context_p->ue_context.ue_release_timer++;
 	if (ue_context_p->ue_context.ue_release_timer >= 
@@ -363,9 +399,17 @@ rrc_rx_tx(
 	}
       }
     }
-    if (ue_to_be_removed)
+    if (ue_to_be_removed) {
+#ifdef UE_EXPANSION
+      if(ue_to_be_removed->ue_context.ul_failure_timer >= 8) {
+          ue_to_be_removed->ue_context.ue_release_timer_s1 = 1;
+          ue_to_be_removed->ue_context.ue_release_timer_thres_s1 = 100;
+          ue_to_be_removed->ue_context.ue_release_timer = 0;
+          ue_to_be_removed->ue_context.ue_reestablishment_timer = 0;
+      }
+#endif
       rrc_eNB_free_UE(ctxt_pP->module_id,ue_to_be_removed);
-
+    }
 #ifdef RRC_LOCALIZATION
 
     /* for the localization, only primary CC_id might be relevant*/
diff --git a/openair2/RRC/LITE/rrc_eNB.c b/openair2/RRC/LITE/rrc_eNB.c
index c8b42d41b354160400dc7317cf58821ef4a95ef5..ae7f823528384f182e2a41470c706e928967c3a1 100644
--- a/openair2/RRC/LITE/rrc_eNB.c
+++ b/openair2/RRC/LITE/rrc_eNB.c
@@ -571,6 +571,7 @@ rrc_eNB_get_next_free_ue_context(
 					ctxt_pP->rnti);
 
   if (ue_context_p == NULL) {
+#ifndef UE_EXPANSION
     RB_FOREACH(ue_context_p, rrc_ue_tree_s, &(RC.rrc[ctxt_pP->module_id]->rrc_ue_head)) {
       if (ue_context_p->ue_context.random_ue_identity == ue_identityP) {
         LOG_D(RRC,
@@ -581,6 +582,7 @@ rrc_eNB_get_next_free_ue_context(
         return NULL;
       }
     }
+#endif
     ue_context_p = rrc_eNB_allocate_new_UE_context(RC.rrc[ctxt_pP->module_id]);
 
     if (ue_context_p == NULL) {
@@ -778,7 +780,11 @@ rrc_eNB_free_UE(const module_id_t enb_mod_idP,const struct rrc_eNB_ue_context_s*
   (void)ue_module_id;
 #endif
   rnti_t rnti = ue_context_pP->ue_context.rnti;
-
+  int i, j , CC_id, pdu_number;
+  LTE_eNB_ULSCH_t *ulsch = NULL;
+  nfapi_ul_config_request_body_t *ul_req_tmp = NULL;
+  PHY_VARS_eNB *eNB_PHY = NULL;
+  eNB_MAC_INST *eNB_MAC = RC.mac[enb_mod_idP];
 
   AssertFatal(enb_mod_idP < NB_eNB_INST, "eNB inst invalid (%d/%d) for UE %x!", enb_mod_idP, NB_eNB_INST, rnti);
   /*  ue_context_p = rrc_eNB_get_ue_context(
@@ -807,7 +813,33 @@ rrc_eNB_free_UE(const module_id_t enb_mod_idP,const struct rrc_eNB_ue_context_s*
     oai_emulation.info.eNB_ue_module_id_to_rnti[enb_mod_idP][ue_module_id] = NOT_A_RNTI;
 #endif
 #endif
+    for (CC_id = 0; CC_id < MAX_NUM_CCs; CC_id++) {
+      eNB_PHY = RC.eNB[enb_mod_idP][CC_id];
+      for (i=0; i<NUMBER_OF_UE_MAX; i++) {
+        ulsch = eNB_PHY->ulsch[i];
+        if((ulsch != NULL) && (ulsch->rnti == rnti)){
+          LOG_I(RRC, "clean_eNb_ulsch UE %x \n", rnti);
+          clean_eNb_ulsch(ulsch);
+          break;
+        }
+      }
 
+      for(j = 0; j < 10; j++){
+        ul_req_tmp = &eNB_MAC->UL_req_tmp[CC_id][j].ul_config_request_body;
+        if(ul_req_tmp){
+          pdu_number = ul_req_tmp->number_of_pdus;
+          for(int pdu_index = pdu_number-1; pdu_index >= 0; pdu_index--){
+            if(ul_req_tmp->ul_config_pdu_list[pdu_index].ulsch_pdu.ulsch_pdu_rel8.rnti == rnti){
+              LOG_I(RRC, "remove UE %x from ul_config_pdu_list %d/%d\n", rnti, pdu_index, pdu_number);
+              if(pdu_index < pdu_number -1){
+                memcpy(&ul_req_tmp->ul_config_pdu_list[pdu_index], &ul_req_tmp->ul_config_pdu_list[pdu_index+1], (pdu_number-1-pdu_index) * sizeof(nfapi_ul_config_request_pdu_t));
+              }
+              ul_req_tmp->number_of_pdus--;
+            }
+          }
+        }
+      }
+    }
     rrc_mac_remove_ue(enb_mod_idP,rnti);
     rrc_rlc_remove_ue(&ctxt);
     pdcp_remove_UE(&ctxt);
@@ -1085,10 +1117,20 @@ rrc_eNB_generate_RRCConnectionRelease(
   memset(buffer, 0, RRC_BUF_SIZE);
 
   size = do_RRCConnectionRelease(ctxt_pP->module_id, buffer,rrc_eNB_get_next_transaction_identifier(ctxt_pP->module_id));
+#ifdef UE_EXPANSION
+  // set release timer
+  ue_context_pP->ue_context.ue_release_timer_rrc = 1;
+  // remove UE after 10 frames after RRCConnectionRelease is triggered
+  ue_context_pP->ue_context.ue_release_timer_thres_rrc = 100;
+  ue_context_pP->ue_context.ue_reestablishment_timer = 0;
+  ue_context_pP->ue_context.ue_release_timer = 0;
+  ue_context_pP->ue_context.ue_release_timer_s1 = 0;
+#else
   // set release timer
   ue_context_pP->ue_context.ue_release_timer=1;
   // remove UE after 10 frames after RRCConnectionRelease is triggered
   ue_context_pP->ue_context.ue_release_timer_thres=100;
+#endif
   LOG_I(RRC,
         PROTOCOL_RRC_CTXT_UE_FMT" Logical Channel DL-DCCH, Generate RRCConnectionRelease (bytes %d)\n",
         PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP),
@@ -3805,10 +3847,17 @@ rrc_eNB_generate_RRCConnectionSetup(
         PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP),
         RC.rrc[ctxt_pP->module_id]->carrier[CC_id].Srb0.Tx_buffer.payload_size);
 
+#ifdef UE_EXPANSION
+   // activate release timer, if RRCSetupComplete not received after 100 frames, remove UE
+   ue_context_pP->ue_context.ue_release_timer=1;
+   // remove UE after 10 frames after RRCConnectionRelease is triggered
+   ue_context_pP->ue_context.ue_release_timer_thres=1000;
+#else
   // activate release timer, if RRCSetupComplete not received after 10 frames, remove UE
   ue_context_pP->ue_context.ue_release_timer=1;
   // remove UE after 10 frames after RRCConnectionRelease is triggered
   ue_context_pP->ue_context.ue_release_timer_thres=100;
+#endif
 }
 
 
@@ -4156,6 +4205,7 @@ rrc_eNB_decode_ccch(
             /* if there is already a registered UE (with another RNTI) with this random_value,
              * the current one must be removed from MAC/PHY (zombie UE)
              */
+#ifndef UE_EXPANSION
             if ((ue_context_p = rrc_eNB_ue_context_random_exist(ctxt_pP, random_value))) {
               LOG_W(RRC, "new UE rnti %x (coming with random value) is already there as UE %x, removing %x from MAC/PHY\n",
                     ctxt_pP->rnti, ue_context_p->ue_context.rnti, ctxt_pP->rnti);
@@ -4165,6 +4215,14 @@ rrc_eNB_decode_ccch(
             } else {
               ue_context_p = rrc_eNB_get_next_free_ue_context(ctxt_pP, random_value);
             }
+#else
+            if ((ue_context_p = rrc_eNB_ue_context_random_exist(ctxt_pP, random_value))) {
+              LOG_W(RRC, "new UE rnti %x (coming with random value) is already there as UE %x, removing %x from MAC/PHY\n",
+                    ctxt_pP->rnti, ue_context_p->ue_context.rnti, ue_context_p->ue_context.rnti);
+              ue_context_p->ue_context.ul_failure_timer = 20000;
+            }
+            ue_context_p = rrc_eNB_get_next_free_ue_context(ctxt_pP, random_value);
+#endif
           } else if (InitialUE_Identity_PR_s_TMSI == rrcConnectionRequest->ue_Identity.present) {
             /* Save s-TMSI */
             S_TMSI_t   s_TMSI = rrcConnectionRequest->ue_Identity.choice.s_TMSI;
@@ -4185,6 +4243,11 @@ rrc_eNB_decode_ccch(
               /* reset timers */
               ue_context_p->ue_context.ul_failure_timer = 0;
               ue_context_p->ue_context.ue_release_timer = 0;
+#ifdef UE_EXPANSION
+              ue_context_p->ue_context.ue_reestablishment_timer = 0;
+              ue_context_p->ue_context.ue_release_timer_s1 = 0;
+              ue_context_p->ue_context.ue_release_timer_rrc = 0;
+#endif
             } else {
 	      LOG_I(RRC," S-TMSI doesn't exist, setting Initialue_identity_s_TMSI.m_tmsi to %p => %x\n",ue_context_p,m_tmsi);
               ue_context_p = rrc_eNB_get_next_free_ue_context(ctxt_pP, NOT_A_RANDOM_UE_IDENTITY);
@@ -4451,6 +4514,11 @@ rrc_eNB_decode_dcch(
       break;
 
     case UL_DCCH_MessageType__c1_PR_measurementReport:
+      // to avoid segmentation fault
+      if(!ue_context_p){
+        LOG_I(RRC, "Processing measurementReport UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+        break;
+      }
       LOG_D(RRC,
             PROTOCOL_RRC_CTXT_UE_FMT" RLC RB %02d --- RLC_DATA_IND "
             "%d bytes (measurementReport) ---> RRC_eNB\n",
@@ -4465,6 +4533,11 @@ rrc_eNB_decode_dcch(
       break;
 
     case UL_DCCH_MessageType__c1_PR_rrcConnectionReconfigurationComplete:
+      // to avoid segmentation fault
+      if(!ue_context_p){
+        LOG_I(RRC, "Processing RRCConnectionReconfigurationComplete UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+        break;
+      }
 #ifdef RRC_MSG_PRINT
       LOG_F(RRC,"[MSG] RRC Connection Reconfiguration Complete\n");
 
@@ -4573,9 +4646,19 @@ rrc_eNB_decode_dcch(
             PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP),
             DCCH,
             sdu_sizeP);
+#ifdef UE_EXPANSION
+        ue_context_p->ue_context.ue_reestablishment_timer = 0;
+#else
+         ue_context_p->ue_context.ue_release_timer = 0;
+#endif
       break;
 
     case UL_DCCH_MessageType__c1_PR_rrcConnectionSetupComplete:
+      // to avoid segmentation fault
+      if(!ue_context_p){
+        LOG_I(RRC, "Processing RRCConnectionSetupComplete UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+        break;
+      }
 #ifdef RRC_MSG_PRINT
       LOG_F(RRC,"[MSG] RRC Connection SetupComplete\n");
 
@@ -4633,7 +4716,11 @@ rrc_eNB_decode_dcch(
     case UL_DCCH_MessageType__c1_PR_securityModeComplete:
       T(T_ENB_RRC_SECURITY_MODE_COMPLETE, T_INT(ctxt_pP->module_id), T_INT(ctxt_pP->frame),
         T_INT(ctxt_pP->subframe), T_INT(ctxt_pP->rnti));
-
+      // to avoid segmentation fault
+      if(!ue_context_p){
+        LOG_I(RRC, "Processing securityModeComplete UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+        break;
+      }
 #ifdef RRC_MSG_PRINT
       LOG_F(RRC,"[MSG] RRC Security Mode Complete\n");
 
@@ -4718,7 +4805,11 @@ rrc_eNB_decode_dcch(
     case UL_DCCH_MessageType__c1_PR_ueCapabilityInformation:
       T(T_ENB_RRC_UE_CAPABILITY_INFORMATION, T_INT(ctxt_pP->module_id), T_INT(ctxt_pP->frame),
         T_INT(ctxt_pP->subframe), T_INT(ctxt_pP->rnti));
-
+      // to avoid segmentation fault
+      if(!ue_context_p){
+          LOG_I(RRC, "Processing ueCapabilityInformation UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+          break;
+      }
 #ifdef RRC_MSG_PRINT
       LOG_F(RRC,"[MSG] RRC UECapablility Information \n");
 
@@ -4797,7 +4888,11 @@ rrc_eNB_decode_dcch(
     case UL_DCCH_MessageType__c1_PR_ulInformationTransfer:
       T(T_ENB_RRC_UL_INFORMATION_TRANSFER, T_INT(ctxt_pP->module_id), T_INT(ctxt_pP->frame),
         T_INT(ctxt_pP->subframe), T_INT(ctxt_pP->rnti));
-
+      // to avoid segmentation fault
+      if(!ue_context_p){
+          LOG_I(RRC, "Processing ulInformationTransfer UE %x, ue_context_p is NULL\n", ctxt_pP->rnti);
+          break;
+      }
 #ifdef RRC_MSG_PRINT
       LOG_F(RRC,"[MSG] RRC UL Information Transfer \n");
 
@@ -5044,6 +5139,12 @@ rrc_enb_task(
       /* Nothing to do. Apparently everything is done in S1AP processing */
       //LOG_I(RRC, "[eNB %d] Received message %s, not processed because procedure not synched\n",
       //instance, msg_name_p);
+#ifdef UE_EXPANSION
+    	if (rrc_eNB_get_ue_context(RC.rrc[instance], GTPV1U_ENB_DELETE_TUNNEL_RESP(msg_p).rnti)) {
+    	  rrc_eNB_get_ue_context(RC.rrc[instance], GTPV1U_ENB_DELETE_TUNNEL_RESP(msg_p).rnti)->ue_context.ue_release_timer_rrc =
+    	  rrc_eNB_get_ue_context(RC.rrc[instance], GTPV1U_ENB_DELETE_TUNNEL_RESP(msg_p).rnti)->ue_context.ue_release_timer_thres_rrc;
+    	}
+#endif
       break;
 
 #   endif
diff --git a/openair2/RRC/LITE/rrc_eNB_S1AP.c b/openair2/RRC/LITE/rrc_eNB_S1AP.c
index 24ee3c06dafa7a46b8920697db8969b87a806f55..4a8317e27f588e77540121c88f153514d7057553 100644
--- a/openair2/RRC/LITE/rrc_eNB_S1AP.c
+++ b/openair2/RRC/LITE/rrc_eNB_S1AP.c
@@ -1191,6 +1191,9 @@ int rrc_eNB_process_S1AP_UE_CONTEXT_RELEASE_COMMAND (MessageDef *msg_p, const ch
     }
     return (-1);
   } else {
+#ifdef UE_EXPANSION
+    ue_context_p->ue_context.ue_release_timer_s1 = 0;
+#endif
     PROTOCOL_CTXT_SET_BY_INSTANCE(&ctxt, instance, ENB_FLAG_YES, ue_context_p->ue_context.rnti, 0, 0);
     rrc_eNB_generate_RRCConnectionRelease(&ctxt, ue_context_p);
     /*
diff --git a/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp b/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
index 8b62a8e69855a36410e191c6832aa91009170906..3a84302130d476f4918397bb75e8ab8714c20b45 100644
--- a/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
+++ b/targets/ARCH/USRP/USERSPACE/LIB/usrp_lib.cpp
@@ -484,7 +484,11 @@ static int trx_usrp_write(openair0_device *device, openair0_timestamp timestamp,
     s->tx_md.start_of_burst = false;
     s->tx_md.end_of_burst = false;
   }
-  
+  if(flags==10){ // fail safe mode
+    s->tx_md.has_time_spec = false;
+    s->tx_md.start_of_burst = false;
+    s->tx_md.end_of_burst = true;
+  } 
   if (cc>1) {
     std::vector<void *> buff_ptrs;
     for (int i=0; i<cc; i++)
diff --git a/targets/RT/USER/lte-ru.c b/targets/RT/USER/lte-ru.c
index 54693f43c81c4b2e75747d1766a13b60678224d3..ea8c35ced0264fb78d8421f024f3251b9b43edd6 100644
--- a/targets/RT/USER/lte-ru.c
+++ b/targets/RT/USER/lte-ru.c
@@ -667,6 +667,20 @@ void fh_if4p5_north_out(RU_t *ru) {
   stop_meas(&ru->tx_fhaul);
 
 }
+
+/* add fail safe for late command */
+typedef enum {
+	STATE_BURST_NORMAL = 0,
+	STATE_BURST_TERMINATE = 1,
+	STATE_BURST_STOP_1 = 2,
+	STATE_BURST_STOP_2 = 3,
+	STATE_BURST_RESTART = 4,
+} late_control_e;
+
+volatile late_control_e late_control=STATE_BURST_NORMAL;
+
+/* add fail safe for late command end */
+
 void rx_rf(RU_t *ru,int *frame,int *subframe) {
 
   RU_proc_t *proc = &ru->proc;
@@ -693,8 +707,12 @@ void rx_rf(RU_t *ru,int *frame,int *subframe) {
  
   proc->timestamp_rx = ts-ru->ts_offset;
 
-  AssertFatal(rxs == fp->samples_per_tti,
-	      "rx_rf: Asked for %d samples, got %d from USRP\n",fp->samples_per_tti,rxs);
+//  AssertFatal(rxs == fp->samples_per_tti,
+//	      "rx_rf: Asked for %d samples, got %d from USRP\n",fp->samples_per_tti,rxs);
+  if(rxs != fp->samples_per_tti){
+    LOG_E(PHY,"rx_rf: Asked for %d samples, got %d from USRP\n",fp->samples_per_tti,rxs);
+    late_control=STATE_BURST_TERMINATE;
+  }
 
   if (proc->first_rx == 1) {
     ru->ts_offset = proc->timestamp_rx;
@@ -750,8 +768,8 @@ void rx_rf(RU_t *ru,int *frame,int *subframe) {
   
   VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME( VCD_SIGNAL_DUMPER_VARIABLES_TRX_TS, proc->timestamp_rx&0xffffffff );
   
-  if (rxs != fp->samples_per_tti)
-    exit_fun( "problem receiving samples" );
+//  if (rxs != fp->samples_per_tti)
+//    exit_fun( "problem receiving samples" );
   
 
   
@@ -802,7 +820,33 @@ void tx_rf(RU_t *ru) {
 
     for (i=0; i<ru->nb_tx; i++)
       txp[i] = (void*)&ru->common.txdata[i][(proc->subframe_tx*fp->samples_per_tti)-sf_extension];
-
+    /* add fail safe for late command */
+    if(late_control!=STATE_BURST_NORMAL){//stop burst
+      switch (late_control) {
+      case STATE_BURST_TERMINATE:
+        flags=10; // end of burst and no time spec
+        late_control=STATE_BURST_STOP_1;
+        break;
+      
+      case STATE_BURST_STOP_1:
+        flags=0; // no send
+        late_control=STATE_BURST_STOP_2;
+        return;//no send
+       break;
+      
+      case STATE_BURST_STOP_2:
+        flags=0; // no send
+        late_control=STATE_BURST_RESTART;
+        return;//no send
+        break;
+      
+      case STATE_BURST_RESTART:
+        flags=2; // start burst
+        late_control=STATE_BURST_NORMAL;
+        break;
+      }
+    }
+    /* add fail safe for late command end */
     
     VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME( VCD_SIGNAL_DUMPER_VARIABLES_TRX_TST, (proc->timestamp_tx-ru->openair0_cfg.tx_sample_advance)&0xffffffff );
     VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME( VCD_SIGNAL_DUMPER_FUNCTIONS_TRX_WRITE, 1 );
@@ -820,8 +864,11 @@ void tx_rf(RU_t *ru) {
     VCD_SIGNAL_DUMPER_DUMP_FUNCTION_BY_NAME( VCD_SIGNAL_DUMPER_FUNCTIONS_TRX_WRITE, 0 );
     
     
-    AssertFatal(txs ==  siglen+sf_extension,"TX : Timeout (sent %d/%d)\n",txs, siglen);
-
+//    AssertFatal(txs ==  siglen+sf_extension,"TX : Timeout (sent %d/%d)\n",txs, siglen);
+    if( (txs !=  siglen+sf_extension) && (late_control==STATE_BURST_NORMAL) ){ /* add fail safe for late command */
+      late_control=STATE_BURST_TERMINATE;
+      LOG_E(PHY,"TX : Timeout (sent %d/%d) state =%d\n",txs, siglen,late_control);
+    }
   }
 }