From 9447e4fb573bd2f8933630abd7442639dde0cc33 Mon Sep 17 00:00:00 2001
From: Guido Casati <hello@guidocasati.com>
Date: Thu, 19 Dec 2024 10:32:41 +0100
Subject: [PATCH] Refactor 5GMM Capability encoding (9.11.3 of 3GPP TS 24.501)

* add all relevant content fields according to the specs and do the encoding

Related to #881
---
 .../NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.c   | 49 +++++++++++++++++--
 .../NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.h   | 24 ++++++++-
 .../NR_UE/5GS/5GMM/MSG/RegistrationRequest.c  |  7 +--
 3 files changed, 67 insertions(+), 13 deletions(-)

diff --git a/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.c b/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.c
index c053070518..8e5c7d1364 100644
--- a/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.c
+++ b/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.c
@@ -35,16 +35,55 @@
 #include "TLVDecoder.h"
 #include "FGMMCapability.h"
 
-int encode_5gmm_capability(const FGMMCapability *fgmmcapability, uint8_t iei, uint8_t *buffer, uint32_t len)
+#define MIN_LENGTH_5GMM_CAPABILITY 3
+
+/**
+ * @brief Encode 5GMM Capability IE (9.11.3 of 3GPP TS 24.501)
+ */
+int encode_5gmm_capability(uint8_t *buffer, const FGMMCapability *fgmmcapability, uint32_t len)
 {
   int encoded = 0;
-  if (iei) {
+
+  // Ensure buffer length is sufficient
+  if (len < MIN_LENGTH_5GMM_CAPABILITY) {
+    printf("Buffer length insufficient for encoding 5GMM capability\n");
+    return -1;
+  }
+
+  if (fgmmcapability->iei) {
     *buffer = fgmmcapability->iei;
     encoded++;
-    *(buffer + encoded) = fgmmcapability->length;
-    encoded++;
-    *(buffer + encoded) = fgmmcapability->value;
+  } else {
+    printf("Missing IEI in fgmmcapability\n");
+  }
+
+  buffer[encoded] = fgmmcapability->length;
+  encoded++;
+
+  // Encode octet 3 (mandatory)
+  buffer[encoded] = (fgmmcapability->sgc << 7) | (fgmmcapability->iphc_cp_cIoT << 6) | (fgmmcapability->n3_data << 5)
+                    | (fgmmcapability->cp_cIoT << 4) | (fgmmcapability->restrict_ec << 3) | (fgmmcapability->lpp << 2)
+                    | (fgmmcapability->ho_attach << 1) | fgmmcapability->s1_mode;
+  encoded++;
+
+  // Encode octet 4
+  if (fgmmcapability->length >= 2) {
+    buffer[encoded] = (fgmmcapability->racs << 7) | (fgmmcapability->nssaa << 6) | (fgmmcapability->lcs << 5)
+                      | (fgmmcapability->v2x_cnpc5 << 4) | (fgmmcapability->v2x_cepc5 << 3) | (fgmmcapability->v2x << 2)
+                      | (fgmmcapability->up_cIoT << 1) | fgmmcapability->srvcc;
     encoded++;
   }
+
+  // Encode octet 5
+  if (fgmmcapability->length >= 3) {
+    buffer[encoded] = (fgmmcapability->ehc_CP_ciot << 3) | (fgmmcapability->multiple_eUP << 2) | (fgmmcapability->wusa << 1)
+                      | fgmmcapability->cag;
+    encoded++;
+  }
+
+  // Encode octets 6 - 15
+  if (fgmmcapability->length >= 4)
+    memset(&buffer[encoded], 0, fgmmcapability->length - 3);
+
   return encoded;
 }
diff --git a/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.h b/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.h
index b125935f9d..fcbdded058 100644
--- a/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.h
+++ b/openair3/NAS/NR_UE/5GS/5GMM/IES/FGMMCapability.h
@@ -30,6 +30,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
+#include <stdbool.h>
 
 #include "OctetString.h"
 
@@ -39,9 +40,28 @@
 typedef struct {
   uint8_t iei;
   uint8_t length;
-  uint8_t value;
+  bool sgc;
+  bool iphc_cp_cIoT;
+  bool n3_data;
+  bool cp_cIoT;
+  bool restrict_ec;
+  bool lpp;
+  bool ho_attach;
+  bool s1_mode;
+  bool racs;
+  bool nssaa;
+  bool lcs;
+  bool v2x_cnpc5;
+  bool v2x_cepc5;
+  bool v2x;
+  bool up_cIoT;
+  bool srvcc;
+  bool ehc_CP_ciot;
+  bool multiple_eUP;
+  bool wusa;
+  bool cag;
 } FGMMCapability;
 
-int encode_5gmm_capability(const FGMMCapability *fgmmcapability, uint8_t iei, uint8_t *buffer, uint32_t len);
+int encode_5gmm_capability(uint8_t *buffer, const FGMMCapability *fgmmcapability, uint32_t len);
 
 #endif /* FGMM_CAPABILITY_H_ */
diff --git a/openair3/NAS/NR_UE/5GS/5GMM/MSG/RegistrationRequest.c b/openair3/NAS/NR_UE/5GS/5GMM/MSG/RegistrationRequest.c
index f71030f1f7..6b124a3b81 100644
--- a/openair3/NAS/NR_UE/5GS/5GMM/MSG/RegistrationRequest.c
+++ b/openair3/NAS/NR_UE/5GS/5GMM/MSG/RegistrationRequest.c
@@ -87,12 +87,7 @@ int encode_registration_request(const registration_request_msg *registration_req
 
   if ((registration_request->presencemask & REGISTRATION_REQUEST_5GMM_CAPABILITY_PRESENT)
       == REGISTRATION_REQUEST_5GMM_CAPABILITY_PRESENT) {
-    if ((encode_result = encode_5gmm_capability(&registration_request->fgmmcapability,
-                                                REGISTRATION_REQUEST_5GMM_CAPABILITY_IEI,
-                                                buffer + encoded,
-                                                len - encoded))
-        < 0)
-      // Return in case of error
+    if ((encode_result = encode_5gmm_capability(buffer + encoded, &registration_request->fgmmcapability, len - encoded)) < 0)
       return encode_result;
     else
       encoded += encode_result;