From 950becb423bc41db42e80050272bdaaa4bbc8741 Mon Sep 17 00:00:00 2001
From: Sakthivel Velumani <mail@sakthi.me>
Date: Wed, 4 Oct 2023 15:30:14 -0400
Subject: [PATCH] Store allowed NSSAIs from NAS message Request default PDU
 session with first NSSAI from the list

---
 executables/nr-uesoftmodem.c         |   1 +
 openair2/COMMON/as_message.h         |   7 ++
 openair2/COMMON/rrc_messages_def.h   |   1 +
 openair2/COMMON/rrc_messages_types.h |   2 +
 openair3/NAS/NR_UE/nr_nas_msg_sim.c  | 158 +++++++++++++++++++++++----
 openair3/NAS/NR_UE/nr_nas_msg_sim.h  |   7 ++
 6 files changed, 154 insertions(+), 22 deletions(-)

diff --git a/executables/nr-uesoftmodem.c b/executables/nr-uesoftmodem.c
index c970b1d09d..b734d9569b 100644
--- a/executables/nr-uesoftmodem.c
+++ b/executables/nr-uesoftmodem.c
@@ -100,6 +100,7 @@ pthread_cond_t sync_cond;
 pthread_mutex_t sync_mutex;
 int sync_var=-1; //!< protected by mutex \ref sync_mutex.
 int config_sync_var=-1;
+nr_nas_msg_snssai_t nas_allowed_nssai[8];
 
 // not used in UE
 instance_t CUuniqInstance=0;
diff --git a/openair2/COMMON/as_message.h b/openair2/COMMON/as_message.h
index 0848714c2c..c4724b5458 100644
--- a/openair2/COMMON/as_message.h
+++ b/openair2/COMMON/as_message.h
@@ -464,6 +464,13 @@ typedef ul_info_transfer_cnf_t dl_info_transfer_cnf_t;
  */
 typedef ul_info_transfer_ind_t dl_info_transfer_ind_t;
 
