diff --git a/lib_system/LibItsSecurity_Functions.ttcn b/lib_system/LibItsSecurity_Functions.ttcn
index 390707608350ccaed29142a53662d03f3c61361a..0bf668bb9e8e24714c42e9f1775312c1771e4b5c 100644
--- a/lib_system/LibItsSecurity_Functions.ttcn
+++ b/lib_system/LibItsSecurity_Functions.ttcn
@@ -358,6 +358,64 @@ module LibItsSecurity_Functions {
 
         } // End of function f_signWithEcdsaBrainpoolp384r1WithSha384
 
+        function f_decryptIeee1609Dot2Data(inout Ieee1609Dot2Data p_encrypedSecuredMessage,
+                                           out octetstring        p_aes_sym_enc_key
+        ) return boolean {
+            if (not ischosen(p_encrypedSecuredMessage.content.encryptedData)) {
+                return false;
+            }
+            var octetstring v_decryptedSecuredMessage;
+            var PKRecipientInfo v_pKRecipientInfo;
+            var Oct32 v_encryptingPrivateKey;
+            var Oct32 v_salt;
+            var RecipientInfo v_recipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0];
+            var SymmetricCiphertext v_ciphertext := p_encrypedSecuredMessage.content.encryptedData.ciphertext;
+            if (ischosen(v_recipientInfo.certRecipInfo)) {
+                // Read the certificate based on the recipientId
+                var charstring v_certificate_id;
+                v_pKRecipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0].certRecipInfo;
+                if(not fx_readCertificateFromDigest(v_pKRecipientInfo.recipientId, v_certificate_id)){
+                    return false;
+                }
+                if(not fx_readEncryptingKey(v_certificate_id, v_encryptingPrivateKey)) {
+                    return false;
+                }
+                if(not fx_readCertificateHash(v_certificate_id, v_salt)){
+                    return false;
+                }
+            } else {
+                log("*** " & testcasename() & ":ERROR: Unsupported RecipientInfo variant ***");
+                return false;
+            }
+            if (ischosen(v_pKRecipientInfo.encKey.eciesNistP256.v.compressed_y_0)) {
+                v_decryptedSecuredMessage := fx_decryptWithEciesNistp256WithSha256(
+                                                                                v_ciphertext.aes128ccm.ccmCiphertext,
+                                                                                v_encryptingPrivateKey,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.v.compressed_y_0, 0,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.c,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.t,
+                                                                                v_ciphertext.aes128ccm.nonce,
+                                                                                v_salt,
+                                                                                p_aes_sym_enc_key
+                                                                                );
+            } else if (ischosen(v_pKRecipientInfo.encKey.eciesNistP256.v.compressed_y_1)) {
+                v_decryptedSecuredMessage := fx_decryptWithEciesNistp256WithSha256(
+                                                                                v_ciphertext.aes128ccm.ccmCiphertext,
+                                                                                v_encryptingPrivateKey,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.v.compressed_y_1, 1,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.c,
+                                                                                v_pKRecipientInfo.encKey.eciesNistP256.t,
+                                                                                v_ciphertext.aes128ccm.nonce,
+                                                                                v_salt,
+                                                                                p_aes_sym_enc_key                                                                                
+                                                                                );
+            }else{
+                return false;
+            }
+            p_encrypedSecuredMessage.content.encryptedData.ciphertext.aes128ccm.ccmCiphertext := v_decryptedSecuredMessage;
+            return true;
+        }
+
         function f_decrypt(
                            in octetstring         p_encryptPrivateKey,
                            in EtsiTs103097Data    p_encrypedSecuredMessage,
@@ -384,7 +442,7 @@ module LibItsSecurity_Functions {
                   v_pKRecipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0].certRecipInfo;
                   // Read the certificate based on the recipientId
                 } else if (ischosen(v_recipientInfo.signedDataRecipInfo)) {
-                v_pKRecipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0].signedDataRecipInfo;
+                  v_pKRecipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0].signedDataRecipInfo;
                   // Read the certificate based on the recipientId
                 } else {
                   log("*** " & testcasename() & ":ERROR: Unsupported RecipientInfo variant ***");
@@ -486,6 +544,137 @@ module LibItsSecurity_Functions {
             return false;
         } // End of function f_decrypt
 
+        function f_encryptData(in octetstring p_inData,
+                               in PublicEncryptionKey p_pubEncKey,
+                               in Oct32 p_salt,
+                               inout EncryptedDataEncryptionKey p_outKey,
+                               out octetstring p_outData,
+                               out octetstring p_aesKey,
+                               out octetstring p_nonce
+        ) return boolean {
+            if(ischosen(p_pubEncKey.publicKey.eciesNistP256)){
+                var octetstring v_public_enc_key;
+                var integer v_compressed_enc_key_mode;
+                if(ischosen(p_pubEncKey.publicKey.eciesNistP256.compressed_y_0)){
+                    v_compressed_enc_key_mode := 0;
+                    v_public_enc_key := p_pubEncKey.publicKey.eciesNistP256.compressed_y_0;
+                }else if(ischosen(p_pubEncKey.publicKey.eciesNistP256.compressed_y_1)){
+                    v_compressed_enc_key_mode := 1;
+                    v_public_enc_key := p_pubEncKey.publicKey.eciesNistP256.compressed_y_1;
+                }else{
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: Unsupported key point type: ", p_pubEncKey.publicKey.eciesNistP256);
+                    return false;
+                }
+                p_outData := fx_encryptWithEciesNistp256WithSha256( p_inData,
+                                                                    v_public_enc_key, v_compressed_enc_key_mode,
+                                                                    p_salt,
+                                                                    p_outKey.eciesNistP256.v.compressed_y_0, v_compressed_enc_key_mode,
+                                                                    p_aesKey,
+                                                                    p_outKey.eciesNistP256.c, p_outKey.eciesNistP256.t,
+                                                                    p_nonce);
+                if(v_compressed_enc_key_mode != 0){
+                    p_outKey.eciesNistP256.v.compressed_y_1 := p_outKey.eciesNistP256.v.compressed_y_0;
+                }
+                                                                   
+            } else if(ischosen(p_pubEncKey.publicKey.eciesBrainpoolP256r1)){
+                var octetstring v_public_enc_key;
+                var integer v_compressed_enc_key_mode;
+                var Oct32 v_publicEphemeralKeyCompressed;
+                if(ischosen(p_pubEncKey.publicKey.eciesBrainpoolP256r1.compressed_y_0)){
+                    v_compressed_enc_key_mode := 0;
+                    v_public_enc_key := p_pubEncKey.publicKey.eciesBrainpoolP256r1.compressed_y_0;
+                }else if(ischosen(p_pubEncKey.publicKey.eciesBrainpoolP256r1.compressed_y_1)){
+                    v_compressed_enc_key_mode := 1;
+                    v_public_enc_key := p_pubEncKey.publicKey.eciesBrainpoolP256r1.compressed_y_1;
+                }else{
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: Unsupported key point type: ", p_pubEncKey.publicKey.eciesBrainpoolP256r1);
+                    return false;
+                }
+                p_outData := fx_encryptWithEciesBrainpoolp256r1WithSha256(  p_inData,
+                                                                            v_public_enc_key, v_compressed_enc_key_mode,
+                                                                            p_salt,
+                                                                            p_outKey.eciesBrainpoolP256r1.v.compressed_y_0, v_compressed_enc_key_mode,
+                                                                            p_aesKey,
+                                                                            p_outKey.eciesBrainpoolP256r1.c, p_outKey.eciesBrainpoolP256r1.t,
+                                                                            p_nonce);
+                if(v_compressed_enc_key_mode != 0){
+                    p_outKey.eciesBrainpoolP256r1.v.compressed_y_1 := p_outKey.eciesBrainpoolP256r1.v.compressed_y_0;
+                }
+/*
+            } else if(ischosen(p_pubEncKey.publicKey.ecencSm2)){
+                var octetstring v_public_enc_key;
+                var integer v_compressed_enc_key_mode;
+                if(ischosen(p_pubEncKey.publicKey.ecencSm2.compressed_y_0)){
+                    v_compressed_enc_key_mode := 0;
+                    v_public_enc_key := p_pubEncKey.publicKey.ecencSm2.compressed_y_0;
+                }else if(ischosen(p_pubEncKey.publicKey.ecencSm2.compressed_y_1)){
+                    v_compressed_enc_key_mode := 1;
+                    v_public_enc_key := p_pubEncKey.publicKey.ecencSm2.compressed_y_1;
+                }else{
+                    return false;
+                }
+                p_outData := fx_encryptWithEciesSm2p256WithSha256(  p_inData,
+                                                                    v_public_enc_key, v_compressed_enc_key_mode,
+                                                                    p_salt,
+                                                                    p_outKey.ecencSm2256.v.compressed_y_0, v_compressed_enc_key_mode,
+                                                                    p_aesKey,
+                                                                    p_outKey.ecencSm2256.c, p_outKey.ecencSm2256.t,
+                                                                    p_nonce);
+                if(v_compressed_enc_key_mode != 0){
+                    p_outKey.ecencSm2256.v.compressed_y_1 := p_outKey.ecencSm2256.v.compressed_y_0;
+                }
+*/
+            }else{
+                log(">>>> f_encryptIeee1609Dot2Data: ERROR: Unsupported public key: ", p_pubEncKey);
+                return false;
+            }
+            log(">>>> f_encryptIeee1609Dot2Data: ", p_outKey );
+            return true;
+        }
+
+        function f_encryptIeee1609Dot2Data( inout Ieee1609Dot2Data p_encrypedSecuredMessage,
+                                            out Oct16 p_aesKey
+        ) return boolean {
+            var octetstring v_decryptedSecuredMessage;
+            var Oct32 v_salt;
+            var RecipientInfo v_recipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0];
+            var SymmetricCiphertext v_ciphertext := p_encrypedSecuredMessage.content.encryptedData.ciphertext;
+            
+            if (ischosen(v_recipientInfo.certRecipInfo)) {
+                // Read the certificate based on the recipientId
+                var charstring v_certificate_id;
+                var EtsiTs103097Certificate v_cert;
+                if(lengthof(p_encrypedSecuredMessage.content.encryptedData.recipients) == 0){
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: No recipients found");
+                    return false;
+                }
+
+                var PKRecipientInfo v_pKRecipientInfo := p_encrypedSecuredMessage.content.encryptedData.recipients[0].certRecipInfo;
+                if( not f_getCertificateFromDigest(v_pKRecipientInfo.recipientId, v_cert, v_certificate_id)){
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: No certificates with digest found: ", v_pKRecipientInfo.recipientId);
+                    return false;
+                }
+                if( not fx_readCertificateHash(v_certificate_id, v_salt)){
+                    return false;
+                }
+                if(not f_encryptData(   v_ciphertext.aes128ccm.ccmCiphertext,
+                                        v_cert.toBeSigned.encryptionKey,
+                                        v_salt,
+                                        v_pKRecipientInfo.encKey,
+                                        v_ciphertext.aes128ccm.ccmCiphertext,
+                                        p_aesKey,
+                                        v_ciphertext.aes128ccm.nonce
+                )){
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: Encryption failed");
+                    return false;
+                }
+            }else{
+                    log(">>>> f_encryptIeee1609Dot2Data: ERROR: Not an encrypted message");
+                return false;
+            }
+            return true;
+        }
+
         /**
          * @desc    Produces a Elliptic Curve Digital Encrytion Algorithm (ECIES) encryption using Nist-P256 algorithm
          * @param   p_toBeEncryptedSecuredMessage    The data to be encrypted
@@ -1118,18 +1307,32 @@ module LibItsSecurity_Functions {
         * @return  the HashedId8 value
         * @see Draft ETSI TS 103 097 V1.1.14 Clause 4.2.13   HashedId8
         */
