diff --git a/doc/FEATURE_SET.md b/doc/FEATURE_SET.md
index 3f7ffc9fdb24bba9dd800ab9876d4e7b4349b3a6..726fe84a74b6648b05b63afcd581de17dd876273 100644
--- a/doc/FEATURE_SET.md
+++ b/doc/FEATURE_SET.md
@@ -357,6 +357,7 @@ The following features are valid for the gNB and the 5G-NR UE.
   - RRCSetupRequest/RRCSetup/RRCSetupComplete
   - RRC Uplink/Downlink Information transfer carrying NAS messages transparently
   - RRC Reconfiguration/Reconfiguration complete
+  - RRC Reestablishment/Reestablishment complete
   - Paging
   - Support for master cell group configuration
   - Interface with NGAP for the interactions with the AMF
diff --git a/openair1/SIMULATION/NR_PHY/dlsim.c b/openair1/SIMULATION/NR_PHY/dlsim.c
index 9b6db46aa89f9f773f7d48bfb7a90c14937da899..1fe4229c04bf4ff51b66d07e7fcd882dc0d43eed 100644
--- a/openair1/SIMULATION/NR_PHY/dlsim.c
+++ b/openair1/SIMULATION/NR_PHY/dlsim.c
@@ -99,6 +99,11 @@ nfapi_ue_release_request_body_t release_rntis;
 instance_t DUuniqInstance=0;
 instance_t CUuniqInstance=0;
 
+int nr_derive_key_ng_ran_star(uint16_t pci, uint64_t nr_arfcn_dl, const uint8_t key[32], uint8_t *key_ng_ran_star)
+{
+  return 0;
+}
+
 // dummy functions
 int dummy_nr_ue_ul_indication(nr_uplink_indication_t *ul_info)              { return(0);  }
 
