From 7b7acea59bce0ca0b2747eb8b2d5c4930f90c9f6 Mon Sep 17 00:00:00 2001
From: Sukchan Lee <acetcom@gmail.com>
Date: Mon, 18 Nov 2019 20:21:20 +0900
Subject: [PATCH] Add IMEISV(MEI) in Create Session Request

---
 lib/core/ogs-3gpp-types.h |  2 ++
 lib/core/ogs-conv.c       | 35 +++++++++++------------------------
 src/mme/emm-handler.c     |  2 ++
 src/mme/mme-context.h     |  2 ++
 src/mme/mme-s11-build.c   |  6 ++++++
 5 files changed, 23 insertions(+), 24 deletions(-)

diff --git a/lib/core/ogs-3gpp-types.h b/lib/core/ogs-3gpp-types.h
index 9200d149e..89ad13cd5 100644
--- a/lib/core/ogs-3gpp-types.h
+++ b/lib/core/ogs-3gpp-types.h
@@ -41,6 +41,8 @@ extern "C" {
     OGS_BCD_TO_BUFFER_LEN(OGS_MAX_IMSI_BCD_LEN)
 
 #define OGS_MAX_IMEISV_BCD_LEN          16
+#define OGS_MAX_IMEISV_LEN              \
+    OGS_BCD_TO_BUFFER_LEN(OGS_MAX_IMEISV_BCD_LEN)
 
 #define OGS_MAX_NUM_OF_HOSTNAME         16
 #define OGS_MAX_APN_LEN                 100
diff --git a/lib/core/ogs-conv.c b/lib/core/ogs-conv.c
index 183214534..73b43dd22 100644
--- a/lib/core/ogs-conv.c
+++ b/lib/core/ogs-conv.c
@@ -30,18 +30,13 @@ void *ogs_ascii_to_hex(char *in, int in_len, void *out, int out_len)
     int i = 0, j = 0, k = 0, hex;
     uint8_t *out_p = out;
 
-    while(i < in_len && j < out_len)
-    {
-        if (!isspace(in[i]))
-        {
+    while(i < in_len && j < out_len) {
+        if (!isspace(in[i])) {
             hex = isdigit(in[i]) ? in[i] - '0' : 
                 islower(in[i]) ? in[i] - 'a' + 10 : in[i] - 'A' + 10;
-            if ((k & 0x1) == 0)
-            {
+            if ((k & 0x1) == 0) {
                 out_p[j] = (hex << 4);
-            }
-            else
-            {
+            } else {
                 out_p[j] |= hex;
                 j++;
             }
@@ -62,8 +57,7 @@ void *ogs_hex_to_ascii(void *in, int in_len, void *out, int out_len)
     p[0] = 0;
 
     l = (in_len - off) > out_len ? out_len : in_len - off;
-    for (i = 0; i < l; i++)
-    {
+    for (i = 0; i < l; i++) {
         p += sprintf(p, "%02X", ((char*)in)[off+i] & 0xff);
         if ((i & 0x3) == 3 && (i != (l-1))) p += sprintf(p, " ");
     }
@@ -87,8 +81,7 @@ uint64_t ogs_buffer_to_uint64(void *buffer, int size)
     uint8_t *buffer_p = buffer;
     int i;
 
-    for (i = 0; i < size; i++)
-    {
+    for (i = 0; i < size; i++) {
         num |= (((uint64_t)buffer_p[i]) << ((size-1-i) * 8));
     }
 
@@ -101,8 +94,7 @@ void *ogs_bcd_to_buffer(const char *in, void *out, int *out_len)
     uint8_t *out_p = out;
     int in_len = strlen(in);
 
-    for (i = 0; i < in_len; i++) 
-    {
+    for (i = 0; i < in_len; i++) {
         if (i & 0x01)
             out_p[i>>1] = out_p[i>>1] | (((in[i] - 0x30) << 4) & 0xF0);
         else
@@ -110,8 +102,7 @@ void *ogs_bcd_to_buffer(const char *in, void *out, int *out_len)
     }
 
     *out_len = (in_len + 1) / 2;
-    if (in_len & 0x01)
-    {
+    if (in_len & 0x01) {
         out_p[(*out_len)-1] |= 0xF0;
     }
 
@@ -123,19 +114,15 @@ void *ogs_buffer_to_bcd(uint8_t *in, int in_len, void *out)
     int i = 0;
     uint8_t *out_p = out;
 
-    for (i = 0; i < in_len-1; i++) 
-    {
+    for (i = 0; i < in_len-1; i++) {
         out_p[i*2] = 0x30 + (in[i] & 0x0F);
         out_p[i*2+1] = 0x30 + ((in[i] & 0xF0) >> 4);
     }
 
-    if ((in[i] & 0xF0) == 0xF0)
-    {
+    if ((in[i] & 0xF0) == 0xF0) {
         out_p[i*2] = 0x30 + (in[i] & 0x0F);
         out_p[i*2+1] = 0;
-    }
-    else
-    {
+    } else {
         out_p[i*2] = 0x30 + (in[i] & 0x0F);
         out_p[i*2+1] = 0x30 + ((in[i] & 0xF0) >> 4);
         out_p[i*2+2] = 0;
diff --git a/src/mme/emm-handler.c b/src/mme/emm-handler.c
index 39d073f28..8bb31c9f3 100644
--- a/src/mme/emm-handler.c
+++ b/src/mme/emm-handler.c
@@ -635,6 +635,8 @@ int emm_handle_security_mode_complete(mme_ue_t *mme_ue,
                 &imeisv->imeisv, imeisv->length);
             ogs_nas_imeisv_to_bcd(&imeisv->imeisv, imeisv->length,
                     mme_ue->imeisv_bcd);
+            ogs_bcd_to_buffer(mme_ue->imeisv_bcd,
+                    mme_ue->imeisv, &mme_ue->imeisv_len);
             mme_ue->imeisv_presence = true;
             break;
         default:
diff --git a/src/mme/mme-context.h b/src/mme/mme-context.h
index 1f6084574..fc409f16e 100644
--- a/src/mme/mme-context.h
+++ b/src/mme/mme-context.h
@@ -306,6 +306,8 @@ struct mme_ue_s {
     ogs_nas_mobile_identity_imsi_t nas_mobile_identity_imsi;
 
     bool            imeisv_presence;
+    uint8_t         imeisv[OGS_MAX_IMEISV_LEN];
+    int             imeisv_len;
     char            imeisv_bcd[OGS_MAX_IMEISV_BCD_LEN+1];
     ogs_nas_mobile_identity_imeisv_t nas_mobile_identity_imeisv;
 
diff --git a/src/mme/mme-s11-build.c b/src/mme/mme-s11-build.c
index 75483bcaf..f41848c35 100644
--- a/src/mme/mme-s11-build.c
+++ b/src/mme/mme-s11-build.c
@@ -61,6 +61,12 @@ int mme_s11_build_create_session_request(
     req->imsi.data = mme_ue->imsi;
     req->imsi.len = mme_ue->imsi_len;
 
+    if (mme_ue->imeisv_presence) {
+        req->me_identity.presence = 1;
+        req->me_identity.data = mme_ue->imeisv;
+        req->me_identity.len = mme_ue->imeisv_len;
+    }
+
     memset(&uli, 0, sizeof(ogs_gtp_uli_t));
     uli.flags.e_cgi = 1;
     uli.flags.tai = 1;