-        function f_calculateDigestFromCertificate(
-                                                  in Certificate p_cert
-        ) return HashedId8 {
-            var octetstring v_hash;
+        function f_calculateCertificateHash(
+                                            in Certificate p_cert
+        ) return octetstring {
             var HashAlgorithm ha := f_getHashAlgorithmOfVerificationKeyIndicator(p_cert.toBeSigned.verifyKeyIndicator);
+            var octetstring v_toBeHashedData;
 
+            v_toBeHashedData := bit2oct(encvalue(p_cert));
             if (ha == sha256) {
-                v_hash := f_calculateDigestSha256FromCertificate(p_cert);
-            }else if (ha == sha384) {
-                v_hash := f_calculateDigestSha384FromCertificate(p_cert);
-            } 
+                return f_hashWithSha256(v_toBeHashedData); 
+            } else if (ha == sha384) {
+                return f_hashWithSha384(v_toBeHashedData); 
+            }
+            return ''O;
+
+        }
 
+        /**
+        * @desc    Calculate digest over the certificate
+        * @param   p_cert The certificate
+        * @return  the HashedId8 value
+        * @see Draft ETSI TS 103 097 V1.1.14 Clause 4.2.13   HashedId8
+        */
+        function f_calculateDigestFromCertificate(
+                                                  in Certificate p_cert
+        ) return HashedId8 {
+            var octetstring v_hash := f_calculateCertificateHash(p_cert);
             return substr(v_hash, lengthof(v_hash) - 8, 8);
         } // End of function f_calculateDigestFromCertificate