diff --git a/include/lib/extensions/fpu.h b/include/lib/extensions/fpu.h
new file mode 100644
index 0000000..c803081
--- /dev/null
+++ b/include/lib/extensions/fpu.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2023, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FPU_H
+#define FPU_H
+
+/* The FPU and SIMD register bank is 32 quadword (128 bits) Q registers. */
+#define FPU_Q_SIZE		16U
+#define FPU_Q_COUNT		32U
+
+/* These defines are needed by assembly code to access FPU registers. */
+#define FPU_OFFSET_Q		0U
+#define FPU_OFFSET_FPSR		(FPU_Q_SIZE * FPU_Q_COUNT)
+#define FPU_OFFSET_FPCR		(FPU_OFFSET_FPSR + 8)
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+typedef struct fpu_reg_state {
+	uint8_t q[FPU_Q_COUNT][FPU_Q_SIZE];
+	unsigned long fpsr;
+	unsigned long fpcr;
+} fpu_reg_state_t;
+
+/*
+ * Read and compare FPU state registers with provided template values in parameters.
+ */
+bool fpu_state_compare_template(fpu_reg_state_t *fpu);
+
+/*
+ * Fill the template with random values and copy it to
+ * FPU state registers(SIMD vectors, FPCR, FPSR).
+ */
+void fpu_state_fill_regs_and_template(fpu_reg_state_t *fpu);
+
+/*
+ * This function populates the provided FPU structure with the provided template
+ * regs_val for all the 32 FPU/SMID registers, and the status registers FPCR/FPSR
+ */
+void fpu_state_set(fpu_reg_state_t *vec,
+		uint8_t regs_val);
+
+/*
+ * This function prints the content of the provided FPU structure
+ */
+void fpu_state_print(fpu_reg_state_t *vec);
+
+#endif /* __ASSEMBLER__ */
+#endif /* FPU_H */
diff --git a/include/runtime_services/cactus_test_cmds.h b/include/runtime_services/cactus_test_cmds.h
index e52f333..9dc0d90 100644
--- a/include/runtime_services/cactus_test_cmds.h
+++ b/include/runtime_services/cactus_test_cmds.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -283,6 +283,20 @@
 }
 
 /**
+ * Request to compare FPU state(SIMD vectors, FPCR, FPSR) content
+ * with previous template values to check a save/restore routine during the
+ * context switches between secure world and normal world.
+ */
+#define CACTUS_CMP_SIMD_VALUE_CMD (CACTUS_REQ_SIMD_FILL_CMD + 1)
+
+static inline struct ffa_value cactus_req_simd_compare_send_cmd(
+	ffa_id_t source, ffa_id_t dest)
+{
+	return cactus_send_cmd(source, dest, CACTUS_CMP_SIMD_VALUE_CMD, 0, 0, 0,
+			       0);
+}
+
+/**
  * Command to request cactus to sleep for the given time in ms
  *
  * The command id is the hex representation of string "sleep"
diff --git a/include/runtime_services/spm_common.h b/include/runtime_services/spm_common.h
index 6fe445a..7f47dc7 100644
--- a/include/runtime_services/spm_common.h
+++ b/include/runtime_services/spm_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -97,27 +97,15 @@
 void dump_ffa_value(struct ffa_value ret);
 
 /*
- * Max. vector length:
- * SIMD: 128 bits = 16 bytes
- */
-#define SIMD_VECTOR_LEN_BYTES		16
-
-#define SIMD_NUM_VECTORS		32
-
-typedef uint8_t simd_vector_t[SIMD_VECTOR_LEN_BYTES];
-
-/*
  * Fills SIMD/SVE registers with the content of the container v.
  * Number of vectors is assumed to be SIMD/SVE_NUM_VECTORS.
  */
-void fill_simd_vector_regs(const simd_vector_t v[SIMD_NUM_VECTORS]);
 void fill_sve_vector_regs(const sve_vector_t v[SVE_NUM_VECTORS]);
 
 /*
  * Reads contents of SIMD/SVE registers into the provided container v.
  * Number of vectors is assumed to be SIMD/SVE_NUM_VECTORS.
  */
-void read_simd_vector_regs(simd_vector_t v[SIMD_NUM_VECTORS]);
 void read_sve_vector_regs(sve_vector_t v[SVE_NUM_VECTORS]);
 
 bool check_spmc_execution_level(void);