diff --git a/openair1/SIMULATION/NR_PHY/prachsim.c b/openair1/SIMULATION/NR_PHY/prachsim.c
index 1e211b7eb7cbd826353fbff348838e9e8332042b..cb7094fe7774f6b19e5767574ad46a9f5a959ccb 100644
--- a/openair1/SIMULATION/NR_PHY/prachsim.c
+++ b/openair1/SIMULATION/NR_PHY/prachsim.c
@@ -94,6 +94,11 @@ int oai_nfapi_nr_rach_indication(nfapi_nr_rach_indication_t *ind) { return(0);
 instance_t DUuniqInstance=0;
 instance_t CUuniqInstance=0;
 
+int nr_derive_key_ng_ran_star(uint16_t pci, uint64_t nr_arfcn_dl, const uint8_t key[32], uint8_t *key_ng_ran_star)
+{
+  return 0;
+}
+
 void
 rrc_data_ind(
   const protocol_ctxt_t *const ctxt_pP,
diff --git a/openair1/SIMULATION/NR_PHY/ulsim.c b/openair1/SIMULATION/NR_PHY/ulsim.c
index fefae6581d1286f4b2637634db5d3031501f87e7..8685626284165d28ce2d0524747f883ebd9da1ae 100644
--- a/openair1/SIMULATION/NR_PHY/ulsim.c
+++ b/openair1/SIMULATION/NR_PHY/ulsim.c
@@ -95,6 +95,11 @@ nfapi_ue_release_request_body_t release_rntis;
 instance_t DUuniqInstance=0;
 instance_t CUuniqInstance=0;
 
+int nr_derive_key_ng_ran_star(uint16_t pci, uint64_t nr_arfcn_dl, const uint8_t key[32], uint8_t *key_ng_ran_star)
+{
+  return 0;
+}
+
 extern void fix_scd(NR_ServingCellConfig_t *scd);// forward declaration
 
 int8_t nr_mac_rrc_data_ind_ue(const module_id_t module_id,
diff --git a/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_RA.c b/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_RA.c
index 1282a0625c2ed7d59db653a29c7ffe84c2f31cdf..6d968a513efe1a7e187e3d385308c6a796a8ab71 100644
--- a/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_RA.c
+++ b/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_RA.c
@@ -625,11 +625,12 @@ void nr_initiate_ra_proc(module_id_t module_idP,
     int loop = 0;
     if (ra->rnti == 0) { // This condition allows for the usage of a preconfigured rnti for the CFRA
       do {
-        ra->rnti = (taus() % 65518) + 1;
+        // 3GPP TS 38.321 version 15.13.0 Section 7.1 Table 7.1-1: RNTI values
+        ra->rnti = (taus() % 0xffef) + 1;
         loop++;
       } while (loop != 100
                && !((find_nr_UE(&nr_mac->UE_info, ra->rnti) == NULL) && (find_nr_RA_id(module_idP, CC_id, ra->rnti) == -1)
-                    && ra->rnti >= 1 && ra->rnti <= 65519));
+                    && ra->rnti >= 0x1 && ra->rnti <= 0xffef));
       if (loop == 100) {
         LOG_E(NR_MAC, "%s:%d:%s: [RAPROC] initialisation random access aborted\n", __FILE__, __LINE__, __FUNCTION__);
         abort();
diff --git a/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_ulsch.c b/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_ulsch.c
index dd44ce44f7780b75420f32147ae82464acde1ac7..a3c5a2e95cae45311f8b765abce566f5f5166f93 100644
--- a/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_ulsch.c
+++ b/openair2/LAYER2/NR_MAC_gNB/gNB_scheduler_ulsch.c
@@ -699,11 +699,9 @@ void nr_rx_sdu(const module_id_t gnb_mod_idP,
           continue;
         }
 
-
-	NR_UE_info_t* UE = add_new_nr_ue(gNB_mac, ra->rnti, ra->CellGroup);
+        NR_UE_info_t *UE = add_new_nr_ue(gNB_mac, ra->rnti, ra->CellGroup);
         if (!UE) {
           LOG_W(NR_MAC, "Random Access %i discarded at state %i (TC_RNTI %04x RNTI %04x): max number of users achieved!\n", i, ra->state,ra->rnti,current_rnti);
-
           nr_mac_remove_ra_rnti(gnb_mod_idP, ra->rnti);
           nr_clear_ra_proc(gnb_mod_idP, CC_idP, frameP, ra);
           return;
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.c b/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.c
index 9a584a3b27edf9fd613a3342f9c033b4a6f0106d..b102bf3b7f3d5b68617645073ef634d893750fb3 100644
--- a/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.c
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.c
@@ -32,7 +32,7 @@ void e1_add_drb(int is_gnb,
                 unsigned char *ciphering_key,
                 unsigned char *integrity_key)
 {
-  add_drb_am(is_gnb, ue_id, s, ciphering_algorithm, integrity_algorithm,
+  add_drb_am(is_gnb, ue_id, 0, s, ciphering_algorithm, integrity_algorithm,
              ciphering_key, integrity_key);
   LOG_I(PDCP, "%s:%s:%d: added DRB for UE ID %ld\n", __FILE__, __FUNCTION__, __LINE__, ue_id);
 }
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.h b/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.h
index a4b44fb74aaaa628336311490bf054e37480c980..68b146efeba2ee5ee976a63768670b1a8f19367d 100644
--- a/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.h
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_e1_api.h
@@ -45,7 +45,7 @@ void nr_pdcp_e1_add_drbs(eNB_flag_t enb_flag,
                          uint8_t        *const kUPenc,
                          uint8_t        *const kUPint);
 
-void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, struct NR_DRB_ToAddMod *s,
+void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, ue_id_t reestablish_ue_id, struct NR_DRB_ToAddMod *s,
                 int ciphering_algorithm,
                 int integrity_algorithm,
                 unsigned char *ciphering_key,
diff --git a/openair2/LAYER2/nr_pdcp/nr_pdcp_oai_api.c b/openair2/LAYER2/nr_pdcp/nr_pdcp_oai_api.c
index 190757d74c73853dec1a9c1f6ac4093c6a9df089..87e3d53ff512d4416f5b360a7206b3e669b6b12e 100644
--- a/openair2/LAYER2/nr_pdcp/nr_pdcp_oai_api.c
+++ b/openair2/LAYER2/nr_pdcp/nr_pdcp_oai_api.c
@@ -877,7 +877,7 @@ static void add_srb(int is_gnb, ue_id_t rntiMaybeUEid, struct NR_SRB_ToAddMod *s
   nr_pdcp_manager_unlock(nr_pdcp_ue_manager);
 }
 
-void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, struct NR_DRB_ToAddMod *s, int ciphering_algorithm, int integrity_algorithm, unsigned char *ciphering_key, unsigned char *integrity_key)
+void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, ue_id_t reestablish_ue_id, struct NR_DRB_ToAddMod *s, int ciphering_algorithm, int integrity_algorithm, unsigned char *ciphering_key, unsigned char *integrity_key)
 {
   nr_pdcp_entity_t *pdcp_drb;
   nr_pdcp_ue_t *ue;
@@ -962,6 +962,14 @@ void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, struct NR_DRB_ToAddMod *s, in
                                   has_integrity ? integrity_key : NULL);
     nr_pdcp_ue_add_drb_pdcp_entity(ue, drb_id, pdcp_drb);
 
+    if (reestablish_ue_id > 0) {
+      nr_pdcp_ue_t *reestablish_ue = nr_pdcp_manager_get_ue(nr_pdcp_ue_manager, reestablish_ue_id);
+      if (reestablish_ue != NULL) {
+        pdcp_drb->tx_next = reestablish_ue->drb[drb_id - 1]->tx_next;
+        LOG_I(PDCP, "Applying tx_next %d in DRB %d from old UEid %lx to new UEid %lx\n", reestablish_ue->drb[drb_id - 1]->tx_next, drb_id, reestablish_ue_id, rntiMaybeUEid);
+      }
+    }
+
     LOG_D(PDCP, "%s:%d:%s: added drb %d to UE ID/RNTI %ld\n", __FILE__, __LINE__, __FUNCTION__, drb_id, rntiMaybeUEid);
 
     new_nr_sdap_entity(is_gnb, has_sdap_rx, has_sdap_tx, rntiMaybeUEid, pdusession_id, is_sdap_DefaultDRB, drb_id, mappedQFIs2Add, mappedQFIs2AddCount);
@@ -971,6 +979,7 @@ void add_drb_am(int is_gnb, ue_id_t rntiMaybeUEid, struct NR_DRB_ToAddMod *s, in
 
 static void add_drb(int is_gnb,
                     ue_id_t rntiMaybeUEid,
+                    ue_id_t reestablish_ue_id,
                     struct NR_DRB_ToAddMod *s,
                     NR_RLC_Config_t *rlc_Config,
                     int ciphering_algorithm,
@@ -980,12 +989,12 @@ static void add_drb(int is_gnb,
 {
   switch (rlc_Config->present) {
   case NR_RLC_Config_PR_am:
-    add_drb_am(is_gnb, rntiMaybeUEid, s, ciphering_algorithm, integrity_algorithm, ciphering_key, integrity_key);
+    add_drb_am(is_gnb, rntiMaybeUEid, reestablish_ue_id, s, ciphering_algorithm, integrity_algorithm, ciphering_key, integrity_key);
     break;
   case NR_RLC_Config_PR_um_Bi_Directional:
     // add_drb_um(rntiMaybeUEid, s);
     /* hack */
-    add_drb_am(is_gnb, rntiMaybeUEid, s, ciphering_algorithm, integrity_algorithm, ciphering_key, integrity_key);
+    add_drb_am(is_gnb, rntiMaybeUEid, reestablish_ue_id, s, ciphering_algorithm, integrity_algorithm, ciphering_key, integrity_key);
     break;
   default:
     LOG_E(PDCP, "%s:%d:%s: fatal: unhandled DRB type\n",
@@ -1011,6 +1020,7 @@ void nr_pdcp_add_srbs(eNB_flag_t enb_flag, ue_id_t rntiMaybeUEid, NR_SRB_ToAddMo
 
 void nr_pdcp_add_drbs(eNB_flag_t enb_flag,
                       ue_id_t rntiMaybeUEid,
+                      ue_id_t reestablish_ue_id,
                       NR_DRB_ToAddModList_t *const drb2add_list,
                       const uint8_t security_modeP,
                       uint8_t *const kUPenc,
@@ -1019,7 +1029,7 @@ void nr_pdcp_add_drbs(eNB_flag_t enb_flag,
 {
   if (drb2add_list != NULL) {
     for (int i = 0; i < drb2add_list->list.count; i++) {
-      add_drb(enb_flag, rntiMaybeUEid, drb2add_list->list.array[i], rlc_bearer2add_list->list.array[i]->rlc_Config, security_modeP & 0x0f, (security_modeP >> 4) & 0x0f, kUPenc, kUPint);
+      add_drb(enb_flag, rntiMaybeUEid, reestablish_ue_id, drb2add_list->list.array[i], rlc_bearer2add_list->list.array[i]->rlc_Config, security_modeP & 0x0f, (security_modeP >> 4) & 0x0f, kUPenc, kUPint);
     }
   } else
     LOG_W(PDCP, "nr_pdcp_add_drbs() with void list\n");
@@ -1109,7 +1119,7 @@ void nr_DRB_preconfiguration(ue_id_t crntiMaybeUEid)
     PROTOCOL_CTXT_SET_BY_MODULE_ID(&ctxt, 0, ENB_FLAG_NO, crntiMaybeUEid, 0, 0, 0);
   }
 
-  nr_pdcp_add_drbs(ctxt.enb_flag, ctxt.rntiMaybeUEid, rbconfig->drb_ToAddModList, 0, NULL, NULL, Rlc_Bearer_ToAdd_list);
+  nr_pdcp_add_drbs(ctxt.enb_flag, ctxt.rntiMaybeUEid, 0, rbconfig->drb_ToAddModList, 0, NULL, NULL, Rlc_Bearer_ToAdd_list);
 
   nr_rrc_rlc_config_asn1_req(&ctxt, (NR_SRB_ToAddModList_t *)NULL, rbconfig->drb_ToAddModList, rbconfig->drb_ToReleaseList, Rlc_Bearer_ToAdd_list);
 
diff --git a/openair2/RRC/NR/MESSAGES/asn1_msg.c b/openair2/RRC/NR/MESSAGES/asn1_msg.c
index 6e57409228a90f30649d4c1a04475ea59bcf583a..3392ea6589bdff1dd443b2d7af6fd8cfcfddf1b1 100644
--- a/openair2/RRC/NR/MESSAGES/asn1_msg.c
+++ b/openair2/RRC/NR/MESSAGES/asn1_msg.c
@@ -46,6 +46,7 @@
 #include "LAYER2/nr_rlc/nr_rlc_oai_api.h"
 #include "asn1_msg.h"
 #include "../nr_rrc_proto.h"
+#include "UTIL/OSA/osa_defs.h"
 #include "RRC/NR/nr_rrc_extern.h"
 #include "NR_DL-CCCH-Message.h"
 #include "NR_UL-CCCH-Message.h"
@@ -1283,7 +1284,7 @@ int do_RRCSetup(rrc_gNB_ue_context_t         *const ue_context_pP,
 				     buffer,
 				     1000);
 
-    
+
     AssertFatal(enc_rval.encoded >0, "ASN1 message encoding failed (%s, %lu)!\n",
 	    enc_rval.failed_type->name, enc_rval.encoded);
 
@@ -1388,7 +1389,7 @@ uint8_t do_NR_SA_UECapabilityEnquiry( const protocol_ctxt_t *const ctxt_pP,
 				   1024);
   AssertFatal(enc_rval.encoded >0, "ASN1 message encoding failed (%s, %lu)!\n",
               enc_rval.failed_type->name, enc_rval.encoded);
-  
+
 
   if ( LOG_DEBUGFLAG(DEBUG_ASN1) ) {
     xer_fprint(stdout, &asn_DEF_NR_UE_CapabilityRequestFilterNR, (void *)sa_band_filter);
@@ -1415,16 +1416,14 @@ uint8_t do_NR_SA_UECapabilityEnquiry( const protocol_ctxt_t *const ctxt_pP,
 
   AssertFatal(enc_rval.encoded >0, "ASN1 message encoding failed (%s, %lu)!\n",
               enc_rval.failed_type->name, enc_rval.encoded);
-  
+
   LOG_D(NR_RRC, "[gNB %d] NR UECapabilityRequest for UE %lx Encoded %zd bits (%zd bytes)\n", ctxt_pP->module_id, ctxt_pP->rntiMaybeUEid, enc_rval.encoded, (enc_rval.encoded + 7) / 8);
 
   return((enc_rval.encoded+7)/8);
 }
 
-
-uint8_t do_NR_RRCRelease(uint8_t                            *buffer,
-                         size_t                              buffer_size,
-                         uint8_t                             Transaction_id) {
+int do_NR_RRCRelease(uint8_t *buffer, size_t buffer_size, uint8_t Transaction_id)
+{
   asn_enc_rval_t enc_rval;
   NR_DL_DCCH_Message_t dl_dcch_msg;
   NR_RRCRelease_t *rrcConnectionRelease;
@@ -1538,32 +1537,32 @@ int16_t do_RRCReconfiguration(
         xer_fprint(stdout, &asn_DEF_NR_CellGroupConfig, (const void *) cellGroupConfig);
       }
       ie->nonCriticalExtension->masterCellGroup = calloc(1,sizeof(OCTET_STRING_t));
-      
+
       ie->nonCriticalExtension->masterCellGroup->buf = masterCellGroup_buf;
       ie->nonCriticalExtension->masterCellGroup->size = (enc_rval.encoded+7)/8;
     }
-    
+
     dl_dcch_msg.message.choice.c1->choice.rrcReconfiguration->criticalExtensions.choice.rrcReconfiguration = ie;
-    
+
     if ( LOG_DEBUGFLAG(DEBUG_ASN1) ) {
       xer_fprint(stdout, &asn_DEF_NR_DL_DCCH_Message, (void *)&dl_dcch_msg);
     }
-    
+
     enc_rval = uper_encode_to_buffer(&asn_DEF_NR_DL_DCCH_Message,
                                      NULL,
                                      (void *)&dl_dcch_msg,
                                      buffer,
                                      buffer_size);
-    
+
     AssertFatal(enc_rval.encoded >0, "ASN1 message encoding failed (%s, %lu)!\n",
                 enc_rval.failed_type->name, enc_rval.encoded);
-    
+
     LOG_D(NR_RRC,"[gNB %d] RRCReconfiguration for UE %lx Encoded %zd bits (%zd bytes)\n",
           ctxt_pP->module_id,
           ctxt_pP->rntiMaybeUEid,
           enc_rval.encoded,
           (enc_rval.encoded+7)/8);
-    
+
     return((enc_rval.encoded+7)/8);
 }
 
@@ -1698,9 +1697,9 @@ uint8_t do_RRCSetupComplete(uint8_t Mod_id, uint8_t *buffer, size_t buffer_size,
   NR_UL_DCCH_Message_t  ul_dcch_msg;
   NR_RRCSetupComplete_t *RrcSetupComplete;
   memset((void *)&ul_dcch_msg,0,sizeof(NR_UL_DCCH_Message_t));
-  
+
   uint8_t buf[6];
-  
+
   ul_dcch_msg.message.present = NR_UL_DCCH_MessageType_PR_c1;
   ul_dcch_msg.message.choice.c1 = CALLOC(1,sizeof(struct NR_UL_DCCH_MessageType__c1));
   ul_dcch_msg.message.choice.c1->present = NR_UL_DCCH_MessageType__c1_PR_rrcSetupComplete;
@@ -1713,7 +1712,7 @@ uint8_t do_RRCSetupComplete(uint8_t Mod_id, uint8_t *buffer, size_t buffer_size,
   //   sizeof(*RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->nonCriticalExtension));
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->selectedPLMN_Identity = sel_plmn_id;
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->registeredAMF = NULL;
-  
+
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value = CALLOC(1, sizeof(struct NR_RRCSetupComplete_IEs__ng_5G_S_TMSI_Value));
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value->present = NR_RRCSetupComplete_IEs__ng_5G_S_TMSI_Value_PR_ng_5G_S_TMSI;
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value->choice.ng_5G_S_TMSI.size = 6;
@@ -1724,13 +1723,13 @@ uint8_t do_RRCSetupComplete(uint8_t Mod_id, uint8_t *buffer, size_t buffer_size,
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value->choice.ng_5G_S_TMSI.buf[3] = 0x78;
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value->choice.ng_5G_S_TMSI.buf[4] = 0x9A;
   RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->ng_5G_S_TMSI_Value->choice.ng_5G_S_TMSI.buf[5] = 0xBC;
-  
+
   memset(&RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->dedicatedNAS_Message,0,sizeof(OCTET_STRING_t));
   OCTET_STRING_fromBuf(&RrcSetupComplete->criticalExtensions.choice.rrcSetupComplete->dedicatedNAS_Message,dedicatedInfoNAS,dedicatedInfoNASLength);
   if ( LOG_DEBUGFLAG(DEBUG_ASN1) ) {
     xer_fprint(stdout, &asn_DEF_NR_UL_DCCH_Message, (void *)&ul_dcch_msg);
   }
-  
+
   enc_rval = uper_encode_to_buffer(&asn_DEF_NR_UL_DCCH_Message,
                                    NULL,
                                    (void *)&ul_dcch_msg,
@@ -1739,7 +1738,7 @@ uint8_t do_RRCSetupComplete(uint8_t Mod_id, uint8_t *buffer, size_t buffer_size,
   AssertFatal(enc_rval.encoded > 0,"ASN1 message encoding failed (%s, %lu)!\n",
               enc_rval.failed_type->name,enc_rval.encoded);
   LOG_D(NR_RRC,"RRCSetupComplete Encoded %zd bits (%zd bytes)\n",enc_rval.encoded,(enc_rval.encoded+7)/8);
-  
+
   return((enc_rval.encoded+7)/8);
 }
 
@@ -1844,133 +1843,139 @@ uint8_t do_RRCReestablishmentRequest(uint8_t Mod_id, uint8_t *buffer, uint16_t c
 }
 
 //------------------------------------------------------------------------------
-uint8_t
-do_RRCReestablishment(
-const protocol_ctxt_t     *const ctxt_pP,
-rrc_gNB_ue_context_t      *const ue_context_pP,
-int                              CC_id,
-uint8_t                   *const buffer,
-size_t                           buffer_size,
-//const uint8_t                    transmission_mode,
-const uint8_t                    Transaction_id,
-NR_SRB_ToAddModList_t               **SRB_configList
-) {
-    asn_enc_rval_t enc_rval;
-    //long *logicalchannelgroup = NULL;
-    struct NR_SRB_ToAddMod *SRB1_config = NULL;
-    struct NR_SRB_ToAddMod *SRB2_config = NULL;
-    //gNB_RRC_INST *nrrrc               = RC.nrrrc[ctxt_pP->module_id];
-    NR_DL_DCCH_Message_t dl_dcch_msg;
-    NR_RRCReestablishment_t *rrcReestablishment = NULL;
-    int i = 0;
-    ue_context_pP->ue_context.reestablishment_xid = Transaction_id;
-    NR_SRB_ToAddModList_t **SRB_configList2 = NULL;
-    SRB_configList2 = &ue_context_pP->ue_context.SRB_configList2[Transaction_id];
-
-    if (*SRB_configList2) {
-      free(*SRB_configList2);
-    }
+int do_RRCReestablishment(const protocol_ctxt_t *const ctxt_pP,
+                              rrc_gNB_ue_context_t *const ue_context_pP,
+                              int CC_id,
+                              uint8_t *const buffer,
+                              size_t buffer_size,
+                              const uint8_t Transaction_id,
+                              NR_SRB_ToAddModList_t **SRB_configList,
+                              const uint8_t *masterCellGroup_from_DU,
+                              NR_ServingCellConfigCommon_t *scc,
+                              rrc_gNB_carrier_data_t *carrier)
+{
+  asn_enc_rval_t enc_rval;
+  struct NR_SRB_ToAddMod *SRB1_config = NULL;
+  struct NR_SRB_ToAddMod *SRB2_config = NULL;
+  NR_DL_DCCH_Message_t dl_dcch_msg = {0};
+  NR_RRCReestablishment_t *rrcReestablishment = NULL;
+  ue_context_pP->ue_context.reestablishment_xid = Transaction_id;
+  NR_SRB_ToAddModList_t **SRB_configList2 = NULL;
+  SRB_configList2 = &ue_context_pP->ue_context.SRB_configList2[Transaction_id];
+
+  if (*SRB_configList2) {
+    free(*SRB_configList2);
+  }
 
-    *SRB_configList2 = CALLOC(1, sizeof(NR_SRB_ToAddModList_t));
-    memset((void *)&dl_dcch_msg, 0, sizeof(NR_DL_DCCH_Message_t));
-    dl_dcch_msg.message.present           = NR_DL_DCCH_MessageType_PR_c1;
-    dl_dcch_msg.message.choice.c1 = calloc(1,sizeof(struct NR_DL_DCCH_MessageType__c1));
-    dl_dcch_msg.message.choice.c1->present = NR_DL_DCCH_MessageType__c1_PR_rrcReestablishment;
-    dl_dcch_msg.message.choice.c1->choice.rrcReestablishment = CALLOC(1,sizeof(NR_RRCReestablishment_t));
-    rrcReestablishment = dl_dcch_msg.message.choice.c1->choice.rrcReestablishment;
-
-    // get old configuration of SRB2
-    if (*SRB_configList != NULL) {
-      for (i = 0; (i < (*SRB_configList)->list.count) && (i < 3); i++) {
-        LOG_D(NR_RRC, "(*SRB_configList)->list.array[%d]->srb_Identity=%ld\n",
-              i, (*SRB_configList)->list.array[i]->srb_Identity);
-    
-        if ((*SRB_configList)->list.array[i]->srb_Identity == 2 ) {
-          SRB2_config = (*SRB_configList)->list.array[i];
-        } else if ((*SRB_configList)->list.array[i]->srb_Identity == 1 ) {
-          SRB1_config = (*SRB_configList)->list.array[i];
-        }
+  *SRB_configList2 = CALLOC(1, sizeof(NR_SRB_ToAddModList_t));
+  dl_dcch_msg.message.present = NR_DL_DCCH_MessageType_PR_c1;
+  dl_dcch_msg.message.choice.c1 = calloc(1, sizeof(struct NR_DL_DCCH_MessageType__c1));
+  dl_dcch_msg.message.choice.c1->present = NR_DL_DCCH_MessageType__c1_PR_rrcReestablishment;
+  dl_dcch_msg.message.choice.c1->choice.rrcReestablishment = CALLOC(1, sizeof(NR_RRCReestablishment_t));
+  rrcReestablishment = dl_dcch_msg.message.choice.c1->choice.rrcReestablishment;
+
+  // get old configuration of SRB2
+  if (*SRB_configList != NULL) {
+    for (int i = 0; (i < (*SRB_configList)->list.count) && (i < 3); i++) {
+      LOG_D(NR_RRC, "(*SRB_configList)->list.array[%d]->srb_Identity=%ld\n", i, (*SRB_configList)->list.array[i]->srb_Identity);
+      if ((*SRB_configList)->list.array[i]->srb_Identity == 2) {
+        SRB2_config = (*SRB_configList)->list.array[i];
+      } else if ((*SRB_configList)->list.array[i]->srb_Identity == 1) {
+        SRB1_config = (*SRB_configList)->list.array[i];
       }
     }
+  }
 
-    if (SRB1_config == NULL) {
-      // default SRB1 configuration
-      LOG_W(NR_RRC,"SRB1 configuration does not exist in SRB configuration list, use default\n");
-      /// SRB1
-      SRB1_config = CALLOC(1, sizeof(*SRB1_config));
-      SRB1_config->srb_Identity = 1;
-    }
-
-    if (SRB2_config == NULL) {
-      LOG_W(NR_RRC,"SRB2 configuration does not exist in SRB configuration list\n");
-    } else {
-      asn1cSeqAdd(&(*SRB_configList2)->list, SRB2_config);
-    }
+  if (SRB1_config == NULL) {
+    // default SRB1 configuration
+    LOG_W(NR_RRC, "SRB1 configuration does not exist in SRB configuration list, use default\n");
+    /// SRB1
+    SRB1_config = CALLOC(1, sizeof(*SRB1_config));
+    SRB1_config->srb_Identity = 1;
+  }
 
-    if (*SRB_configList) {
-      free(*SRB_configList);
-    }
+  if (SRB2_config == NULL) {
+    LOG_W(NR_RRC, "SRB2 configuration does not exist in SRB configuration list\n");
+  } else {
+    asn1cSeqAdd(&(*SRB_configList2)->list, SRB2_config);
+  }
 
-    *SRB_configList = CALLOC(1, sizeof(LTE_SRB_ToAddModList_t));
-    asn1cSeqAdd(&(*SRB_configList)->list,SRB1_config);
+  if (*SRB_configList) {
+    free(*SRB_configList);
+  }
 
-    rrcReestablishment->rrc_TransactionIdentifier = Transaction_id;
-    rrcReestablishment->criticalExtensions.present = NR_RRCReestablishment__criticalExtensions_PR_rrcReestablishment;
-    rrcReestablishment->criticalExtensions.choice.rrcReestablishment = CALLOC(1,sizeof(NR_RRCReestablishment_IEs_t));
+  *SRB_configList = CALLOC(1, sizeof(NR_SRB_ToAddModList_t));
+  asn1cSeqAdd(&(*SRB_configList)->list, SRB1_config);
+  /****************************** masterCellGroup ******************************/
+  /*
+   NR_CellGroupConfig_t *cellGroupConfig = NULL;
+   char masterCellGroup_buf[1000];
+   gNB_RRC_UE_t *ue_p = &ue_context_pP->ue_context;
+   if (masterCellGroup_from_DU) {
+    // decode masterCellGroup OCTET_STRING received from DU and place in ue context
+    uper_decode(NULL,
+    &asn_DEF_NR_CellGroupConfig,   //might be added prefix later
+    (void **)&cellGroupConfig,
+    (uint8_t *)masterCellGroup_from_DU->buf,
+    masterCellGroup_from_DU->size, 0, 0);
 
-    uint8_t KgNB_star[32] = { 0 };
-    /** TODO
-    uint16_t pci = nrrrc->carrier[CC_id].physCellId;
-    uint32_t earfcn_dl = (uint32_t)freq_to_arfcn10(RC.mac[ctxt_pP->module_id]->common_channels[CC_id].eutra_band,
-                         nrrrc->carrier[CC_id].dl_CarrierFreq);
-    bool     is_rel8_only = true;
-    
-    if (earfcn_dl > 65535) {
-      is_rel8_only = false;
-    }
-    LOG_D(NR_RRC, "pci=%d, eutra_band=%d, downlink_frequency=%d, earfcn_dl=%u, is_rel8_only=%s\n",
-          pci,
-          RC.mac[ctxt_pP->module_id]->common_channels[CC_id].eutra_band,
-          nrrrc->carrier[CC_id].dl_CarrierFreq,
-          earfcn_dl,
-          is_rel8_only == true ? "true": "false");
-    */
-    
-    if (ue_context_pP->ue_context.nh_ncc >= 0) {
-      //TODO derive_keNB_star(ue_context_pP->ue_context.nh, pci, earfcn_dl, is_rel8_only, KgNB_star);
-      rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nextHopChainingCount = ue_context_pP->ue_context.nh_ncc;
-    } else { // first HO
-      //TODO derive_keNB_star (ue_context_pP->ue_context.kgnb, pci, earfcn_dl, is_rel8_only, KgNB_star);
-      // LG: really 1
-      rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nextHopChainingCount = 0;
+    xer_fprint(stdout, &asn_DEF_NR_CellGroupConfig, (const void*)cellGroupConfig);
+  }
+  else {
+    cellGroupConfig = calloc(1, sizeof(NR_CellGroupConfig_t));
+    fill_initial_cellGroupConfig(ue_context_pP->ue_context.rnti,ue_context_pP->local_uid,cellGroupConfig,scc,carrier);
+
+    enc_rval = uper_encode_to_buffer(&asn_DEF_NR_CellGroupConfig,
+             NULL,
+             (void *)cellGroupConfig,
+             masterCellGroup_buf,
+             1000);
+
+    if(enc_rval.encoded == -1) {
+      LOG_E(NR_RRC, "ASN1 message CellGroupConfig encoding failed (%s, %lu)!\n",
+      enc_rval.failed_type->name, enc_rval.encoded);
+      return -1;
     }
-    // copy KgNB_star to ue_context_pP->ue_context.kgnb
-    memcpy (ue_context_pP->ue_context.kgnb, KgNB_star, 32);
-    ue_context_pP->ue_context.kgnb_ncc = 0;
-    rrcReestablishment->criticalExtensions.choice.rrcReestablishment->lateNonCriticalExtension = NULL;
-    rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nonCriticalExtension = NULL;
+  }
+  ue_p->masterCellGroup = cellGroupConfig;
+  */
+  rrcReestablishment->rrc_TransactionIdentifier = Transaction_id;
+  rrcReestablishment->criticalExtensions.present = NR_RRCReestablishment__criticalExtensions_PR_rrcReestablishment;
+  rrcReestablishment->criticalExtensions.choice.rrcReestablishment = CALLOC(1, sizeof(NR_RRCReestablishment_IEs_t));
+
+  uint16_t pci = RC.nrrrc[ctxt_pP->module_id]->carrier.physCellId;
+  uint32_t nr_arfcn_dl = (uint64_t)*scc->downlinkConfigCommon->frequencyInfoDL->absoluteFrequencySSB;
+  LOG_I(NR_RRC, "Reestablishment update key pci=%d, earfcn_dl=%u\n", pci, nr_arfcn_dl);
+
+  // 3GPP TS 33.501 Section 6.11 Security handling for RRC connection re-establishment procedure
+  if (ue_context_pP->ue_context.nh_ncc >= 0) {
+    nr_derive_key_ng_ran_star(pci, nr_arfcn_dl, ue_context_pP->ue_context.nh, ue_context_pP->ue_context.kgnb);
+    rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nextHopChainingCount = ue_context_pP->ue_context.nh_ncc;
+  } else { // first HO
+    nr_derive_key_ng_ran_star(pci, nr_arfcn_dl, ue_context_pP->ue_context.kgnb, ue_context_pP->ue_context.kgnb);
+    // LG: really 1
+    rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nextHopChainingCount = 0;
+  }
 
-    if ( LOG_DEBUGFLAG(DEBUG_ASN1) ) {
-      xer_fprint(stdout, &asn_DEF_NR_DL_DCCH_Message, (void *)&dl_dcch_msg);
-    }
+  ue_context_pP->ue_context.kgnb_ncc = 0;
+  rrcReestablishment->criticalExtensions.choice.rrcReestablishment->lateNonCriticalExtension = NULL;
+  rrcReestablishment->criticalExtensions.choice.rrcReestablishment->nonCriticalExtension = NULL;
 
-    enc_rval = uper_encode_to_buffer(&asn_DEF_NR_DL_DCCH_Message,
-                                     NULL,
-                                     (void *)&dl_dcch_msg,
-                                     buffer,
-                                     100);
+  if (LOG_DEBUGFLAG(DEBUG_ASN1)) {
+    xer_fprint(stdout, &asn_DEF_NR_DL_DCCH_Message, (void *)&dl_dcch_msg);
+  }
 
-    AssertFatal(enc_rval.encoded > 0, "ASN1 message encoding failed (%s, %lu)!\n",
-                enc_rval.failed_type->name, enc_rval.encoded);
-    
-    LOG_D(NR_RRC,"RRCReestablishment Encoded %u bits (%u bytes)\n",
-          (uint32_t)enc_rval.encoded, (uint32_t)(enc_rval.encoded+7)/8);
-    return((enc_rval.encoded+7)/8);
+  enc_rval = uper_encode_to_buffer(&asn_DEF_NR_DL_DCCH_Message, NULL, (void *)&dl_dcch_msg, buffer, 100);
 
+  AssertFatal(enc_rval.encoded > 0, "ASN1 message encoding failed (%s, %lu)!\n",
+              enc_rval.failed_type->name, enc_rval.encoded);
+
+  LOG_D(NR_RRC, "RRCReestablishment Encoded %u bits (%u bytes)\n", (uint32_t)enc_rval.encoded, (uint32_t)(enc_rval.encoded + 7) / 8);
+  return ((enc_rval.encoded + 7) / 8);
 }
 
-uint8_t 
-do_RRCReestablishmentComplete(uint8_t *buffer, size_t buffer_size, int64_t rrc_TransactionIdentifier) {
+int do_RRCReestablishmentComplete(uint8_t *buffer, size_t buffer_size, int64_t rrc_TransactionIdentifier)
+{
   asn_enc_rval_t enc_rval;
   NR_UL_DCCH_Message_t ul_dcch_msg;
   NR_RRCReestablishmentComplete_t *rrcReestablishmentComplete;
diff --git a/openair2/RRC/NR/MESSAGES/asn1_msg.h b/openair2/RRC/NR/MESSAGES/asn1_msg.h
index 87124c11720f7cefa60d1b89c193c911c0d303d8..090bbb5c84cae3b678ed832060af5d2faf9e2f72 100644
--- a/openair2/RRC/NR/MESSAGES/asn1_msg.h
+++ b/openair2/RRC/NR/MESSAGES/asn1_msg.h
@@ -119,8 +119,7 @@ uint8_t do_NR_SA_UECapabilityEnquiry( const protocol_ctxt_t *const ctxt_pP,
                                    uint8_t               *const buffer,
                                    const uint8_t                Transaction_id);
 
-uint8_t do_NR_RRCRelease(uint8_t *buffer, size_t buffer_size,
-                         uint8_t Transaction_id);
+int do_NR_RRCRelease(uint8_t *buffer, size_t buffer_size, uint8_t Transaction_id);
 
 int16_t do_RRCReconfiguration(
     const protocol_ctxt_t        *const ctxt_pP,
@@ -174,22 +173,18 @@ uint8_t do_NR_ULInformationTransfer(uint8_t **buffer,
 
 uint8_t do_RRCReestablishmentRequest(uint8_t Mod_id, uint8_t *buffer, uint16_t c_rnti);
 
-uint8_t
-do_RRCReestablishment(
-  const protocol_ctxt_t     *const ctxt_pP,
-  rrc_gNB_ue_context_t      *const ue_context_pP,
-  int                              CC_id,
-  uint8_t                   *const buffer,
-  size_t                           buffer_size,
-  //const uint8_t                    transmission_mode,
-  const uint8_t                    Transaction_id,
-  NR_SRB_ToAddModList_t               **SRB_configList
-);
-
-uint8_t 
-do_RRCReestablishmentComplete(
-    uint8_t *buffer, size_t buffer_size,
-    int64_t rrc_TransactionIdentifier);
+int do_RRCReestablishment(const protocol_ctxt_t *const ctxt_pP,
+                              rrc_gNB_ue_context_t *const ue_context_pP,
+                              int CC_id,
+                              uint8_t *const buffer,
+                              size_t buffer_size,
+                              const uint8_t Transaction_id,
+                              NR_SRB_ToAddModList_t **SRB_configList,
+                              const uint8_t *masterCellGroup_from_DU,
+                              NR_ServingCellConfigCommon_t *scc,
+                              rrc_gNB_carrier_data_t *carrier);
+
+int do_RRCReestablishmentComplete(uint8_t *buffer, size_t buffer_size, int64_t rrc_TransactionIdentifier);
 
 NR_MeasConfig_t *get_defaultMeasConfig(const gNB_RrcConfigurationReq *conf);
 uint8_t do_NR_Paging(uint8_t Mod_id, uint8_t *buffer, uint32_t tmsi);
diff --git a/openair2/RRC/NR/cucp_cuup_direct.c b/openair2/RRC/NR/cucp_cuup_direct.c
index 12530c7b8212c7218b52dd13273cfa04c0c3e15a..5bc1452b381b792a2a83ea98cd1cbb75f707c777 100644
--- a/openair2/RRC/NR/cucp_cuup_direct.c
+++ b/openair2/RRC/NR/cucp_cuup_direct.c
@@ -164,7 +164,7 @@ static int drb_config_gtpu_create(const protocol_ctxt_t *const ctxt_p,
                    kRRCenc,
                    kRRCint);
                    
-  nr_pdcp_add_drbs(ctxt_p->enb_flag, ctxt_p->rntiMaybeUEid,
+  nr_pdcp_add_drbs(ctxt_p->enb_flag, ctxt_p->rntiMaybeUEid, 0,
                    DRB_configList,
                    (ue_context_p->ue_context.integrity_algorithm << 4)
                    | ue_context_p->ue_context.ciphering_algorithm,
diff --git a/openair2/RRC/NR/nr_rrc_defs.h b/openair2/RRC/NR/nr_rrc_defs.h
index b8982d0bd56a69291f419101d65e678b70362b91..b3dafd5d0ad0282bdcc31ebc573cd16443321fb2 100644
--- a/openair2/RRC/NR/nr_rrc_defs.h
+++ b/openair2/RRC/NR/nr_rrc_defs.h
@@ -298,6 +298,9 @@ typedef struct gNB_RRC_UE_s {
   NR_RRCReconfiguration_t            *reconfig;
   NR_RadioBearerConfig_t             *rb_config;
 
+  /* Pointer to save spCellConfig during RRC Reestablishment procedures */
+  NR_SpCellConfig_t                  *spCellConfigReestablishment;
+
   ImsiMobileIdentity_t               imsi;
 
   /* KgNB as derived from KASME received from EPC */
@@ -320,7 +323,7 @@ typedef struct gNB_RRC_UE_s {
   uint16_t                           ng_5G_S_TMSI_Part2;
   NR_EstablishmentCause_t            establishment_cause;
 
-  /* Information from UE RRC ConnectionReestablishmentRequest */
+  /* Information from UE RRCReestablishmentRequest */
   NR_ReestablishmentCause_t          reestablishment_cause;
 
   /* UE id for initial connection to S1AP */
@@ -475,6 +478,11 @@ typedef struct cucp_cuup_if_s {
   cucp_cuup_bearer_context_setup_func_t bearer_context_mod;
 } cucp_cuup_if_t;
 
+typedef struct nr_reestablish_rnti_map_s {
+  ue_id_t ue_id;
+  rnti_t c_rnti;
+} nr_reestablish_rnti_map_t;
+
 //---NR---(completely change)---------------------
 typedef struct gNB_RRC_INST_s {
 
@@ -528,6 +536,8 @@ typedef struct gNB_RRC_INST_s {
   // security configuration (preferred algorithms)
   nr_security_configuration_t security;
 
+  nr_reestablish_rnti_map_t nr_reestablish_rnti_map[MAX_MOBILES_PER_GNB];
+
   nr_mac_rrc_dl_if_t mac_rrc;
   cucp_cuup_if_t cucp_cuup;
 
diff --git a/openair2/RRC/NR/nr_rrc_extern.h b/openair2/RRC/NR/nr_rrc_extern.h
index f5d9f4d2e46a586689578c7a92d6b5e6406ef34d..34041b8a8855dd2b767a14c14ae0ca0914f774fd 100644
--- a/openair2/RRC/NR/nr_rrc_extern.h
+++ b/openair2/RRC/NR/nr_rrc_extern.h
@@ -59,8 +59,4 @@ extern UE_PF_PO_t UE_PF_PO[NFAPI_CC_MAX][MAX_MOBILES_PER_ENB];
 
 extern pthread_mutex_t ue_pf_po_mutex;
 
-extern uint16_t reestablish_rnti_map[MAX_MOBILES_PER_ENB][2];
-
 #endif
-
-
diff --git a/openair2/RRC/NR/nr_rrc_proto.h b/openair2/RRC/NR/nr_rrc_proto.h
index dc32e71aea9d58390d455a5f83756dc1d4e8050d..cac95239e5ceda6d831671a5790c383843b5936a 100644
--- a/openair2/RRC/NR/nr_rrc_proto.h
+++ b/openair2/RRC/NR/nr_rrc_proto.h
@@ -210,6 +210,7 @@ void nr_pdcp_add_srbs(eNB_flag_t enb_flag, ue_id_t rntiMaybeUEid, NR_SRB_ToAddMo
 
 void nr_pdcp_add_drbs(eNB_flag_t enb_flag,
                       ue_id_t rntiMaybeUEid,
+                      ue_id_t reestablish_ue_id,
                       NR_DRB_ToAddModList_t *const drb2add_list,
                       const uint8_t security_modeP,
                       uint8_t *const kUPenc,
diff --git a/openair2/RRC/NR/rrc_gNB.c b/openair2/RRC/NR/rrc_gNB.c
index 8e68e7312c3aa704e6a12e26642dcd9780f36d4b..af82a58d34bbe08f8d254ffa08593fd119967b42 100644
--- a/openair2/RRC/NR/rrc_gNB.c
+++ b/openair2/RRC/NR/rrc_gNB.c
@@ -107,6 +107,15 @@
 
 extern RAN_CONTEXT_t RC;
 
+extern void pdcp_config_set_security(const protocol_ctxt_t *const ctxt_pP,
+                                     pdcp_t *const pdcp_pP,
+                                     const rb_id_t rb_idP,
+                                     const uint16_t lc_idP,
+                                     const uint8_t security_modeP,
+                                     uint8_t *const kRRCenc,
+                                     uint8_t *const kRRCint,
+                                     uint8_t *const  kUPenc);
+
 static inline uint64_t bitStr_to_uint64(BIT_STRING_t *asn);
 
 mui_t                               rrc_gNB_mui = 0;
@@ -316,6 +325,27 @@ static void apply_macrlc_config(gNB_RRC_INST *rrc, rrc_gNB_ue_context_t *const u
                              get_softmodem_params()->sa ? cgc->rlc_BearerToAddModList : NULL);
 }
 
+void apply_macrlc_config_reest(gNB_RRC_INST *rrc, rrc_gNB_ue_context_t *const ue_context_pP, const protocol_ctxt_t *const ctxt_pP, ue_id_t ue_id)
+{
+  rrc_mac_config_req_gNB(rrc->module_id,
+                         rrc->configuration.pdsch_AntennaPorts,
+                         rrc->configuration.pusch_AntennaPorts,
+                         rrc->configuration.sib1_tda,
+                         rrc->configuration.minRXTXTIME,
+                         NULL,
+                         NULL,
+                         NULL,
+                         0,
+                         ue_id,
+                         get_softmodem_params()->sa ? ue_context_pP->ue_context.masterCellGroup : NULL);
+
+  nr_rrc_rlc_config_asn1_req(ctxt_pP,
+                             ue_context_pP->ue_context.SRB_configList,
+                             NULL,
+                             NULL,
+                             get_softmodem_params()->sa ? ue_context_pP->ue_context.masterCellGroup->rlc_BearerToAddModList : NULL);
+}
+
 //-----------------------------------------------------------------------------
 static void rrc_gNB_generate_RRCSetup(const protocol_ctxt_t *const ctxt_pP,
                                       rrc_gNB_ue_context_t *const ue_context_pP,
@@ -420,6 +450,11 @@ static void rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(const protoc
   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;
+  // configure MAC
+  apply_macrlc_config(rrc_instance_p,ue_context_pP,ctxt_pP);
+
+  //nr_pdcp_add_srbs(ctxt_pP->enb_flag, ctxt_pP->rntiMaybeUEid, ue_context_pP->ue_context.SRB_configList, 0, NULL, NULL);
+  //apply_pdcp_config(ue_context_pP,ctxt_pP);
   /* init timers */
   //   ue_context_pP->ue_context.ue_rrc_inactivity_timer = 0;
 
@@ -479,6 +514,7 @@ static void rrc_gNB_process_RRCSetupComplete(const protocol_ctxt_t *const ctxt_p
       PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
   ue_context_pP->ue_context.Srb1.Active = 1;
   ue_context_pP->ue_context.Srb1.Srb_info.Srb_id = 1;
+  ue_context_pP->ue_context.Srb2.Active = 0;
   ue_context_pP->ue_context.StatusRrc = NR_RRC_CONNECTED;
 
   if (get_softmodem_params()->sa) {
@@ -538,7 +574,7 @@ static void rrc_gNB_generate_defaultRRCReconfiguration(const protocol_ctxt_t *co
   uint8_t buffer[RRC_BUF_SIZE] = {0};
   int size = do_RRCReconfiguration(ctxt_pP,
                                    buffer,
-                                   sizeof(buffer),
+                                   RRC_BUF_SIZE,
                                    xid,
                                    NULL, //*SRB_configList2,
                                    NULL, //*DRB_configList,
@@ -777,8 +813,6 @@ rrc_gNB_generate_dedicatedRRCReconfiguration(
   gNB_RRC_INST *rrc = RC.nrrrc[ctxt_pP->module_id];
   long drb_priority[1] = {13}; // For now, we assume only one drb per pdu sessions with a default preiority (will be dynamique in future)
   NR_CellGroupConfig_t *cellGroupConfig = NULL;
-  uint8_t  buffer[RRC_BUF_SIZE];
-  uint16_t size;
   int xid = -1;
 
   int drb_id_to_setup_start = 1;
@@ -816,7 +850,6 @@ rrc_gNB_generate_dedicatedRRCReconfiguration(
     dedicatedNAS_MessageList = NULL;
   }
 
-  memset(buffer, 0, sizeof(buffer));
   if(cell_groupConfig_from_DU == NULL){
     cellGroupConfig = calloc(1, sizeof(NR_CellGroupConfig_t));
     // FIXME: fill_mastercellGroupConfig() won't fill the right priorities or
@@ -832,20 +865,23 @@ rrc_gNB_generate_dedicatedRRCReconfiguration(
   NR_SRB_ToAddModList_t *SRB_configList2 = NULL;
   SRB_configList2 = ue_context_pP->ue_context.SRB_configList2[xid];
 
-  size = do_RRCReconfiguration(ctxt_pP, buffer, sizeof(buffer),
-                                xid,
-                                SRB_configList2,
-                                DRB_configList,
-                                NULL,
-                                NULL,
-                                NULL,
-                                NULL,
-                                dedicatedNAS_MessageList,
-                                ue_context_pP,
-                                &rrc->carrier,
-                                &rrc->configuration,
-                                NULL,
-                                cellGroupConfig);
+  uint8_t buffer[RRC_BUF_SIZE] = {0};
+  int size = do_RRCReconfiguration(ctxt_pP,
+                                   buffer,
+                                   RRC_BUF_SIZE,
+                                   xid,
+                                   SRB_configList2,
+                                   DRB_configList,
+                                   NULL,
+                                   NULL,
+                                   NULL,
+                                   NULL,
+                                   dedicatedNAS_MessageList,
+                                   ue_context_pP,
+                                   &rrc->carrier,
+                                   &rrc->configuration,
+                                   NULL,
+                                   cellGroupConfig);
   LOG_DUMPMSG(NR_RRC,DEBUG_RRC,(char *)buffer,size,"[MSG] RRC Reconfiguration\n");
 
   /* Free all NAS PDUs */
@@ -912,8 +948,6 @@ rrc_gNB_modify_dedicatedRRCReconfiguration(
   struct NR_RRCReconfiguration_v1530_IEs__dedicatedNAS_MessageList
                                 *dedicatedNAS_MessageList = NULL;
   NR_DedicatedNAS_Message_t     *dedicatedNAS_Message = NULL;
-  uint8_t                        buffer[RRC_BUF_SIZE];
-  uint16_t                       size;
   int                            qos_flow_index = 0;
   int i, j;
 
@@ -1017,21 +1051,23 @@ rrc_gNB_modify_dedicatedRRCReconfiguration(
     dedicatedNAS_MessageList = NULL;
   }
 
-  memset(buffer, 0, sizeof(buffer));
-  size = do_RRCReconfiguration(ctxt_pP, buffer, sizeof(buffer),
-                                xid,
-                                NULL,
-                                DRB_configList2,
-                                NULL,
-                                NULL,
-                                NULL,
-                                NULL,
-                                dedicatedNAS_MessageList,
-                                NULL,
-                                NULL,
-                                NULL,
-                                NULL,
-                                NULL);
+  uint8_t buffer[RRC_BUF_SIZE];
+  int size = do_RRCReconfiguration(ctxt_pP,
+                                       buffer,
+                                       RRC_BUF_SIZE,
+                                       xid,
+                                       NULL,
+                                       DRB_configList2,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       dedicatedNAS_MessageList,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL);
   LOG_DUMPMSG(NR_RRC, DEBUG_RRC, (char *)buffer, size, "[MSG] RRC Reconfiguration\n");
 
   /* Free all NAS PDUs */
@@ -1083,9 +1119,7 @@ rrc_gNB_generate_dedicatedRRCReconfiguration_release(
     uint8_t                 *nas_buffer)
 //-----------------------------------------------------------------------------
 {
-  uint8_t                             buffer[RRC_BUF_SIZE];
   int                                 i;
-  uint16_t                            size  = 0;
   NR_DRB_ToReleaseList_t             **DRB_Release_configList2 = NULL;
   NR_DRB_Identity_t                  *DRB_release;
   struct NR_RRCReconfiguration_v1530_IEs__dedicatedNAS_MessageList
@@ -1120,13 +1154,15 @@ rrc_gNB_generate_dedicatedRRCReconfiguration_release(
     LOG_W(NR_RRC,"dedlicated NAS list is empty\n");
   }
 
-  memset(buffer, 0, sizeof(buffer));
-  size = do_RRCReconfiguration(ctxt_pP, buffer, sizeof(buffer), xid, NULL, NULL, *DRB_Release_configList2, NULL, NULL, NULL, dedicatedNAS_MessageList, NULL, NULL, NULL, NULL, NULL);
+  uint8_t buffer[RRC_BUF_SIZE] = {0};
+  int size = do_RRCReconfiguration(ctxt_pP, buffer, RRC_BUF_SIZE, xid, NULL, NULL, *DRB_Release_configList2, NULL, NULL, NULL, dedicatedNAS_MessageList, NULL, NULL, NULL, NULL, NULL);
 
   ue_context_pP->ue_context.pdu_session_release_command_flag = 1;
 
   LOG_DUMPMSG(NR_RRC,DEBUG_RRC,(char *)buffer,size, "[MSG] RRC Reconfiguration\n");
 
+  ue_context_pP->ue_context.pdu_session_release_command_flag = 1;
+
   /* Free all NAS PDUs */
   if (nas_length > 0) {
     /* Free the NAS PDU buffer and invalidate it */
@@ -1174,6 +1210,10 @@ rrc_gNB_process_RRCReconfigurationComplete(
 )
 {
   int                                 drb_id;
+  uint8_t                            *kRRCenc = NULL;
+  uint8_t                            *kRRCint = NULL;
+  uint8_t                            *kUPenc = NULL;
+  uint8_t                            *kUPint = NULL;
   NR_DRB_ToAddModList_t              *DRB_configList = ue_context_pP->ue_context.DRB_configList2[xid];
   NR_SRB_ToAddModList_t              *SRB_configList = ue_context_pP->ue_context.SRB_configList2[xid];
   NR_DRB_ToReleaseList_t             *DRB_Release_configList2 = ue_context_pP->ue_context.DRB_Release_configList2[xid];
@@ -1182,9 +1222,55 @@ rrc_gNB_process_RRCReconfigurationComplete(
 
   ue_context_pP->ue_context.ue_reestablishment_timer = 0;
 
+  /* Derive the keys from kgnb */
+  if (DRB_configList != NULL) {
+    nr_derive_key_up_enc(ue_context_pP->ue_context.ciphering_algorithm,
+                         ue_context_pP->ue_context.kgnb,
+                         &kUPenc);
+    nr_derive_key_up_int(ue_context_pP->ue_context.integrity_algorithm,
+                         ue_context_pP->ue_context.kgnb,
+                         &kUPint);
+  }
+
+  nr_derive_key_rrc_enc(ue_context_pP->ue_context.ciphering_algorithm,
+                        ue_context_pP->ue_context.kgnb,
+                        &kRRCenc);
+  nr_derive_key_rrc_int(ue_context_pP->ue_context.integrity_algorithm,
+                        ue_context_pP->ue_context.kgnb,
+                        &kRRCint);
+  /* Refresh SRBs/DRBs */
+
+  LOG_D(NR_RRC, "Configuring PDCP DRBs/SRBs for UE %04x\n", ue_context_pP->ue_context.rnti);
+
+  ue_id_t reestablish_ue_id = 0;
+  if (DRB_configList && DRB_configList->list.array[0]->reestablishPDCP && *DRB_configList->list.array[0]->reestablishPDCP == NR_DRB_ToAddMod__reestablishPDCP_true) {
+    for (int i = 0; i < MAX_MOBILES_PER_GNB; i++) {
+      nr_reestablish_rnti_map_t *nr_reestablish_rnti_map = &(RC.nrrrc[ctxt_pP->module_id])->nr_reestablish_rnti_map[i];
+      if (nr_reestablish_rnti_map->ue_id == ctxt_pP->rntiMaybeUEid) {
+        reestablish_ue_id = nr_reestablish_rnti_map[i].c_rnti;
+        LOG_D(NR_RRC, "Removing reestablish_rnti_map[%d] UEid %lx, RNTI %04x\n", i, nr_reestablish_rnti_map->ue_id, nr_reestablish_rnti_map->c_rnti);
+        // clear current C-RNTI from map
+        nr_reestablish_rnti_map->ue_id = 0;
+        nr_reestablish_rnti_map->c_rnti = 0;
+        break;
+      }
+    }
+  }
+
+  nr_pdcp_add_srbs(ctxt_pP->enb_flag, ctxt_pP->rntiMaybeUEid, SRB_configList, (ue_context_pP->ue_context.integrity_algorithm << 4) | ue_context_pP->ue_context.ciphering_algorithm, kRRCenc, kRRCint);
+
+  nr_pdcp_add_drbs(ctxt_pP->enb_flag,
+                   ctxt_pP->rntiMaybeUEid,
+                   reestablish_ue_id,
+                   DRB_configList,
+                   (ue_context_pP->ue_context.integrity_algorithm << 4) | ue_context_pP->ue_context.ciphering_algorithm,
+                   kUPenc,
+                   kUPint,
+                   get_softmodem_params()->sa ? ue_context_pP->ue_context.masterCellGroup->rlc_BearerToAddModList : NULL);
+
   /* Refresh SRBs/DRBs */
   if (!NODE_IS_CU(RC.nrrrc[ctxt_pP->module_id]->node_type)) {
-    LOG_D(NR_RRC,"Configuring RLC DRBs/SRBs for UE %x\n",ue_context_pP->ue_context.rnti);
+    LOG_D(NR_RRC,"Configuring RLC DRBs/SRBs for UE %04x\n",ue_context_pP->ue_context.rnti);
     nr_rrc_rlc_config_asn1_req(ctxt_pP,
                                SRB_configList, // NULL,
                                DRB_configList,
@@ -1201,12 +1287,12 @@ rrc_gNB_process_RRCReconfigurationComplete(
       } else if (SRB_configList->list.array[i]->srb_Identity == 2) {
         ue_context_pP->ue_context.Srb2.Active = 1;
         ue_context_pP->ue_context.Srb2.Srb_info.Srb_id = 2;
-        LOG_I(NR_RRC,"[gNB %d] Frame      %d CC %d : SRB2 is now active\n",
+        LOG_I(NR_RRC,"[gNB %d] Frame %d CC %d: SRB2 is now active\n",
               ctxt_pP->module_id,
               ctxt_pP->frame,
               ue_context_pP->ue_context.primaryCC_id);
       } else {
-        LOG_W(NR_RRC,"[gNB %d] Frame      %d CC %d : invalide SRB identity %ld\n",
+        LOG_W(NR_RRC,"[gNB %d] Frame %d CC %d: invalid SRB identity %ld\n",
               ctxt_pP->module_id,
               ctxt_pP->frame,
               ue_context_pP->ue_context.primaryCC_id,
@@ -1259,7 +1345,7 @@ rrc_gNB_process_RRCReconfigurationComplete(
                                 nr_DRB2LCHAN[i]);*/
           }
 
-          ue_context_pP->ue_context.DRB_active[drb_id] = 0;
+          //ue_context_pP->ue_context.DRB_active[drb_id] = 0;
           LOG_D(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT" RRC_eNB --- MAC_CONFIG_REQ  (DRB) ---> MAC_eNB\n",
                   PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
 
@@ -1291,87 +1377,105 @@ rrc_gNB_process_RRCReconfigurationComplete(
 }
 
 //-----------------------------------------------------------------------------
-void
-rrc_gNB_generate_RRCReestablishment(
-  const protocol_ctxt_t *const ctxt_pP,
-  rrc_gNB_ue_context_t  *const ue_context_pP,
-  const int             CC_id)
+void rrc_gNB_generate_RRCReestablishment(const protocol_ctxt_t *ctxt_pP,
+                                         rrc_gNB_ue_context_t *ue_context_pP,
+                                         const uint8_t *masterCellGroup_from_DU,
+                                         NR_ServingCellConfigCommon_t *scc,
+                                         const int CC_id)
 //-----------------------------------------------------------------------------
 {
-    // int UE_id = -1;
-    //NR_LogicalChannelConfig_t  *SRB1_logicalChannelConfig = NULL;
-    NR_SRB_ToAddModList_t      **SRB_configList;
-    // NR_SRB_ToAddMod_t          *SRB1_config = NULL;
-    //rrc_gNB_carrier_data_t     *carrier = NULL;
-    gNB_RRC_UE_t               *ue_context = NULL;
-    module_id_t                 module_id = ctxt_pP->module_id;
-    // uint16_t                    rnti = ctxt_pP->rnti;
-    gNB_RRC_INST *rrc = RC.nrrrc[ctxt_pP->module_id];
-    
-    SRB_configList = &(ue_context_pP->ue_context.SRB_configList);
-    //carrier = &(RC.nrrrc[ctxt_pP->module_id]->carrier);
-    ue_context = &(ue_context_pP->ue_context);
-    unsigned char buf[1024];
-    int size = do_RRCReestablishment(ctxt_pP,
-        ue_context_pP,
-        CC_id,
-        buf,
-        sizeof(buf),
-        //(uint8_t) carrier->p_gNB, // at this point we do not have the UE capability information, so it can only be TM1 or TM2
-        rrc_gNB_get_next_transaction_identifier(module_id),
-        SRB_configList
-        //&(ue_context->physicalConfigDedicated)
-        );
-
-    /* Configure SRB1 for UE */
-    if (*SRB_configList != NULL) {
-      for (int cnt = 0; cnt < (*SRB_configList)->list.count; cnt++) {
-        if ((*SRB_configList)->list.array[cnt]->srb_Identity == 1) {
-          // SRB1_config = (*SRB_configList)->list.array[cnt];
-        }
+  // int UE_id = -1;
+  // NR_LogicalChannelConfig_t  *SRB1_logicalChannelConfig = NULL;
+  NR_SRB_ToAddModList_t **SRB_configList;
+  // NR_SRB_ToAddMod_t          *SRB1_config = NULL;
+  // rrc_gNB_carrier_data_t     *carrier = NULL;
+  module_id_t module_id = ctxt_pP->module_id;
+  gNB_RRC_INST *rrc = RC.nrrrc[ctxt_pP->module_id];
+  int enable_ciphering = 0;
 
-        LOG_D(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT" RRC_gNB --- MAC_CONFIG_REQ  (SRB1) ---> MAC_gNB\n",
-              PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
+  // Need to drop spCellConfig when there is a RRCReestablishment
+  // Save spCellConfig in spCellConfigReestablishment to recover after Reestablishment is completed
+  ue_context_pP->ue_context.spCellConfigReestablishment = ue_context_pP->ue_context.masterCellGroup->spCellConfig;
+  ue_context_pP->ue_context.masterCellGroup->spCellConfig = NULL;
 
-        // rrc_mac_config_req_eNB
+  SRB_configList = &(ue_context_pP->ue_context.SRB_configList);
+
+  uint8_t buffer[RRC_BUF_SIZE] = {0};
+  int size = do_RRCReestablishment(ctxt_pP,
+                                       ue_context_pP,
+                                       CC_id,
+                                       buffer,
+                                       RRC_BUF_SIZE,
+                                       rrc_gNB_get_next_transaction_identifier(module_id),
+                                       SRB_configList,
+                                       masterCellGroup_from_DU,
+                                       scc,
+                                       &rrc->carrier);
+
+  LOG_I(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT " [RAPROC] Logical Channel DL-DCCH, Generating NR_RRCReestablishment (bytes %d)\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP), size);
+#if (0)
+  /* TODO : It may be needed if gNB goes into full stack working. */
+  UE = find_nr_UE(module_id, rnti);
+  if (UE_id != -1) {
+    /* Activate reject timer, if RRCComplete not received after 10 frames, reject UE */
+    RC.nrmac[module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer = 1;
+    /* Reject UE after 10 frames, NR_RRCReestablishmentReject is triggered */
+    RC.nrmac[module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer_thres = 100;
+  } else {
+    LOG_E(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT " Generating NR_RRCReestablishment without UE_id(MAC) rnti %x\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP), rnti);
+  }
+#endif
+
+  uint8_t *kRRCenc = NULL;
+  uint8_t *kRRCint = NULL;
+  uint8_t *kUPenc = NULL;
+  /* Derive the keys from kgnb */
+  if (SRB_configList != NULL) {
+    nr_derive_key_up_enc(ue_context_pP->ue_context.ciphering_algorithm, ue_context_pP->ue_context.kgnb, &kUPenc);
+  }
+
+  nr_derive_key_rrc_enc(ue_context_pP->ue_context.ciphering_algorithm, ue_context_pP->ue_context.kgnb, &kRRCenc);
+  nr_derive_key_rrc_int(ue_context_pP->ue_context.integrity_algorithm, ue_context_pP->ue_context.kgnb, &kRRCint);
+
+  /* Configure SRB1 for UE */
+  if (*SRB_configList != NULL) {
+    for (int cnt = 0; cnt < (*SRB_configList)->list.count; cnt++) {
+      if ((*SRB_configList)->list.array[cnt]->srb_Identity == 1) {
+        nr_pdcp_add_srbs(ctxt_pP->enb_flag,
+                         ctxt_pP->rntiMaybeUEid,
+                         *SRB_configList,
+                         0,
+                         NULL,
+                         NULL);
       }
-    }  // if (*SRB_configList != NULL)
-    
-    LOG_I(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT" [RAPROC] Logical Channel DL-DCCH, Generating NR_RRCReestablishment (bytes %d)\n",
-          PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-          size);
-#if(0)
-    /* TODO : It may be needed if gNB goes into full stack working. */
-    UE = find_nr_UE(module_id, rnti);
-    if (UE_id != -1) {
-      /* Activate reject timer, if RRCComplete not received after 10 frames, reject UE */
-      RC.nrmac[module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer = 1;
-      /* Reject UE after 10 frames, LTE_RRCConnectionReestablishmentReject is triggered */
-      RC.nrmac[module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer_thres = 100;
-    } else {
-      LOG_E(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT" Generating NR_RRCReestablishment without UE_id(MAC) rnti %x\n",
-            PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-            rnti);
+      LOG_D(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT " RRC_gNB --- MAC_CONFIG_REQ  (SRB1) ---> MAC_gNB\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
     }
-#endif
+  } // if (*SRB_configList != NULL)
 
-  /* correct? specs say RRCReestablishment goes through srb1... */
-  f1ap_dl_rrc_message_t dl_rrc = {
-    .old_gNB_DU_ue_id = 0xFFFFFF,
-    .rrc_container = buf,
-    .rrc_container_length = size,
-    .rnti = ue_context->rnti,
-    .srb_id = CCCH
-  };
-  rrc->mac_rrc.dl_rrc_message_transfer(ctxt_pP->module_id, &dl_rrc);
+  LOG_I(NR_RRC, "Set PDCP security RNTI %04lx nca %ld nia %d in RRCReestablishment\n", ctxt_pP->rntiMaybeUEid, ue_context_pP->ue_context.ciphering_algorithm, ue_context_pP->ue_context.integrity_algorithm);
+  pdcp_config_set_security(
+      ctxt_pP,
+      NULL, /* pdcp_pP not used anymore in NR */
+      DCCH,
+      DCCH + 2,
+      enable_ciphering ? ue_context_pP->ue_context.ciphering_algorithm | (ue_context_pP->ue_context.integrity_algorithm << 4) : 0 | (ue_context_pP->ue_context.integrity_algorithm << 4),
+      kRRCenc,
+      kRRCint,
+      kUPenc);
+
+  if (!NODE_IS_CU(rrc->node_type)) {
+    apply_macrlc_config_reest(rrc, ue_context_pP, ctxt_pP, ctxt_pP->rntiMaybeUEid);
+  }
+
+  nr_rrc_data_req(ctxt_pP, DCCH, rrc_gNB_mui++, SDU_CONFIRM_NO, size, buffer, PDCP_TRANSMISSION_MODE_CONTROL);
 }
 
 //-----------------------------------------------------------------------------
-void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t *const ctxt_pP, const rnti_t reestablish_rnti, rrc_gNB_ue_context_t *ue_context_pP, const uint8_t old_xid)
+void rrc_gNB_process_RRCReestablishmentComplete(const protocol_ctxt_t *const ctxt_pP, const rnti_t reestablish_rnti, rrc_gNB_ue_context_t *ue_context_pP, const uint8_t xid)
 //-----------------------------------------------------------------------------
 {
   LOG_I(NR_RRC,
-        PROTOCOL_RRC_CTXT_UE_FMT" [RAPROC] Logical Channel UL-DCCH, processing NR_RRCConnectionReestablishmentComplete from UE (SRB1 Active)\n",
+        PROTOCOL_RRC_CTXT_UE_FMT" [RAPROC] Logical Channel UL-DCCH, processing NR_RRCReestablishmentComplete from UE (SRB1 Active)\n",
         PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP));
 
   NR_DRB_ToAddModList_t                 *DRB_configList = ue_context_pP->ue_context.DRB_configList;
@@ -1382,15 +1486,13 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
   NR_DRB_ToAddMod_t                     *DRB_config      = NULL;
   //NR_SDAP_Config_t                      *sdap_config     = NULL;
   int i = 0;
-  uint8_t                             buffer[RRC_BUF_SIZE];
-  uint16_t                            size;
 
   uint8_t new_xid = rrc_gNB_get_next_transaction_identifier(ctxt_pP->module_id);
   int ret = 0;
   ue_context_pP->ue_context.StatusRrc = NR_RRC_CONNECTED;
   ue_context_pP->ue_context.ue_rrc_inactivity_timer = 1; // set rrc inactivity when UE goes into RRC_CONNECTED
   ue_context_pP->ue_context.reestablishment_xid = new_xid;
-  SRB_configList2 = &ue_context_pP->ue_context.SRB_configList2[old_xid];
+  SRB_configList2 = &ue_context_pP->ue_context.SRB_configList2[xid];
 
   // get old configuration of SRB2
   if (*SRB_configList2 != NULL) {
@@ -1401,8 +1503,10 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
 
     for (i = 0; (i < (*SRB_configList2)->list.count) && (i < 3); i++) {
       if ((*SRB_configList2)->list.array[i]->srb_Identity == 2 ) {
-        LOG_D(NR_RRC, "get SRB2_config from (ue_context_pP->ue_context.SRB_configList2[%d])\n", old_xid);
+        LOG_D(NR_RRC, "get SRB2_config from (ue_context_pP->ue_context.SRB_configList2[%d])\n", xid);
         SRB2_config = (*SRB_configList2)->list.array[i];
+        SRB2_config->reestablishPDCP = CALLOC(1, sizeof(*SRB2_config->reestablishPDCP));
+        *SRB2_config->reestablishPDCP = NR_SRB_ToAddMod__reestablishPDCP_true;
         break;
       }
     }
@@ -1445,6 +1549,7 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
 
     for (i = 0; (i < DRB_configList->list.count) && (i < 3); i++) {
       DRB_config = DRB_configList->list.array[i];
+      asn1cCallocOne(DRB_config->reestablishPDCP, NR_DRB_ToAddMod__reestablishPDCP_true);
       // Add DRB to DRB configuration list, for LTE_RRCConnectionReconfigurationComplete
       asn1cSeqAdd(&(*DRB_configList2)->list, DRB_config);
     }
@@ -1530,9 +1635,10 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
       ue_context_pP->ue_context.ul_failure_timer = 0;
       return;
     }
-  } 
+  }
 
   /* Update RNTI in ue_context */
+  LOG_I(NR_RRC, "Updating UEid from %04x to %lx\n", ue_context_pP->ue_context.rnti, ctxt_pP->rntiMaybeUEid);
   ue_context_pP->ue_id_rnti = ctxt_pP->rntiMaybeUEid; // here ue_id_rnti is just a key, may be something else
   ue_context_pP->ue_context.rnti = ctxt_pP->rntiMaybeUEid;
 
@@ -1545,35 +1651,54 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
     LOG_D(NR_RRC, "set security successfully \n");
   }
 
+  uint8_t drb_id_to_setup_start = DRB_configList ? DRB_configList->list.array[0]->drb_Identity : 1;
+  uint8_t nb_drb_to_setup = DRB_configList ? DRB_configList->list.count : ue_context_pP->ue_context.nb_of_pdusessions;
+  /* TODO: hardcoded to 13 for the time being, to be changed? */
+  long drb_priority[NGAP_MAX_DRBS_PER_UE] = {13};
+
   /* Add all NAS PDUs to the list */
   for (i = 0; i < ue_context_pP->ue_context.nb_of_pdusessions; i++) {
 
     /* TODO parameters yet to process ... */
     /* TODO should test if pdu session are Ok before! */
     ue_context_pP->ue_context.pduSession[i].status = PDU_SESSION_STATUS_DONE;
-    ue_context_pP->ue_context.pduSession[i].xid = old_xid;
+    ue_context_pP->ue_context.pduSession[i].xid = xid;
     LOG_D(NR_RRC, "setting the status for the default DRB (index %d) to (%d,%s)\n",
           i, ue_context_pP->ue_context.pduSession[i].status, "PDU_SESSION_STATUS_DONE");
   }
 
-  memset(buffer, 0, sizeof(buffer));
+  gNB_RRC_INST *rrc = RC.nrrrc[ctxt_pP->module_id];
+  NR_CellGroupConfig_t *cellGroupConfig = calloc(1, sizeof(NR_CellGroupConfig_t));
 
-  size = do_RRCReconfiguration(ctxt_pP,
-                               buffer,
-                               sizeof(buffer),
-                               old_xid,
-                               *SRB_configList2,
-                               DRB_configList,
-                               NULL,
-                               NULL,
-                               NULL,
-                               NULL, // MeasObj_list,
-                               NULL,
-                               NULL,
-                               NULL,
-                               NULL,
-                               NULL,
-                               NULL);
+  // Revert spCellConfig stored in spCellConfigReestablishment before had been dropped during RRC Reestablishment
+  ue_context_pP->ue_context.masterCellGroup->spCellConfig = ue_context_pP->ue_context.spCellConfigReestablishment;
+  ue_context_pP->ue_context.spCellConfigReestablishment = NULL;
+  cellGroupConfig->spCellConfig = ue_context_pP->ue_context.masterCellGroup->spCellConfig;
+
+  fill_mastercellGroupConfig(cellGroupConfig, ue_context_pP->ue_context.masterCellGroup, rrc->um_on_default_drb, (drb_id_to_setup_start < 2) ? 1 : 0, drb_id_to_setup_start, nb_drb_to_setup, drb_priority);
+
+  for(i = 0; i < cellGroupConfig->rlc_BearerToAddModList->list.count; i++) {
+    cellGroupConfig->rlc_BearerToAddModList->list.array[i]->reestablishRLC = CALLOC(1, sizeof(*cellGroupConfig->rlc_BearerToAddModList->list.array[i]->reestablishRLC));
+    *cellGroupConfig->rlc_BearerToAddModList->list.array[i]->reestablishRLC = NR_RLC_BearerConfig__reestablishRLC_true;
+  }
+
+  uint8_t buffer[RRC_BUF_SIZE] = {0};
+  int size = do_RRCReconfiguration(ctxt_pP,
+                                       buffer,
+                                       RRC_BUF_SIZE,
+                                       new_xid,
+                                       *SRB_configList2,
+                                       DRB_configList,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL, // MeasObj_list,
+                                       NULL,
+                                       ue_context_pP,
+                                       &rrc->carrier,
+                                       NULL,
+                                       NULL,
+                                       cellGroupConfig);
 
   LOG_DUMPMSG(NR_RRC,DEBUG_RRC,(char *)buffer,size, "[MSG] RRC Reconfiguration\n");
 
@@ -1586,24 +1711,35 @@ void rrc_gNB_process_RRCConnectionReestablishmentComplete(const protocol_ctxt_t
     }
   }
 
-  if(size==65535) {
-    LOG_E(NR_RRC,"RRC decode err!!! do_RRCReconfiguration\n");
+  if (size < 0) {
+    LOG_E(NR_RRC, "RRC decode err!!! do_RRCReconfiguration\n");
     return;
   } else {
-    LOG_I(NR_RRC,
-          "[gNB %d] Frame %d, Logical Channel DL-DCCH, Generate NR_RRCConnectionReconfiguration (bytes %d, UE id %x)\n",
-          ctxt_pP->module_id, ctxt_pP->frame, size, ue_context_pP->ue_context.rnti);
+    LOG_I(NR_RRC, "[gNB %d] Frame %d, Logical Channel DL-DCCH, Generate NR_RRCReconfiguration (bytes %d, UE id %04x)\n", ctxt_pP->module_id, ctxt_pP->frame, size, ue_context_pP->ue_context.rnti);
     LOG_D(NR_RRC,
-          "[FRAME %05d][RRC_gNB][MOD %u][][--- PDCP_DATA_REQ/%d Bytes (rrcConnectionReconfiguration to UE %x MUI %d) --->][PDCP][MOD %u][RB %u]\n",
-          ctxt_pP->frame, ctxt_pP->module_id, size, ue_context_pP->ue_context.rnti, rrc_gNB_mui, ctxt_pP->module_id, DCCH);
-    nr_rrc_data_req(
-      ctxt_pP,
-      DCCH,
-      rrc_gNB_mui++,
-      SDU_CONFIRM_NO,
-      size,
-      buffer,
-      PDCP_TRANSMISSION_MODE_CONTROL);
+          "[FRAME %05d][RRC_gNB][MOD %u][][--- PDCP_DATA_REQ/%d Bytes (RRCReconfiguration to UE %04x MUI %d) --->][PDCP][MOD %u][RB %u]\n",
+          ctxt_pP->frame,
+          ctxt_pP->module_id,
+          size,
+          ue_context_pP->ue_context.rnti,
+          rrc_gNB_mui,
+          ctxt_pP->module_id,
+          DCCH);
+
+    gNB_RrcConfigurationReq *configuration = &RC.nrrrc[ctxt_pP->module_id]->configuration;
+    rrc_mac_config_req_gNB(ctxt_pP->module_id,
+                           configuration->pdsch_AntennaPorts,
+                           configuration->pusch_AntennaPorts,
+                           configuration->sib1_tda,
+                           configuration->minRXTXTIME,
+                           NULL,
+                           NULL,
+                           NULL,
+                           0,
+                           ue_context_pP->ue_context.rnti,
+                           cellGroupConfig);
+
+    nr_rrc_data_req(ctxt_pP, DCCH, rrc_gNB_mui++, SDU_CONFIRM_NO, size, buffer, PDCP_TRANSMISSION_MODE_CONTROL);
   }
 
   if (NODE_IS_DU(RC.nrrrc[ctxt_pP->module_id]->node_type) || NODE_IS_MONOLITHIC(RC.nrrrc[ctxt_pP->module_id]->node_type)) {
@@ -1626,8 +1762,6 @@ int nr_rrc_reconfiguration_req(rrc_gNB_ue_context_t         *const ue_context_pP
                                const int                    dl_bwp_id,
                                const int                    ul_bwp_id) {
 
-  uint8_t buffer[RRC_BUF_SIZE];
-  memset(buffer, 0, sizeof(buffer));
   uint8_t xid = rrc_gNB_get_next_transaction_identifier(ctxt_pP->module_id);
 
   NR_CellGroupConfig_t *masterCellGroup = ue_context_pP->ue_context.masterCellGroup;
@@ -1639,22 +1773,23 @@ int nr_rrc_reconfiguration_req(rrc_gNB_ue_context_t         *const ue_context_pP
     *masterCellGroup->spCellConfig->spCellConfigDedicated->uplinkConfig->firstActiveUplinkBWP_Id = ul_bwp_id;
   }
 
-  uint16_t  size = do_RRCReconfiguration(ctxt_pP,
-                                         buffer,
-                                         sizeof(buffer),
-                                         xid,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         ue_context_pP,
-                                         NULL,
-                                         NULL,
-                                         NULL,
-                                         masterCellGroup);
+  uint8_t buffer[RRC_BUF_SIZE];
+  int size = do_RRCReconfiguration(ctxt_pP,
+                                       buffer,
+                                       RRC_BUF_SIZE,
+                                       xid,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       ue_context_pP,
+                                       NULL,
+                                       NULL,
+                                       NULL,
+                                       masterCellGroup);
 
   gNB_RrcConfigurationReq *configuration = &RC.nrrrc[ctxt_pP->module_id]->configuration;
   rrc_mac_config_req_gNB(ctxt_pP->module_id,
@@ -1836,160 +1971,131 @@ int nr_rrc_gNB_decode_ccch(protocol_ctxt_t    *const ctxt_pP,
                 LOG_I(NR_RRC, "receive rrcResumeRequest message \n");
         break;
 
-      case NR_UL_CCCH_MessageType__c1_PR_rrcReestablishmentRequest:
-        LOG_I(NR_RRC, "receive rrcReestablishmentRequest message \n");
-        LOG_DUMPMSG(NR_RRC, DEBUG_RRC,(char *)(buffer), buffer_length,
-              "[MSG] RRC Reestablishment Request\n");
-        LOG_D(NR_RRC,
-              PROTOCOL_NR_RRC_CTXT_UE_FMT"MAC_gNB--- MAC_DATA_IND (rrcReestablishmentRequest on SRB0) --> RRC_gNB\n",
-              PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
+      case NR_UL_CCCH_MessageType__c1_PR_rrcReestablishmentRequest: {
+        LOG_I(NR_RRC, "Received rrcReestablishmentRequest message\n");
+        LOG_DUMPMSG(NR_RRC, DEBUG_RRC, (char *)(buffer), buffer_length, "[MSG] RRC Reestablishment Request\n");
+        LOG_D(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT "MAC_gNB--- MAC_DATA_IND (rrcReestablishmentRequest on SRB0) --> RRC_gNB\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
 
         rrcReestablishmentRequest = ul_ccch_msg->message.choice.c1->choice.rrcReestablishmentRequest->rrcReestablishmentRequest;
         LOG_I(NR_RRC,
-          PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest cause %s\n",
-          PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-          ((rrcReestablishmentRequest.reestablishmentCause == NR_ReestablishmentCause_otherFailure) ?   "Other Failure" :
-          (rrcReestablishmentRequest.reestablishmentCause == NR_ReestablishmentCause_handoverFailure) ? "Handover Failure" :
-          "reconfigurationFailure"));
-        {
-          uint16_t                          c_rnti = 0;
-          if (rrcReestablishmentRequest.ue_Identity.physCellId != RC.nrrrc[ctxt_pP->module_id]->carrier.physCellId) {
-            /* UE was moving from previous cell so quickly that RRCReestablishment for previous cell was recieved in this cell */
-            LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest ue_Identity.physCellId(%ld) is not equal to current physCellId(%d), fallback to RRC establishment\n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-                  rrcReestablishmentRequest.ue_Identity.physCellId,
-                  RC.nrrrc[ctxt_pP->module_id]->carrier.physCellId);
-            rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
-            break;
-          }
+              PROTOCOL_NR_RRC_CTXT_UE_FMT " NR_RRCReestablishmentRequest cause %s\n",
+              PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
+              ((rrcReestablishmentRequest.reestablishmentCause == NR_ReestablishmentCause_otherFailure)      ? "Other Failure"
+               : (rrcReestablishmentRequest.reestablishmentCause == NR_ReestablishmentCause_handoverFailure) ? "Handover Failure"
+                                                                                                             : "reconfigurationFailure"));
+
+        if (rrcReestablishmentRequest.ue_Identity.physCellId != RC.nrrrc[ctxt_pP->module_id]->carrier.physCellId) {
+          /* UE was moving from previous cell so quickly that RRCReestablishment for previous cell was received in this cell */
+          LOG_E(NR_RRC,
+                PROTOCOL_NR_RRC_CTXT_UE_FMT " NR_RRCReestablishmentRequest ue_Identity.physCellId(%ld) is not equal to current physCellId(%d), fallback to RRC establishment\n",
+                PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
+                rrcReestablishmentRequest.ue_Identity.physCellId,
+                RC.nrrrc[ctxt_pP->module_id]->carrier.physCellId);
+          rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
+          break;
+        }
 
-          LOG_D(NR_RRC, "physCellId is %ld\n", rrcReestablishmentRequest.ue_Identity.physCellId);
+        LOG_I(NR_RRC, "physCellId: %ld\n", rrcReestablishmentRequest.ue_Identity.physCellId);
 
-          for (int i = 0; i < rrcReestablishmentRequest.ue_Identity.shortMAC_I.size; i++) {
-            LOG_D(NR_RRC, "rrcReestablishmentRequest.ue_Identity.shortMAC_I.buf[%d] = %x\n",
-                  i, rrcReestablishmentRequest.ue_Identity.shortMAC_I.buf[i]);
-          }
+        for (int i = 0; i < rrcReestablishmentRequest.ue_Identity.shortMAC_I.size; i++) {
+          LOG_D(NR_RRC, "rrcReestablishmentRequest.ue_Identity.shortMAC_I.buf[%d] = %x\n", i, rrcReestablishmentRequest.ue_Identity.shortMAC_I.buf[i]);
+        }
 
-          if (rrcReestablishmentRequest.ue_Identity.c_RNTI < 0 ||
-              rrcReestablishmentRequest.ue_Identity.c_RNTI > 65535) {
-            /* c_RNTI range error should not happen */
-            LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest c_RNTI range error, fallback to RRC establishment\n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
-            rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
-            break;
-          }
+        // 3GPP TS 38.321 version 15.13.0 Section 7.1 Table 7.1-1: RNTI values
+        if (rrcReestablishmentRequest.ue_Identity.c_RNTI < 0x1 || rrcReestablishmentRequest.ue_Identity.c_RNTI > 0xffef) {
+          /* c_RNTI range error should not happen */
+          LOG_E(NR_RRC, "NR_RRCReestablishmentRequest c_RNTI range error, fallback to RRC establishment\n");
+          rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
+          break;
+        }
 
-          c_rnti = rrcReestablishmentRequest.ue_Identity.c_RNTI;
-          LOG_I(NR_RRC, "c_rnti is %x\n", c_rnti);
-          ue_context_p = rrc_gNB_get_ue_context(gnb_rrc_inst, c_rnti);
+        rnti_t c_rnti = rrcReestablishmentRequest.ue_Identity.c_RNTI;
+        LOG_I(NR_RRC, "c_RNTI: %04x\n", c_rnti);
+        ue_context_p = rrc_gNB_get_ue_context(gnb_rrc_inst, c_rnti);
+        if (ue_context_p == NULL) {
+          LOG_E(NR_RRC, "NR_RRCReestablishmentRequest without UE context, fallback to RRC establishment\n");
+          rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
+          break;
+        }
 
-          if (ue_context_p == NULL) {
-            LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest without UE context, fallback to RRC establishment\n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
-            rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
-            break;
-          }
-#if(0)
-          /* TODO : It may be needed if gNB goes into full stack working. */
-          int UE_id = find_nr_UE_id(ctxt_pP->module_id, c_rnti);
+#if (0)
+        /* TODO : It may be needed if gNB goes into full stack working. */
+        int UE_id = find_nr_UE_id(ctxt_pP->module_id, c_rnti);
 
-          if(UE_id == -1) {
-            LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest without UE_id(MAC) rnti %x, fallback to RRC establishment\n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),c_rnti);
-            rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
-            break;
-          }
+        if (UE_id == -1) {
+          LOG_E(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT " NR_RRCReestablishmentRequest without UE_id(MAC) rnti %x, fallback to RRC establishment\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP), c_rnti);
+          rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
+          break;
+        }
 
-          //previous rnti
-          rnti_t previous_rnti = 0;
+        // previous rnti
+        rnti_t previous_rnti = 0;
 
-          for (int i = 0; i < MAX_MOBILES_PER_ENB; i++) {
-            if (reestablish_rnti_map[i][1] == c_rnti) {
-              previous_rnti = reestablish_rnti_map[i][0];
-              break;
-            }
+        for (int i = 0; i < MAX_MOBILES_PER_ENB; i++) {
+          if (reestablish_rnti_map[i][1] == c_rnti) {
+            previous_rnti = reestablish_rnti_map[i][0];
+            break;
           }
+        }
 
-          if(previous_rnti != 0) {
-            UE_id = find_nr_UE_id(ctxt_pP->module_id, previous_rnti);
+        if (previous_rnti != 0) {
+          UE_id = find_nr_UE_id(ctxt_pP->module_id, previous_rnti);
 
-            if(UE_id == -1) {
-              LOG_E(NR_RRC,
-                    PROTOCOL_NR_RRC_CTXT_UE_FMT" RRCReestablishmentRequest without UE_id(MAC) previous rnti %x, fallback to RRC establishment\n",
-                    PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),previous_rnti);
-              rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
-              break;
-            }
-          }
-#endif
-          //c-plane not end
-          if((ue_context_p->ue_context.StatusRrc != NR_RRC_RECONFIGURED) && (ue_context_p->ue_context.reestablishment_cause == NR_ReestablishmentCause_spare1)) {
+          if (UE_id == -1) {
             LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCReestablishmentRequest (UE %x c-plane is not end), RRC establishment failed \n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),c_rnti);
-            /* TODO RRC Release ? */
+                  PROTOCOL_NR_RRC_CTXT_UE_FMT " RRCReestablishmentRequest without UE_id(MAC) previous rnti %x, fallback to RRC establishment\n",
+                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
+                  previous_rnti);
+            rrc_gNB_generate_RRCSetup_for_RRCReestablishmentRequest(ctxt_pP, 0);
             break;
           }
+        }
+#endif
+        // c-plane not end
+        if ((ue_context_p->ue_context.StatusRrc != NR_RRC_RECONFIGURED) && (ue_context_p->ue_context.reestablishment_cause == NR_ReestablishmentCause_spare1)) {
+          LOG_E(NR_RRC, "NR_RRCReestablishmentRequest (UE %x c-plane is not end), RRC establishment failed\n", c_rnti);
+          /* TODO RRC Release ? */
+          break;
+        }
 
-          if(ue_context_p->ue_context.ue_reestablishment_timer > 0) {
-            LOG_E(NR_RRC,
-                  PROTOCOL_NR_RRC_CTXT_UE_FMT" RRRCReconfigurationComplete(Previous) don't receive, delete the Previous UE,\nprevious Status %d, new Status NR_RRC_RECONFIGURED\n",
-                  PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-                  ue_context_p->ue_context.StatusRrc
-                  );
-            ue_context_p->ue_context.StatusRrc = NR_RRC_RECONFIGURED;
-            protocol_ctxt_t  ctxt_old_p;
-            PROTOCOL_CTXT_SET_BY_INSTANCE(&ctxt_old_p,
-                                          ctxt_pP->instance,
-                                          GNB_FLAG_YES,
-                                          c_rnti,
-                                          ctxt_pP->frame,
-                                          ctxt_pP->subframe);
-            rrc_gNB_process_RRCReconfigurationComplete(&ctxt_old_p,
-                ue_context_p,
-                ue_context_p->ue_context.reestablishment_xid);
+        if (ue_context_p->ue_context.ue_reestablishment_timer > 0) {
+          LOG_E(NR_RRC, "RRRCReconfigurationComplete(Previous) don't receive, delete the Previous UE,\nprevious Status %d, new Status NR_RRC_RECONFIGURED\n", ue_context_p->ue_context.StatusRrc);
+          ue_context_p->ue_context.StatusRrc = NR_RRC_RECONFIGURED;
+          protocol_ctxt_t ctxt_old_p;
+          PROTOCOL_CTXT_SET_BY_INSTANCE(&ctxt_old_p, ctxt_pP->instance, GNB_FLAG_YES, c_rnti, ctxt_pP->frame, ctxt_pP->subframe);
+          rrc_gNB_process_RRCReconfigurationComplete(&ctxt_old_p, ue_context_p, ue_context_p->ue_context.reestablishment_xid);
 
-            for (uint8_t pdusessionid = 0; pdusessionid < ue_context_p->ue_context.nb_of_pdusessions; pdusessionid++) {
-              if (ue_context_p->ue_context.pduSession[pdusessionid].status == PDU_SESSION_STATUS_DONE) {
-                ue_context_p->ue_context.pduSession[pdusessionid].status = PDU_SESSION_STATUS_ESTABLISHED;
-              } else {
-                ue_context_p->ue_context.pduSession[pdusessionid].status = PDU_SESSION_STATUS_FAILED;
-              }
+          for (uint8_t pdusessionid = 0; pdusessionid < ue_context_p->ue_context.nb_of_pdusessions; pdusessionid++) {
+            if (ue_context_p->ue_context.pduSession[pdusessionid].status == PDU_SESSION_STATUS_DONE) {
+              ue_context_p->ue_context.pduSession[pdusessionid].status = PDU_SESSION_STATUS_ESTABLISHED;
+            } else {
+              ue_context_p->ue_context.pduSession[pdusessionid].status = PDU_SESSION_STATUS_FAILED;
             }
           }
+        }
 
-          LOG_D(NR_RRC,
-                PROTOCOL_NR_RRC_CTXT_UE_FMT" UE context: %p\n",
-                PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-                ue_context_p);
-          /* reset timers */
-          ue_context_p->ue_context.ul_failure_timer = 0;
-          ue_context_p->ue_context.ue_release_timer = 0;
-          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;
-          ue_context_p->ue_context.reestablishment_xid = -1;
-
-          // insert C-RNTI to map
-          for (int i = 0; i < MAX_MOBILES_PER_ENB; i++) {
-            if (reestablish_rnti_map[i][0] == 0) {
-              reestablish_rnti_map[i][0] = ctxt_pP->rntiMaybeUEid;
-              reestablish_rnti_map[i][1] = c_rnti;
-              LOG_D(NR_RRC, "reestablish_rnti_map[%d] [0] %x, [1] %x\n",
-                    i, reestablish_rnti_map[i][0], reestablish_rnti_map[i][1]);
-              break;
-            }
+        /* reset timers */
+        ue_context_p->ue_context.ul_failure_timer = 0;
+        ue_context_p->ue_context.ue_release_timer = 0;
+        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;
+        ue_context_p->ue_context.reestablishment_xid = -1;
+
+        // Insert C-RNTI to map
+        for (int i = 0; i < MAX_MOBILES_PER_GNB; i++) {
+          nr_reestablish_rnti_map_t *nr_reestablish_rnti_map = &(RC.nrrrc[ctxt_pP->module_id])->nr_reestablish_rnti_map[i];
+          LOG_I(NR_RRC, "Insert nr_reestablish_rnti_map[%d] UEid: %lx, RNTI: %04x\n", i, nr_reestablish_rnti_map->ue_id, nr_reestablish_rnti_map->c_rnti);
+          if (nr_reestablish_rnti_map->ue_id == 0) {
+            nr_reestablish_rnti_map->ue_id = ctxt_pP->rntiMaybeUEid;
+            nr_reestablish_rnti_map->c_rnti = c_rnti;
+            LOG_I(NR_RRC, "Insert nr_reestablish_rnti_map[%d] UEid: %lx, RNTI: %04x\n", i, nr_reestablish_rnti_map->ue_id, nr_reestablish_rnti_map->c_rnti);
+            break;
           }
+        }
 
-          ue_context_p->ue_context.reestablishment_cause = rrcReestablishmentRequest.reestablishmentCause;
-          LOG_D(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT" Accept reestablishment request from UE physCellId %ld cause %ld\n",
-                PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP),
-                rrcReestablishmentRequest.ue_Identity.physCellId,
-                ue_context_p->ue_context.reestablishment_cause);
+        ue_context_p->ue_context.reestablishment_cause = rrcReestablishmentRequest.reestablishmentCause;
+        LOG_D(NR_RRC, "Accept RRCReestablishmentRequest from UE physCellId %ld cause %ld\n", rrcReestablishmentRequest.ue_Identity.physCellId, ue_context_p->ue_context.reestablishment_cause);
 
           ue_context_p->ue_context.primaryCC_id = 0;
           //LG COMMENT Idx = (ue_mod_idP * NB_RB_MAX) + DCCH;
@@ -2005,11 +2111,10 @@ int nr_rrc_gNB_decode_ccch(protocol_ctxt_t    *const ctxt_pP,
           rrc_init_nr_srb_param(&ue_context_p->ue_context.Srb2.Srb_info.Lchan_desc[0]);
           rrc_init_nr_srb_param(&ue_context_p->ue_context.Srb2.Srb_info.Lchan_desc[1]);
 
-          rrc_gNB_generate_RRCReestablishment(ctxt_pP, ue_context_p, 0);
+        rrc_gNB_generate_RRCReestablishment(ctxt_pP, ue_context_p, du_to_cu_rrc_container, gnb_rrc_inst->carrier.servingcellconfigcommon, 0);
 
-          LOG_I(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT "CALLING RLC CONFIG SRB1 (rbid %d)\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP), Idx);
-        }
-        break;
+        LOG_I(NR_RRC, "CALLING RLC CONFIG SRB1 (rbid %d)\n", Idx);
+      } break;
 
       case NR_UL_CCCH_MessageType__c1_PR_rrcSystemInfoRequest:
         LOG_I(NR_RRC, "receive rrcSystemInfoRequest message \n");
@@ -2178,15 +2283,17 @@ rrc_gNB_decode_dcch(
                 ue_context_p->ue_context.gnb_gtp_psi[i]  = 0;
               }
             }
-	    gtpv1u_delete_ngu_tunnel(ctxt_pP->instance, &req);
+            gtpv1u_delete_ngu_tunnel(ctxt_pP->instance, &req);
             //NGAP_PDUSESSION_RELEASE_RESPONSE
             rrc_gNB_send_NGAP_PDUSESSION_RELEASE_RESPONSE(ctxt_pP, ue_context_p, xid);
           } else if (ue_context_p->ue_context.established_pdu_sessions_flag != 1) {
-            if (ue_context_p->ue_context.nb_of_pdusessions > 0) {
-              rrc_gNB_send_NGAP_PDUSESSION_SETUP_RESP(ctxt_pP,
-                ue_context_p,
-                ul_dcch_msg->message.choice.c1->choice.rrcReconfigurationComplete->rrc_TransactionIdentifier);
-              ue_context_p->ue_context.nb_of_pdusessions = 0;
+            if (ue_context_p->ue_context.reestablishment_xid < 0) {
+              if (ue_context_p->ue_context.nb_of_pdusessions > 0) {
+                rrc_gNB_send_NGAP_PDUSESSION_SETUP_RESP(ctxt_pP, ue_context_p, ul_dcch_msg->message.choice.c1->choice.rrcReconfigurationComplete->rrc_TransactionIdentifier);
+                ue_context_p->ue_context.nb_of_pdusessions = 0;
+              }
+            } else {
+              ue_context_p->ue_context.reestablishment_xid = -1;
             }
           }
           if (ue_context_p->ue_context.nb_of_modify_pdusessions > 0) {
@@ -2528,68 +2635,61 @@ rrc_gNB_decode_dcch(
 
       break;
 
-            case NR_UL_DCCH_MessageType__c1_PR_rrcReestablishmentComplete:
-              LOG_DUMPMSG(NR_RRC,DEBUG_RRC,(char *)Rx_sdu,sdu_sizeP,
-                          "[MSG] NR_RRC Connection Reestablishment Complete\n");
-              LOG_I(NR_RRC,
-                    PROTOCOL_RRC_CTXT_UE_FMT" RLC RB %02d --- RLC_DATA_IND %d bytes "
-                    "(rrcConnectionReestablishmentComplete) ---> RRC_gNB\n",
-                    PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP),
-                    DCCH,
-                    sdu_sizeP);
-              {
-                rnti_t reestablish_rnti = 0;
-
-                // select C-RNTI from map
-                for (i = 0; i < MAX_MOBILES_PER_ENB; i++) {
-                  if (reestablish_rnti_map[i][0] == ctxt_pP->rntiMaybeUEid) {
-                    reestablish_rnti = reestablish_rnti_map[i][1];
-                    ue_context_p = rrc_gNB_get_ue_context(
-                                     RC.nrrrc[ctxt_pP->module_id],
-                                     reestablish_rnti);
-                    // clear currentC-RNTI from map
-                    reestablish_rnti_map[i][0] = 0;
-                    reestablish_rnti_map[i][1] = 0;
-                    LOG_D(NR_RRC, "reestablish_rnti_map[%d] [0] %x, [1] %x\n",
-                          i, reestablish_rnti_map[i][0], reestablish_rnti_map[i][1]);
-                    break;
-                  }
-                }
-
-                if (!ue_context_p) {
-                  LOG_E(NR_RRC,
-                        PROTOCOL_NR_RRC_CTXT_UE_FMT" NR_RRCConnectionReestablishmentComplete without UE context, falt\n",
-                        PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
-                  break;
-                }
-
-#if(0)
-                /* TODO : It may be needed if gNB goes into full stack working. */
-                //clear
-                int UE_id = find_nr_UE_id(ctxt_pP->module_id, ctxt_pP->rntiMaybeUEid);
-
-                if(UE_id == -1) {
-                  LOG_E(NR_RRC, PROTOCOL_RRC_CTXT_UE_FMT " NR_RRCConnectionReestablishmentComplete without UE_id(MAC) rnti %lx, fault\n", PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP), ctxt_pP->rntiMaybeUEid);
-                  break;
-                }
-
-                RC.nrmac[ctxt_pP->module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer = 0;
+      case NR_UL_DCCH_MessageType__c1_PR_rrcReestablishmentComplete: {
+        LOG_DUMPMSG(NR_RRC, DEBUG_RRC, (char *)Rx_sdu, sdu_sizeP, "[MSG] NR_RRC Connection Reestablishment Complete\n");
+        LOG_I(NR_RRC,
+              PROTOCOL_RRC_CTXT_UE_FMT
+              " RLC RB %02d --- RLC_DATA_IND %d bytes "
+              "(rrcReestablishmentComplete) ---> RRC_gNB\n",
+              PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP),
+              DCCH,
+              sdu_sizeP);
+
+        rnti_t reestablish_rnti = 0;
+
+        //  Select C-RNTI from map
+        for (i = 0; i < MAX_MOBILES_PER_GNB; i++) {
+          nr_reestablish_rnti_map_t *nr_reestablish_rnti_map = &(RC.nrrrc[ctxt_pP->module_id])->nr_reestablish_rnti_map[i];
+          LOG_I(NR_RRC, "nr_reestablish_rnti_map[%d] UEid %lx, RNTI %04x, ctxt_pP->rntiMaybeUEid: %lx\n", i, nr_reestablish_rnti_map->ue_id, nr_reestablish_rnti_map->c_rnti, ctxt_pP->rntiMaybeUEid);
+          if (nr_reestablish_rnti_map->ue_id == ctxt_pP->rntiMaybeUEid) {
+          LOG_I(NR_RRC, "Removing nr_reestablish_rnti_map[%d] UEid %lx, RNTI %04x\n", i, nr_reestablish_rnti_map->ue_id, nr_reestablish_rnti_map->c_rnti);
+          reestablish_rnti = nr_reestablish_rnti_map->c_rnti;
+          ue_context_p = rrc_gNB_get_ue_context(RC.nrrrc[ctxt_pP->module_id], reestablish_rnti);
+          break;
+          }
+        }
+
+        if (!ue_context_p) {
+          LOG_E(NR_RRC, PROTOCOL_NR_RRC_CTXT_UE_FMT " NR_RRCReestablishmentComplete without UE context, falt\n", PROTOCOL_NR_RRC_CTXT_UE_ARGS(ctxt_pP));
+          break;
+        }
+
+#if (0)
+        /* TODO : It may be needed if gNB goes into full stack working. */
+        // clear
+        int UE_id = find_nr_UE_id(ctxt_pP->module_id, ctxt_pP->rntiMaybeUEid);
+
+        if (UE_id == -1) {
+          LOG_E(NR_RRC, PROTOCOL_RRC_CTXT_UE_FMT " NR_RRCReestablishmentComplete without UE_id(MAC) rnti %lx, fault\n", PROTOCOL_RRC_CTXT_UE_ARGS(ctxt_pP), ctxt_pP->rntiMaybeUEid);
+          break;
+        }
+
+        RC.nrmac[ctxt_pP->module_id]->UE_info.UE_sched_ctrl[UE_id].ue_reestablishment_reject_timer = 0;
 #endif
-                ue_context_p->ue_context.reestablishment_xid = -1;
 
-                if (ul_dcch_msg->message.choice.c1->choice.rrcReestablishmentComplete->criticalExtensions.present ==
-                    NR_RRCReestablishmentComplete__criticalExtensions_PR_rrcReestablishmentComplete) {
-                  rrc_gNB_process_RRCConnectionReestablishmentComplete(ctxt_pP, reestablish_rnti, ue_context_p,
-                      ul_dcch_msg->message.choice.c1->choice.rrcReestablishmentComplete->rrc_TransactionIdentifier);
+        if (ul_dcch_msg->message.choice.c1->choice.rrcReestablishmentComplete->criticalExtensions.present == NR_RRCReestablishmentComplete__criticalExtensions_PR_rrcReestablishmentComplete) {
+          rrc_gNB_process_RRCReestablishmentComplete(ctxt_pP, reestablish_rnti, ue_context_p, ul_dcch_msg->message.choice.c1->choice.rrcReestablishmentComplete->rrc_TransactionIdentifier);
 
-                }
+          gNB_MAC_INST *nrmac = RC.nrmac[ctxt_pP->module_id]; // WHAT A BEAUTIFULL RACE CONDITION !!!
+          mac_remove_nr_ue(nrmac, reestablish_rnti);
+        }
+
+        // ue_context_p->ue_context.ue_release_timer = 0;
+        ue_context_p->ue_context.ue_reestablishment_timer = 1;
+        // remove UE after 100 frames after NR_RRCReestablishmentRelease is triggered
+        ue_context_p->ue_context.ue_reestablishment_timer_thres = 1000;
+      } break;
 
-                //ue_context_p->ue_context.ue_release_timer = 0;
-                ue_context_p->ue_context.ue_reestablishment_timer = 1;
-                // remove UE after 100 frames after LTE_RRCConnectionReestablishmentRelease is triggered
-                ue_context_p->ue_context.ue_reestablishment_timer_thres = 1000;
-              }
-              break;
       default:
         break;
     }
@@ -4152,13 +4252,8 @@ rrc_gNB_generate_RRCRelease(
 )
 //-----------------------------------------------------------------------------
 {
-  uint8_t buffer[RRC_BUF_SIZE];
-  uint16_t size = 0;
-
-  memset(buffer, 0, sizeof(buffer));
-
-  size = do_NR_RRCRelease(buffer, sizeof(buffer),
-                          rrc_gNB_get_next_transaction_identifier(ctxt_pP->module_id));
+  uint8_t buffer[RRC_BUF_SIZE] = {0};
+  int size = do_NR_RRCRelease(buffer, RRC_BUF_SIZE, rrc_gNB_get_next_transaction_identifier(ctxt_pP->module_id));
   ue_context_pP->ue_context.ue_reestablishment_timer = 0;
   ue_context_pP->ue_context.ue_release_timer = 0;
   ue_context_pP->ue_context.ul_failure_timer = 0;
diff --git a/openair2/RRC/NR/rrc_gNB_UE_context.c b/openair2/RRC/NR/rrc_gNB_UE_context.c
index ec96a446c2a643123765b007adb2ac8df02e400e..87b4de23b2fc1cc27c7492f7d387d1d58d38ff89 100644
--- a/openair2/RRC/NR/rrc_gNB_UE_context.c
+++ b/openair2/RRC/NR/rrc_gNB_UE_context.c
@@ -189,7 +189,7 @@ rrc_gNB_ue_context_5g_s_tmsi_exist(
 {
     struct rrc_gNB_ue_context_s        *ue_context_p = NULL;
     RB_FOREACH(ue_context_p, rrc_nr_ue_tree_s, &rrc_instance_pP->rrc_ue_head) {
-        LOG_I(NR_RRC,"checking for UE 5G S-TMSI %ld: rnti %d \n",
+        LOG_I(NR_RRC,"Checking for UE 5G S-TMSI %ld: RNTI %04x\n",
               s_TMSI, ue_context_p->ue_context.rnti);
 
         if (ue_context_p->ue_context.ng_5G_S_TMSI_Part1 == s_TMSI) {
diff --git a/openair2/RRC/NR/rrc_gNB_nsa.c b/openair2/RRC/NR/rrc_gNB_nsa.c
index 379e4d8bdc70f3afc37f96f8944f0a45e4a1fc1d..df307535956effafb210005154f6b1f9f5d59d21 100644
--- a/openair2/RRC/NR/rrc_gNB_nsa.c
+++ b/openair2/RRC/NR/rrc_gNB_nsa.c
@@ -386,6 +386,7 @@ void rrc_add_nsa_user(gNB_RRC_INST *rrc,struct rrc_gNB_ue_context_s *ue_context_
 
   nr_pdcp_add_drbs(ctxt.enb_flag,
                    ctxt.rntiMaybeUEid,
+                   0,
                    ue_context_p->ue_context.rb_config->drb_ToAddModList,
                    (ue_context_p->ue_context.integrity_algorithm << 4) | ue_context_p->ue_context.ciphering_algorithm,
                    kUPenc,
diff --git a/openair2/RRC/NR_UE/rrc_UE.c b/openair2/RRC/NR_UE/rrc_UE.c
index b09b09efb399d751d8e93f7be082a7153140b37a..9c6174fb53bd47bd95217fa770d5496bce96dfbd 100644
--- a/openair2/RRC/NR_UE/rrc_UE.c
+++ b/openair2/RRC/NR_UE/rrc_UE.c
@@ -2062,6 +2062,7 @@ nr_rrc_ue_establish_srb2(
        // Refresh DRBs
      nr_pdcp_add_drbs(ctxt_pP->enb_flag,
                       ctxt_pP->rntiMaybeUEid,
+                      0,
                       radioBearerConfig->drb_ToAddModList,
                       NR_UE_rrc_inst[ctxt_pP->module_id].cipheringAlgorithm | (NR_UE_rrc_inst[ctxt_pP->module_id].integrityProtAlgorithm << 4),
                       kUPenc,
@@ -2596,12 +2597,11 @@ nr_rrc_ue_generate_rrcReestablishmentComplete(
 )
 //-----------------------------------------------------------------------------
 {
-    uint32_t length;
-    uint8_t buffer[100];
-    length = do_RRCReestablishmentComplete(buffer, sizeof(buffer),
+    uint8_t buffer[RRC_BUFFER_SIZE] = {0};
+    int size = do_RRCReestablishmentComplete(buffer, RRC_BUFFER_SIZE,
                                            rrcReestablishment->rrc_TransactionIdentifier);
     LOG_I(NR_RRC,"[UE %d][RAPROC] Frame %d : Logical Channel UL-DCCH (SRB1), Generating RRCReestablishmentComplete (bytes%d, gNB %d)\n",
-          ctxt_pP->module_id,ctxt_pP->frame, length, gNB_index);
+          ctxt_pP->module_id,ctxt_pP->frame, size, gNB_index);
 }
 
 void *recv_msgs_from_lte_ue(void *args_p)
diff --git a/openair3/ocp-gtpu/gtp_itf.cpp b/openair3/ocp-gtpu/gtp_itf.cpp
index b598a29c16609e2aab2b4e20b9089a1de7e831bc..284b5cda35ba67469d62edab7050c8a179e06569 100644
--- a/openair3/ocp-gtpu/gtp_itf.cpp
+++ b/openair3/ocp-gtpu/gtp_itf.cpp
@@ -761,13 +761,32 @@ int gtpv1u_create_ngu_tunnel(  const instance_t instance,
   return !GTPNOK;
 }
 
-int gtpv1u_update_ngu_tunnel(
-  const instance_t instanceP,
-  const gtpv1u_gnb_create_tunnel_req_t *const  create_tunnel_req_pP,
-  const ue_id_t prior_ueid
-) {
-  AssertFatal( false, "to be developped\n");
-  return GTPNOK;
+int gtpv1u_update_ngu_tunnel(const instance_t instanceP, const gtpv1u_gnb_create_tunnel_req_t *const create_tunnel_req_pP, const ue_id_t prior_ue_id)
+{
+  LOG_D(GTPU, "[%ld] Update tunnels from UEid %lx to UEid %lx\n", instanceP, prior_ue_id, create_tunnel_req_pP->ue_id);
+
+  pthread_mutex_lock(&globGtp.gtp_lock);
+
+  auto inst = &globGtp.instances[compatInst(instanceP)];
+  auto it = inst->ue2te_mapping.find(prior_ue_id);
+  if (it == inst->ue2te_mapping.end()) {
+    LOG_W(GTPU, "[%ld] Delete GTP tunnels for UEid: %lx, but no tunnel exits\n", instanceP, prior_ue_id);
+    pthread_mutex_unlock(&globGtp.gtp_lock);
+    return GTPNOK;
+  }
+
+  for (int i = 0; i < create_tunnel_req_pP->num_tunnels; i++) {
+    teid_t incoming_teid = inst->ue2te_mapping[prior_ue_id].bearers[create_tunnel_req_pP->pdusession_id[i]].teid_incoming;
+    if (globGtp.te2ue_mapping[incoming_teid].ue_id == prior_ue_id) {
+      globGtp.te2ue_mapping[incoming_teid].ue_id = create_tunnel_req_pP->ue_id;
+    }
+  }
+
+  inst->ue2te_mapping[create_tunnel_req_pP->ue_id] = it->second;
+  inst->ue2te_mapping.erase(it);
+
+  pthread_mutex_unlock(&globGtp.gtp_lock);
+  return !GTPNOK;
 }
 
 int gtpv1u_create_x2u_tunnel(
diff --git a/targets/PROJECTS/GENERIC-NR-5GC/CONF/gnb.sa.band78.fr1.106PRB.usrpb210.conf b/targets/PROJECTS/GENERIC-NR-5GC/CONF/gnb.sa.band78.fr1.106PRB.usrpb210.conf
index ceb312e9c26ad68db3cc081581f06a7ead0f6341..36614b0e9caffd9daf9e2b6285d886ad7b3108c7 100644
--- a/targets/PROJECTS/GENERIC-NR-5GC/CONF/gnb.sa.band78.fr1.106PRB.usrpb210.conf
+++ b/targets/PROJECTS/GENERIC-NR-5GC/CONF/gnb.sa.band78.fr1.106PRB.usrpb210.conf
@@ -11,7 +11,7 @@ gNBs =
 
     // Tracking area code, 0x0000 and 0xfffe are reserved values
     tracking_area_code  =  1;
-    plmn_list = ({ mcc = 001; mnc = 01; mnc_length = 2; snssaiList = ({ sst = 1; sd = 0x1; }) });
+    plmn_list = ({ mcc = 001; mnc = 01; mnc_length = 2; snssaiList = ({ sst = 1; }) });
 
     nr_cellid = 12345678L;