diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index de7d33b..1b123c6 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -59,6 +59,7 @@
 #define MPIDR_MAX_AFFLVL	U(2)
 
 #define MPID_MASK		(MPIDR_MT_MASK				 | \
+				 (MPIDR_AFFLVL_MASK << MPIDR_AFF3_SHIFT) | \
 				 (MPIDR_AFFLVL_MASK << MPIDR_AFF2_SHIFT) | \
 				 (MPIDR_AFFLVL_MASK << MPIDR_AFF1_SHIFT) | \
 				 (MPIDR_AFFLVL_MASK << MPIDR_AFF0_SHIFT))
diff --git a/tftf/framework/aarch64/entrypoint.S b/tftf/framework/aarch64/entrypoint.S
index 1d524d2..34ba90f 100644
--- a/tftf/framework/aarch64/entrypoint.S
+++ b/tftf/framework/aarch64/entrypoint.S
@@ -142,7 +142,8 @@
 	 * --------------------------------------------------------------------
 	 */
 	mrs	x0, mpidr_el1
-	and	x0, x0, #MPID_MASK
+	mov_imm x1, MPID_MASK
+	and	x0, x0, x1
 	bl	platform_get_core_pos
 
 	mov	x1, x19
@@ -167,7 +168,7 @@
 	mov	w2, #INVALID_MPID
 	cmp	w0, w2
 	b.ne	panic
-	mov	x2, #MPID_MASK
+	mov_imm	x2, MPID_MASK
 	mrs	x0, mpidr_el1
 	and	x0, x0, x2
 	str	w0, [x1, :lo12:tftf_primary_core]
diff --git a/tftf/tests/runtime_services/standard_service/sdei/system_tests/sdei_entrypoint.S b/tftf/tests/runtime_services/standard_service/sdei/system_tests/sdei_entrypoint.S
index aae85fc..27ffa39 100644
--- a/tftf/tests/runtime_services/standard_service/sdei/system_tests/sdei_entrypoint.S
+++ b/tftf/tests/runtime_services/standard_service/sdei/system_tests/sdei_entrypoint.S
@@ -35,7 +35,8 @@
 
 	/* Calculate address of event completion variable */
 	mrs	x0, mpidr_el1
-	and	x0, x0, #MPID_MASK
+	mov_imm	x1, MPID_MASK
+	and	x0, x0, x1
 	bl	platform_get_core_pos
 	lsl	x0, x0, #2
 	adrp	x1, event_handled
@@ -59,7 +60,8 @@
 
 	/* Calculate address of event completion variable */
 	mrs	x0, mpidr_el1
-	and	x0, x0, #MPID_MASK
+	mov_imm	x1, MPID_MASK
+	and	x0, x0, x1
 	mov	x29, x30
 	bl	platform_get_core_pos
 	mov	x30, x29
@@ -93,7 +95,8 @@
 
 	/* Calculate address of event completion variable */
 	mrs	x0, mpidr_el1
-	and	x0, x0, #MPID_MASK
+	mov_imm	x1, MPID_MASK
+	and	x0, x0, x1
 	bl	platform_get_core_pos
 	lsl	x0, x0, #2
 	adrp	x1, event_handled