From 4b4f6b9d7f9e1a255c5109979d7d34d4d2c72578 Mon Sep 17 00:00:00 2001
From: Carles Fernandez <carles.fernandez@cttc.es>
Date: Mon, 1 Jul 2024 22:43:54 +0200
Subject: [PATCH] Fix CMAC-AES with GnuTLS

---
 cmake/Modules/GnssSdrCrypto.cmake         |  6 ++++
 src/core/system_parameters/gnss_crypto.cc | 36 +++++++++++++++++++----
 2 files changed, 37 insertions(+), 5 deletions(-)

diff --git a/cmake/Modules/GnssSdrCrypto.cmake b/cmake/Modules/GnssSdrCrypto.cmake
index a5e383790..9649da9da 100644
--- a/cmake/Modules/GnssSdrCrypto.cmake
+++ b/cmake/Modules/GnssSdrCrypto.cmake
@@ -119,6 +119,9 @@ else()
     if("${gnutls_gnutls_file_contents}" MATCHES "#define GNUTLS_VERSION_MAJOR 2")
         set(GNUTLS_HMAC_INIT_WITH_DIGEST TRUE)
     endif()
+    if("${gnutls_gnutls_file_contents}" MATCHES "GNUTLS_MAC_AES_CMAC_128")
+       set(GNUTLS_MAC_AES_CMAC_128 TRUE)
+    endif()
     file(READ "${GNUTLS_INCLUDE_DIR}/gnutls/abstract.h" gnutls_abstract_file_contents)
     if("${gnutls_abstract_file_contents}" MATCHES "gnutls_pubkey_export2")
         set(GNUTLS_PUBKEY_EXPORT2 TRUE)
@@ -184,5 +187,8 @@ function(link_to_crypto_dependencies target)
         if(GNUTLS_HMAC_INIT_WITH_DIGEST)
             target_compile_definitions(${target} PRIVATE -DHAVE_GNUTLS_HMAC_INIT_WITH_DIGEST=1)
         endif()
+        if(GNUTLS_MAC_AES_CMAC_128)
+            target_compile_definitions(${target} PRIVATE -DHAVE_GNUTLS_MAC_AES_CMAC_128=1)
+        endif()
     endif()
 endfunction()
diff --git a/src/core/system_parameters/gnss_crypto.cc b/src/core/system_parameters/gnss_crypto.cc
index 2062b0674..055bd0382 100644
--- a/src/core/system_parameters/gnss_crypto.cc
+++ b/src/core/system_parameters/gnss_crypto.cc
@@ -436,7 +436,32 @@ std::vector<uint8_t> Gnss_Crypto::computeCMAC_AES(const std::vector<uint8_t>& ke
 {
     std::vector<uint8_t> output(16);
 #if USE_GNUTLS_FALLBACK
-    // CMAC-AES not implemented in GnuTLS
+#if HAVE_GNUTLS_MAC_AES_CMAC_128
+    gnutls_hmac_hd_t hmac;
+
+    // Initialize the HMAC context with the CMAC algorithm and key
+    int ret = gnutls_hmac_init(&hmac, GNUTLS_MAC_AES_CMAC_128, key.data(), key.size());
+    if (ret != GNUTLS_E_SUCCESS)
+        {
+            LOG(INFO) << "OSNMA CMAC-AES: gnutls_hmac_init failed: " << gnutls_strerror(ret);
+            return output;
+        }
+
+    // Update the HMAC context with the input data
+    ret = gnutls_hmac(hmac, input.data(), input.size());
+    if (ret != GNUTLS_E_SUCCESS)
+        {
+            LOG(INFO) << "OSNMA CMAC-AES: gnutls_hmac failed: " << gnutls_strerror(ret);
+            gnutls_hmac_deinit(hmac, nullptr);
+            return output;
+        }
+
+    // Retrieve the HMAC output
+    gnutls_hmac_output(hmac, output.data());
+
+    // Clean up the HMAC context
+    gnutls_hmac_deinit(hmac, nullptr);
+#else
     if (!key.empty())
         {
             // do nothing
@@ -445,6 +470,7 @@ std::vector<uint8_t> Gnss_Crypto::computeCMAC_AES(const std::vector<uint8_t>& ke
         {
             // do nothing
         }
+#endif
 #else  // OpenSSL
 #if USE_OPENSSL_3
     std::vector<uint8_t> aux(EVP_MAX_MD_SIZE);  // CMAC-AES output size
@@ -515,7 +541,7 @@ std::vector<uint8_t> Gnss_Crypto::computeCMAC_AES(const std::vector<uint8_t>& ke
         }
 
     // Initialize the CMAC context with the key and cipher
-    if (1 != CMAC_Init(cmacCtx, key.data(), key.size(), EVP_aes_128_cbc(), nullptr))
+    if (CMAC_Init(cmacCtx, key.data(), key.size(), EVP_aes_128_cbc(), nullptr) != 1)
         {
             LOG(INFO) << "OSNMA CMAC-AES: MAC_Init failed";
             CMAC_CTX_free(cmacCtx);
@@ -523,7 +549,7 @@ std::vector<uint8_t> Gnss_Crypto::computeCMAC_AES(const std::vector<uint8_t>& ke
         }
 
     // Compute the CMAC
-    if (1 != CMAC_Update(cmacCtx, input.data(), input.size()))
+    if (CMAC_Update(cmacCtx, input.data(), input.size()) != 1)
         {
             LOG(INFO) << "OSNMA CMAC-AES: CMAC_Update failed";
             CMAC_CTX_free(cmacCtx);
@@ -531,9 +557,9 @@ std::vector<uint8_t> Gnss_Crypto::computeCMAC_AES(const std::vector<uint8_t>& ke
         }
 
     // Finalize the CMAC computation and retrieve the output
-    if (1 != CMAC_Final(cmacCtx, output.data(), &mac_length))
+    if (CMAC_Final(cmacCtx, output.data(), &mac_length) != 1)
         {
-            LOG(INFO) << "OSNMA CMAC-AES:CMAC_Final failed";
+            LOG(INFO) << "OSNMA CMAC-AES: CMAC_Final failed";
             CMAC_CTX_free(cmacCtx);
             return output;
         }