+typedef struct nas_pdu_session_req_s {
+  int pdusession_id;
+  int pdusession_type;
+  int sst;
+  int sd;
+} nas_pdu_session_req_t;
+
 /*
  * --------------------------------------------------------------------------
  *          Radio Access Bearer establishment
diff --git a/openair2/COMMON/rrc_messages_def.h b/openair2/COMMON/rrc_messages_def.h
index 8609aa45d2..80e191db9f 100644
--- a/openair2/COMMON/rrc_messages_def.h
+++ b/openair2/COMMON/rrc_messages_def.h
@@ -82,3 +82,4 @@ MESSAGE_DEF(NRRRC_FRAME_PROCESS,        MESSAGE_PRIORITY_MED,       NRRrcFramePr
 
 // eNB: RLC -> RRC messages
 MESSAGE_DEF(RLC_SDU_INDICATION,         MESSAGE_PRIORITY_MED,       RlcSduIndication,           rlc_sdu_indication)
+MESSAGE_DEF(NAS_PDU_SESSION_REQ, MESSAGE_PRIORITY_MED, NasPduSessionReq, nas_pdu_session_req)
diff --git a/openair2/COMMON/rrc_messages_types.h b/openair2/COMMON/rrc_messages_types.h
index 037dfbe86c..c7fc58b6cb 100644
--- a/openair2/COMMON/rrc_messages_types.h
+++ b/openair2/COMMON/rrc_messages_types.h
@@ -92,6 +92,7 @@
 #define NRDuDlReq(mSGpTR)      (mSGpTR)->ittiMsg.nr_du_dl_req
 
 #define NAS_OAI_TUN_NSA(mSGpTR)         (mSGpTR)->ittiMsg.nas_oai_tun_nsa
+#define NAS_PDU_SESSION_REQ(mSGpTR) (mSGpTR)->ittiMsg.nas_pdu_session_req
 
 //-------------------------------------------------------------------------------------------//
 typedef struct RrcStateInd_s {
@@ -440,6 +441,7 @@ typedef nas_release_ind_t       NasConnReleaseInd;
 typedef ul_info_transfer_cnf_t  NasUlDataCnf;
 typedef dl_info_transfer_ind_t  NasDlDataInd;
 
+typedef nas_pdu_session_req_t NasPduSessionReq;
 // eNB: realtime -> RRC messages
 typedef struct rrc_subframe_process_s {
   protocol_ctxt_t ctxt;
diff --git a/openair3/NAS/NR_UE/nr_nas_msg_sim.c b/openair3/NAS/NR_UE/nr_nas_msg_sim.c
index 015b2a8f9e..2fc5209f6f 100644
--- a/openair3/NAS/NR_UE/nr_nas_msg_sim.c
+++ b/openair3/NAS/NR_UE/nr_nas_msg_sim.c
@@ -58,6 +58,8 @@ uint32_t  registration_request_len;
 extern char *baseNetAddress;
 extern uint16_t NB_UE_INST;
 static nr_ue_nas_t nr_ue_nas = {0};
+static nr_ue_nas_t nr_ue_nas;
+extern nr_nas_msg_snssai_t nas_allowed_nssai[8];
 
 static int nas_protected_security_header_encode(
   char                                       *buffer,
@@ -778,7 +780,7 @@ static void generateDeregistrationRequest(nr_ue_nas_t *nas, as_nas_info_t *initi
     initialNasMsg->data[2 + i] = mac[i];
 }
 
-static void generatePduSessionEstablishRequest(nr_ue_nas_t *nas, as_nas_info_t *initialNasMsg)
+static void generatePduSessionEstablishRequest(nr_ue_nas_t *nas, as_nas_info_t *initialNasMsg, nas_pdu_session_req_t *pdu_req)
 {
   //wait send RegistrationComplete
   usleep(100*150);
@@ -790,11 +792,11 @@ static void generatePduSessionEstablishRequest(nr_ue_nas_t *nas, as_nas_info_t *
   uint8_t *req_buffer = malloc(req_length);
   pdu_session_establishment_request_msg pdu_session_establish;
   pdu_session_establish.protocoldiscriminator = FGS_SESSION_MANAGEMENT_MESSAGE;
-  pdu_session_establish.pdusessionid = 10;
+  pdu_session_establish.pdusessionid = pdu_req->pdusession_id;
   pdu_session_establish.pti = 1;
   pdu_session_establish.pdusessionestblishmsgtype = FGS_PDU_SESSION_ESTABLISHMENT_REQ;
   pdu_session_establish.maxdatarate = 0xffff;
-  pdu_session_establish.pdusessiontype = 0x91;
+  pdu_session_establish.pdusessiontype = pdu_req->pdusession_type;
   encode_pdu_session_establishment_request(&pdu_session_establish, req_buffer);
 
 
@@ -827,21 +829,21 @@ static void generatePduSessionEstablishRequest(nr_ue_nas_t *nas, as_nas_info_t *
   mm_msg->uplink_nas_transport.fgspayloadcontainer.payloadcontainercontents.length = req_length;
   mm_msg->uplink_nas_transport.fgspayloadcontainer.payloadcontainercontents.value = req_buffer;
   size += (2+req_length);
-  mm_msg->uplink_nas_transport.pdusessionid = 10;
+  mm_msg->uplink_nas_transport.pdusessionid = pdu_req->pdusession_id;
   mm_msg->uplink_nas_transport.requesttype = 1;
   size += 3;
-  const bool has_nssai_sd = nas->uicc->nssai_sd != 0xffffff; // 0xffffff means "no SD", TS 23.003
+  const bool has_nssai_sd = pdu_req->sd != 0xffffff; // 0xffffff means "no SD", TS 23.003
   const size_t nssai_len = has_nssai_sd ? 4 : 1;
   mm_msg->uplink_nas_transport.snssai.length = nssai_len;
   //Fixme: it seems there are a lot of memory errors in this: this value was on the stack, 
   // but pushed  in a itti message to another thread
   // this kind of error seems in many places in 5G NAS
   mm_msg->uplink_nas_transport.snssai.value = calloc(1, nssai_len);
-  mm_msg->uplink_nas_transport.snssai.value[0] = nas->uicc->nssai_sst;
+  mm_msg->uplink_nas_transport.snssai.value[0] = pdu_req->sst;
   if (has_nssai_sd) {
-    mm_msg->uplink_nas_transport.snssai.value[1] = (nas->uicc->nssai_sd >> 16) & 0xFF;
-    mm_msg->uplink_nas_transport.snssai.value[2] = (nas->uicc->nssai_sd >> 8)  & 0xFF;
-    mm_msg->uplink_nas_transport.snssai.value[3] = (nas->uicc->nssai_sd)       & 0xFF;
+    mm_msg->uplink_nas_transport.snssai.value[1] = (pdu_req->sd >> 16) & 0xFF;
+    mm_msg->uplink_nas_transport.snssai.value[2] = (pdu_req->sd >> 8) & 0xFF;
+    mm_msg->uplink_nas_transport.snssai.value[3] = (pdu_req->sd) & 0xFF;
   }
   size += 1 + 1 + nssai_len;
   int dnnSize=strlen(nas->uicc->dnnStr);
@@ -912,6 +914,116 @@ static void send_nas_uplink_data_req(instance_t instance, const as_nas_info_t *i
   itti_send_msg_to_task(TASK_RRC_NRUE, instance, msg);
 }
 
+static void parse_allowed_nssai(nr_nas_msg_snssai_t nssaiList[8], const uint8_t *buf, const uint32_t len)
+{
+  int offset = 0;
+  int nssai_cnt = 0;
+  while (offset < len) {
+    int length = *(buf + offset);
+    nr_nas_msg_snssai_t *nssai = nssaiList + nssai_cnt;
+    nssai->sd = 0xFFFFFF;
+
+    switch (length) {
+      case 1:
+        nssai->sst = *(buf + offset + 1);
+        offset += 2;
+        nssai_cnt++;
+        break;
+
+      case 2:
+        nssai->sst = *(buf + offset + 1);
+        nssai->hplmn_sst = *(buf + offset + 2);
+        offset += 3;
+        nssai_cnt++;
+        break;
+
+      case 4:
+        nssai->sst = *(buf + offset + 1);
+        nssai->sd = 0xFFFFFF & (*(buf + offset + 4) | (*(buf + offset + 3) << 8) | (*(buf + offset + 2) << 16));
+        offset += 5;
+        nssai_cnt++;
+        break;
+
+      case 5:
+        nssai->sst = *(buf + offset + 1);
+        nssai->sd = 0xFFFFFF & (*(buf + offset + 4) | (*(buf + offset + 3) << 8) | (*(buf + offset + 2) << 16));
+        nssai->hplmn_sst = *(buf + offset + 5);
+        offset += 6;
+        nssai_cnt++;
+        break;
+
+      case 8:
+        nssai->sst = *(buf + offset + 1);
+        nssai->sd = 0xFFFFFF & (*(buf + offset + 4) | (*(buf + offset + 3) << 8) | (*(buf + offset + 2) << 16));
+        nssai->hplmn_sst = *(buf + offset + 5);
+        nssai->hplmn_sd = 0xFFFFFF & (*(buf + offset + 8) | (*(buf + offset + 7) << 8) | (*(buf + offset + 6) << 16));
+        offset += 9;
+        nssai_cnt++;
+        break;
+
+      default:
+        LOG_E(NAS, "UE received unknown length in an allowed S-NSSAI\n");
+        break;
+    }
+  }
+}
+
+/* Extract Allowed NSSAI from Regestration Accept according to
+   3GPP TS 24.501 Table 8.2.7.1.1
+*/
+static void get_allowed_nssai(nr_nas_msg_snssai_t nssai[8], const uint8_t *pdu_buffer, const uint32_t pdu_length)
+{
+  if ((pdu_buffer == NULL) || (pdu_length <= 0))
+    return;
+
+  int offset = 1 + 1 + 1 + 2; // Mandatory fields offset
+  if (((nas_msg_header_t *)(pdu_buffer))->choice.security_protected_nas_msg_header_t.security_header_type > 0) {
+    offset += SECURITY_PROTECTED_5GS_NAS_MESSAGE_HEADER_LENGTH;
+  }
+
+  bool unknown_IEI_found = false;
+
+  /* optional fields */
+  int length = 0;
+  while ((offset < pdu_length) && !unknown_IEI_found) {
+    int type = *(pdu_buffer + offset);
+    switch (type) {
+      case 0x77: // 5GS mobile identity
+        length = (*(pdu_buffer + offset + 1) << 8) | *(pdu_buffer + offset + 2);
+        offset += (length + 1 + 2); // 1: type, 2: length
+        break;
+
+      case 0x4A: // PLMN list
+      case 0x54: // 5GS tracking area identity
+        length = *((uint8_t *)(pdu_buffer + offset + 1));
+        offset += (length + 1 + 1); // 1: type, 2: length
+        break;
+
+      case 0x15: // allowed NSSAI
+        length = *((uint8_t *)(pdu_buffer + offset + 1));
+        parse_allowed_nssai(nssai, pdu_buffer + offset + 2, length);
+        offset += (length + 1 + 1); // 1: type, 2: length
+        break;
+
+      default:
+        LOG_E(NAS, "This NAS IEI is not handled when extracting list of allowed NSSAI\n");
+        unknown_IEI_found = true;
+        break;
+    }
+  }
+}
+
+static void request_default_pdusession(int instance)
+{
+  MessageDef *message_p = itti_alloc_new_message(TASK_NAS_NRUE, 0, NAS_PDU_SESSION_REQ);
+  NAS_PDU_SESSION_REQ(message_p).pdusession_id = 10; /* first or default pdu session */
+  NAS_PDU_SESSION_REQ(message_p).pdusession_type = 0x91;
+  /* take the first allowed S-NSSAI for default PDU session */
+  NAS_PDU_SESSION_REQ(message_p).sst = nas_allowed_nssai[0].sst;
+  NAS_PDU_SESSION_REQ(message_p).sd = nas_allowed_nssai[0].sd;
+  itti_send_msg_to_task(TASK_NAS_NRUE, instance, message_p);
+}
+
 void *nas_nrue_task(void *args_p)
 {
   nr_ue_nas.uicc = checkUicc(0);
@@ -975,6 +1087,18 @@ void *nas_nrue(void *args_p)
         /* TODO not processed by NAS currently */
         break;
 
+      case NAS_PDU_SESSION_REQ: {
+        as_nas_info_t pduEstablishMsg = {0};
+        nas_pdu_session_req_t *pduReq = &NAS_PDU_SESSION_REQ(msg_p);
+        nr_ue_nas_t *nas = get_ue_nas_info(0);
+        generatePduSessionEstablishRequest(nas, &pduEstablishMsg, pduReq);
+        if (pduEstablishMsg.length > 0) {
+          send_nas_uplink_data_req(instance, &pduEstablishMsg);
+          LOG_I(NAS, "Send NAS_UPLINK_DATA_REQ message(PduSessionEstablishRequest)\n");
+        }
+        break;
+      }
+
       case NAS_CONN_ESTABLI_CNF: {
         LOG_I(NAS,
               "[UE %ld] Received %s: errCode %u, length %u\n",
@@ -990,6 +1114,7 @@ void *nas_nrue(void *args_p)
           LOG_I(NAS, "[UE] Received REGISTRATION ACCEPT message\n");
           nr_ue_nas_t *nas = get_ue_nas_info(0);
           decodeRegistrationAccept(pdu_buffer, NAS_CONN_ESTABLI_CNF(msg_p).nasMsg.length, nas);
+          get_allowed_nssai(nas_allowed_nssai, pdu_buffer, NAS_CONN_ESTABLI_CNF(msg_p).nasMsg.length);
 
           as_nas_info_t initialNasMsg = {0};
           generateRegistrationComplete(nas, &initialNasMsg, NULL);
@@ -998,12 +1123,7 @@ void *nas_nrue(void *args_p)
             LOG_I(NAS, "Send NAS_UPLINK_DATA_REQ message(RegistrationComplete)\n");
           }
 
-          as_nas_info_t pduEstablishMsg = {0};
-          generatePduSessionEstablishRequest(nas, &pduEstablishMsg);
-          if (pduEstablishMsg.length > 0) {
-            send_nas_uplink_data_req(instance, &pduEstablishMsg);
-            LOG_I(NAS, "Send NAS_UPLINK_DATA_REQ message(PduSessionEstablishRequest)\n");
-          }
+          request_default_pdusession(instance);
         } else if (msg_type == FGS_PDU_SESSION_ESTABLISHMENT_ACC) {
           capture_pdu_session_establishment_accept_msg(pdu_buffer, NAS_CONN_ESTABLI_CNF(msg_p).nasMsg.length);
         }
@@ -1081,13 +1201,7 @@ void *nas_nrue(void *args_p)
               send_nas_uplink_data_req(instance, &initialNasMsg);
               LOG_I(NAS, "Send NAS_UPLINK_DATA_REQ message(RegistrationComplete)\n");
             }
