diff --git a/programs/test/generate_cpp_dummy_build.sh b/programs/test/generate_cpp_dummy_build.sh
index d27c7ae..05bdd34 100755
--- a/programs/test/generate_cpp_dummy_build.sh
+++ b/programs/test/generate_cpp_dummy_build.sh
@@ -52,6 +52,10 @@
         esac
     done
 
+    for header in tf-psa-crypto/include/mbedtls/*.h; do
+        echo "#include \"${header#tf-psa-crypto/include/}\""
+    done
+
     for header in tf-psa-crypto/include/psa/*.h; do
         case ${header#tf-psa-crypto/include/} in
             psa/crypto_config.h) :;; # not meant for direct inclusion
diff --git a/tests/Makefile b/tests/Makefile
index 87a6ca1..783f766 100644
--- a/tests/Makefile
+++ b/tests/Makefile
@@ -324,25 +324,6 @@
 	perl -p -e 's/^(# *(define|ifndef) +\w+_)H\b/$${1}ALT_H/' $< >$@
 
 # Generate test library
-
-# Perl code that is executed to transform each original line from a library
-# source file into the corresponding line in the test driver copy of the
-# library. Add a LIBTESTDRIVER1_/libtestdriver1_ to mbedtls_xxx and psa_xxx
-# symbols.
-define libtestdriver1_rewrite :=
-	s!^(\s*#\s*include\s*[\"<])mbedtls/build_info.h!$${1}libtestdriver1/include/mbedtls/build_info.h!; \
-	s!^(\s*#\s*include\s*[\"<])mbedtls/mbedtls_config.h!$${1}libtestdriver1/include/mbedtls/mbedtls_config.h!; \
-	s!^(\s*#\s*include\s*[\"<])mbedtls/config_adjust_x509.h!$${1}libtestdriver1/include/mbedtls/config_adjust_x509.h!; \
-	s!^(\s*#\s*include\s*[\"<])mbedtls/config_adjust_ssl.h!$${1}libtestdriver1/include/mbedtls/config_adjust_ssl.h!; \
-	s!^(\s*#\s*include\s*[\"<])mbedtls/check_config.h!$${1}libtestdriver1/include/mbedtls/check_config.h!; \
-	s!^(\s*#\s*include\s*[\"<])mbedtls/!$${1}libtestdriver1/tf-psa-crypto/drivers/builtin/include/mbedtls/!; \
-	s!^(\s*#\s*include\s*[\"<])psa/!$${1}libtestdriver1/tf-psa-crypto/include/psa/!; \
-	s!^(\s*#\s*include\s*[\"<])tf-psa-crypto/!$${1}libtestdriver1/tf-psa-crypto/include/tf-psa-crypto/!; \
-	next if /^\s*#\s*include/; \
-	s/\b(?=MBEDTLS_|PSA_|TF_PSA_CRYPTO_)/LIBTESTDRIVER1_/g; \
-	s/\b(?=mbedtls_|psa_|tf_psa_crypto_)/libtestdriver1_/g;
-endef
-
 libtestdriver1.a:
 	rm -Rf ./libtestdriver1
 	mkdir ./libtestdriver1
@@ -384,12 +365,12 @@
 	# Prefix MBEDTLS_* PSA_* symbols with LIBTESTDRIVER1_ as well as
 	# mbedtls_* psa_* symbols with libtestdriver1_ to avoid symbol clash
 	# when this test driver library is linked with the Mbed TLS library.
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/library/*.[ch]
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/include/*/*.h
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/tf-psa-crypto/core/*.[ch]
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/tf-psa-crypto/include/*/*.h
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/tf-psa-crypto/drivers/builtin/include/*/*.h
-	perl -pi -e '$(libtestdriver1_rewrite)' ./libtestdriver1/tf-psa-crypto/drivers/builtin/src/*.[ch]
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/library/*.[ch]
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/include/*/*.h
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/tf-psa-crypto/core/*.[ch]
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/tf-psa-crypto/include/*/*.h
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/tf-psa-crypto/drivers/builtin/include/*/*.h
+	perl -i ./libtestdriver1_rewrite.pl ./libtestdriver1/tf-psa-crypto/drivers/builtin/src/*.[ch]
 
 	$(MAKE) -C ./libtestdriver1/library CFLAGS="-I../../ $(CFLAGS)" LDFLAGS="$(LDFLAGS)" libmbedcrypto.a
 	cp ./libtestdriver1/library/libmbedcrypto.a ../library/libtestdriver1.a
diff --git a/tests/libtestdriver1_rewrite.pl b/tests/libtestdriver1_rewrite.pl
new file mode 100644
index 0000000..c9790bb
--- /dev/null
+++ b/tests/libtestdriver1_rewrite.pl
@@ -0,0 +1,41 @@
+#!/usr/bin/perl
+
+# Perl code that is executed to transform each original line from a library
+# source file into the corresponding line in the test driver copy of the
+# library. Add a LIBTESTDRIVER1_/libtestdriver1_ to mbedtls_xxx and psa_xxx
+# symbols.
+
+# Copyright The Mbed TLS Contributors
+# SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later
+
+use warnings;
+use File::Basename;
+
+my @public_files = map { basename($_) } glob("../tf-psa-crypto/include/mbedtls/*.h");
+
+my $public_files_regex = join('|', map { quotemeta($_) } @public_files);
+
+while (<>) {
+    s!^(\s*#\s*include\s*[\"<])mbedtls/build_info.h!${1}libtestdriver1/include/mbedtls/build_info.h!;
+    s!^(\s*#\s*include\s*[\"<])mbedtls/mbedtls_config.h!${1}libtestdriver1/include/mbedtls/mbedtls_config.h!;
+    s!^(\s*#\s*include\s*[\"<])mbedtls/config_adjust_x509.h!${1}libtestdriver1/include/mbedtls/config_adjust_x509.h!;
+    s!^(\s*#\s*include\s*[\"<])mbedtls/config_adjust_ssl.h!${1}libtestdriver1/include/mbedtls/config_adjust_ssl.h!;
+    s!^(\s*#\s*include\s*[\"<])mbedtls/check_config.h!${1}libtestdriver1/include/mbedtls/check_config.h!;
+    # Files in include/mbedtls and drivers/builtin/include/mbedtls are both
+    # included in files via #include mbedtls/<file>.h, so when expanding to the
+    # full path make sure that files in include/mbedtls are not expanded
+    # to driver/builtin/include/mbedtls.
+    if ( $public_files_regex ) {
+        s!^(\s*#\s*include\s*[\"<])mbedtls/($public_files_regex)!${1}libtestdriver1/tf-psa-crypto/include/mbedtls/${2}!;
+    }
+    s!^(\s*#\s*include\s*[\"<])mbedtls/!${1}libtestdriver1/tf-psa-crypto/drivers/builtin/include/mbedtls/!;
+    s!^(\s*#\s*include\s*[\"<])psa/!${1}libtestdriver1/tf-psa-crypto/include/psa/!;
+    s!^(\s*#\s*include\s*[\"<])tf-psa-crypto/!${1}libtestdriver1/tf-psa-crypto/include/tf-psa-crypto/!;
+    if (/^\s*#\s*include/) {
+        print;
+        next;
+    }
+    s/\b(?=MBEDTLS_|PSA_|TF_PSA_CRYPTO_)/LIBTESTDRIVER1_/g;
+    s/\b(?=mbedtls_|psa_|tf_psa_crypto_)/libtestdriver1_/g;
+    print;
+}
diff --git a/tf-psa-crypto b/tf-psa-crypto
index 4a9f29b..d653d1b 160000
--- a/tf-psa-crypto
+++ b/tf-psa-crypto
@@ -1 +1 @@
-Subproject commit 4a9f29b05c661bd874c75d80339fcce00adea4e0
+Subproject commit d653d1b02d71d1579bc6e6281a2f9ef814eea3e9