From 1e45b969e773c109bb5c28e936fbef4555bb7bd7 Mon Sep 17 00:00:00 2001
From: Rupesh Chiluka <rchiluka@marvell.com>
Date: Thu, 26 Dec 2024 13:36:55 +0530
Subject: [PATCH] Added Decryption

Signed-off-by: Rupesh Chiluka <rchiluka@marvell.com>
---
 .../pna_nic/externs/pna_ipsec_accelerator.cpp | 91 +++++++++++++++++++
 .../pna_nic/externs/pna_ipsec_accelerator.h   | 15 +++
 2 files changed, 106 insertions(+)

diff --git a/targets/pna_nic/externs/pna_ipsec_accelerator.cpp b/targets/pna_nic/externs/pna_ipsec_accelerator.cpp
index b8e64110..66f660d9 100644
--- a/targets/pna_nic/externs/pna_ipsec_accelerator.cpp
+++ b/targets/pna_nic/externs/pna_ipsec_accelerator.cpp
@@ -39,6 +39,97 @@ void PNA_IpsecAccelerator::disable() {
     _is_enabled = false;
 }
 
+void PNA_IpsecAccelerator::cipher(std::vector<unsigned char> input, std::vector<unsigned char> &output,
+                                unsigned char key[16], unsigned char iv[16], int encrypt) {
+    EVP_CIPHER_CTX *ctx;
+
+    ctx = EVP_CIPHER_CTX_new();
+    if (! EVP_CipherInit_ex2(ctx, EVP_aes_128_cbc(), key, iv, encrypt, NULL)) {
+        // Error
+        EVP_CIPHER_CTX_free(ctx);
+        return;
+    }
+
+    EVP_CIPHER_CTX_set_padding(ctx, 0);
+    
+    int outlen = 0;
+    int tmplen = 0;
+
+    if (!EVP_CipherUpdate(ctx, output.data(), &outlen, input.data(),
+                            input.size())) {
+        EVP_CIPHER_CTX_free(ctx);
+        ERR_print_errors_fp(stderr);
+        BMLOG_DEBUG("Error in CipherUpdate");
+        return;
+    }
+
+    if (!EVP_CipherFinal_ex(ctx, output.data() + outlen, &tmplen)) {
+        EVP_CIPHER_CTX_free(ctx);
+        ERR_print_errors_fp(stderr);
+        BMLOG_DEBUG("Error in CipherFinal");
+        return;
+    }
+
+    outlen += tmplen;
+    output.resize(outlen);
+    EVP_CIPHER_CTX_free(ctx);
+}
+
+void PNA_IpsecAccelerator::decrypt(std::string string_key) {
+    BMLOG_DEBUG("[IPSEC] In Decrypt");
+
+    std::vector<unsigned char> raw_packet_data;
+    raw_packet_data.resize(get_packet().get_data_size(), '\0');
+    std::copy(get_packet().data(),
+            get_packet().data() + get_packet().get_data_size(),
+            raw_packet_data.begin());
+    
+    unsigned int block_size = EVP_CIPHER_block_size(EVP_aes_128_cbc());
+    
+    // TODO:
+    // check the ICV
+    // compute HMAC
+    // drop the packet if ICV and the computed hmac are not the same
+    unsigned char iv[block_size + 1] = {0};
+    unsigned char key[string_key.length()];
+    std::copy(string_key.begin(), string_key.end(), key);
+
+    // Copy IV from the packet
+    std::copy_n(raw_packet_data.begin() + ETH_HEADER_LENGTH + IP_HEADER_LENGTH 
+                        + ESP_SPI_LENGTH + ESP_SEQ_LENGTH, block_size, iv);
+
+    std::vector<unsigned char> encrypted;
+
+    encrypted.resize(raw_packet_data.size() - ETH_HEADER_LENGTH 
+                        - IP_HEADER_LENGTH - ESP_SPI_LENGTH 
+                        - ESP_SEQ_LENGTH - block_size, '\0');
+    std::copy(raw_packet_data.begin() + ETH_HEADER_LENGTH + IP_HEADER_LENGTH 
+                    + ESP_SPI_LENGTH + ESP_SEQ_LENGTH + block_size, 
+                    raw_packet_data.end(), encrypted.begin());
+
+    std::vector <unsigned char> decrypted;
+    decrypted.resize(encrypted.size() + block_size, '\0');
+    
+    this->cipher(encrypted, decrypted, key, iv, 0);
+
+    int padding_length = *(decrypted.data() + decrypted.size() - NEXT_HEADER_LENGTH);
+
+    // replace payload
+
+    // first, remove all the data
+    get_packet().remove(get_packet().get_data_size());
+    // make room for the ciphertext and write the ciphertext in it
+    char *payload_start = get_packet().prepend( (size_t) (decrypted.size() 
+                            + ETH_HEADER_LENGTH - NEXT_HEADER_LENGTH - padding_length) );
+    
+    std::copy(raw_packet_data.begin(), 
+                raw_packet_data.begin() + ETH_HEADER_LENGTH, 
+                payload_start);
+    std::copy(decrypted.begin(), 
+                decrypted.end() - NEXT_HEADER_LENGTH - padding_length,
+                payload_start + ETH_HEADER_LENGTH);
+}
+
 BM_REGISTER_EXTERN_W_NAME(ipsec_accelerator, PNA_IpsecAccelerator);
 BM_REGISTER_EXTERN_W_NAME_METHOD(ipsec_accelerator, PNA_IpsecAccelerator, set_sa_index, const Data &);
 BM_REGISTER_EXTERN_W_NAME_METHOD(ipsec_accelerator, PNA_IpsecAccelerator, enable);
diff --git a/targets/pna_nic/externs/pna_ipsec_accelerator.h b/targets/pna_nic/externs/pna_ipsec_accelerator.h
index 8fcdb8e7..ded03bdd 100644
--- a/targets/pna_nic/externs/pna_ipsec_accelerator.h
+++ b/targets/pna_nic/externs/pna_ipsec_accelerator.h
@@ -21,7 +21,17 @@
 #ifndef PNA_NIC_PNA_IPSECACCELERATOR_H_
 #define PNA_NIC_PNA_IPSECACCELERATOR_H_
 
+#define ETH_HEADER_LENGTH     14
+#define IP_HEADER_LENGTH      20
+#define ESP_SPI_LENGTH        4
+#define ESP_SEQ_LENGTH        4
+#define NEXT_HEADER_LENGTH    2
+
 #include <bm/bm_sim/extern.h>
+#include <bm/bm_sim/logger.h>
+#include <bm/bm_sim/packet.h>
+#include <openssl/evp.h>
+#include <openssl/err.h>
 
 namespace bm {
 
@@ -41,6 +51,11 @@ class PNA_IpsecAccelerator : public bm::ExternType {
 
    void disable();
 
+   void cipher(std::vector<unsigned char> input, std::vector<unsigned char> &output,
+                unsigned char key[16], unsigned char iv[16], int encrypt);
+
+   void decrypt(std::string string_key);
+
   private:
    uint32_t _sa_index;
    bool _is_enabled;