-
-            as_nas_info_t pduEstablishMsg = {0};
-            generatePduSessionEstablishRequest(nas, &pduEstablishMsg);
-            if (pduEstablishMsg.length > 0) {
-              send_nas_uplink_data_req(instance, &pduEstablishMsg);
-              LOG_I(NAS, "Send NAS_UPLINK_DATA_REQ message(PduSessionEstablishRequest)\n");
-            }
+            request_default_pdusession(instance);
             break;
           case FGS_DEREGISTRATION_ACCEPT:
             LOG_I(NAS, "received deregistration accept\n");
diff --git a/openair3/NAS/NR_UE/nr_nas_msg_sim.h b/openair3/NAS/NR_UE/nr_nas_msg_sim.h
index fa4911c738..6ca20a00cd 100644
--- a/openair3/NAS/NR_UE/nr_nas_msg_sim.h
+++ b/openair3/NAS/NR_UE/nr_nas_msg_sim.h
@@ -171,6 +171,13 @@ typedef struct {
     fgs_sm_nas_msg_header_t sm_nas_msg_header;
 } dl_nas_transport_t;
 
+typedef struct {
+  int sst;
+  int hplmn_sst;
+  int sd;
+  int hplmn_sd;
+} nr_nas_msg_snssai_t;
+
 nr_ue_nas_t *get_ue_nas_info(module_id_t module_id);
 void generateRegistrationRequest(as_nas_info_t *initialNasMsg, nr_ue_nas_t *nas);
 void *nas_nrue_task(void *args_p);
-- 
2.26.2