arm_fpga: Add support for unknown MPIDs

This patch allows the system to fallback to a default CPU library
in case the MPID does not match with any of the supported ones.

This feature can be enabled by setting SUPPORT_UNKNOWN_MPID build
option to 1 (enabled by default only on arm_fpga platform).

This feature can be very dangerous on a production image and
therefore it MUST be disabled for Release images.

Signed-off-by: Javier Almansa Sobrino <javier.almansasobrino@arm.com>
Change-Id: I0df7ef2b012d7d60a4fd5de44dea1fbbb46881ba

2 files changed, 129 insertions, 10 deletions

diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index da663be0e..730b09beb 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2020, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -140,6 +140,13 @@ endfunc do_cpu_reg_dump
 	 * midr of the core. It reads the MIDR_EL1 and finds the matching
 	 * entry in cpu_ops entries. Only the implementation and part number
 	 * are used to match the entries.
+	 *
+	 * If cpu_ops for the MIDR_EL1 cannot be found and
+	 * SUPPORT_UNKNOWN_MPID is enabled, it will try to look for a
+	 * default cpu_ops with an MIDR value of 0.
+	 * (Implementation number 0x0 should be reseverd for software use
+	 * and therefore no clashes should happen with that default value).
+	 *
 	 * Return :
 	 *     x0 - The matching cpu_ops pointer on Success
 	 *     x0 - 0 on failure.
@@ -147,23 +154,26 @@ endfunc do_cpu_reg_dump
 	 */
 	.globl	get_cpu_ops_ptr
 func get_cpu_ops_ptr
-	/* Get the cpu_ops start and end locations */
-	adr	x4, (__CPU_OPS_START__ + CPU_MIDR)
-	adr	x5, (__CPU_OPS_END__ + CPU_MIDR)
-
-	/* Initialize the return parameter */
-	mov	x0, #0
-
 	/* Read the MIDR_EL1 */
 	mrs	x2, midr_el1
 	mov_imm	x3, CPU_IMPL_PN_MASK
 
 	/* Retain only the implementation and part number using mask */
 	and	w2, w2, w3
+
+	/* Get the cpu_ops end location */
+	adr	x5, (__CPU_OPS_END__ + CPU_MIDR)
+
+	/* Initialize the return parameter */
+	mov	x0, #0
 1:
+	/* Get the cpu_ops start location */
+	adr	x4, (__CPU_OPS_START__ + CPU_MIDR)
+
+2:
 	/* Check if we have reached end of list */
 	cmp	x4, x5
-	b.eq	error_exit
+	b.eq	search_def_ptr
 
 	/* load the midr from the cpu_ops */
 	ldr	x1, [x4], #CPU_OPS_SIZE
@@ -171,7 +181,7 @@ func get_cpu_ops_ptr
 
 	/* Check if midr matches to midr of this core */
 	cmp	w1, w2
-	b.ne	1b
+	b.ne	2b
 
 	/* Subtract the increment and offset to get the cpu-ops pointer */
 	sub	x0, x4, #(CPU_OPS_SIZE + CPU_MIDR)
@@ -179,7 +189,27 @@ func get_cpu_ops_ptr
 	cmp	x0, #0
 	ASM_ASSERT(ne)
 #endif
+#ifdef SUPPORT_UNKNOWN_MPID
+	cbnz	x2, exit_mpid_found
+	/* Mark the unsupported MPID flag */
+	adrp	x1, unsupported_mpid_flag
+	add	x1, x1, :lo12:unsupported_mpid_flag
+	str	w2, [x1]
+exit_mpid_found:
+#endif
+	ret
+
+	/*
+	 * Search again for a default pointer (MIDR = 0x0)
+	 * or return error if already searched.
+	 */
+search_def_ptr:
+#ifdef SUPPORT_UNKNOWN_MPID
+	cbz	x2, error_exit
+	mov	x2, #0
+	b	1b
 error_exit:
+#endif
 	ret
 endfunc get_cpu_ops_ptr
 
diff --git a/lib/cpus/aarch64/generic.S b/lib/cpus/aarch64/generic.S
new file mode 100644
index 000000000..ef1f048a1
--- /dev/null
+++ b/lib/cpus/aarch64/generic.S
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <generic.h>
+#include <cpu_macros.S>
+#include <plat_macros.S>
+
+	/* ---------------------------------------------
+	 * Disable L1 data cache and unified L2 cache
+	 * ---------------------------------------------
+	 */
+func generic_disable_dcache
+	mrs	x1, sctlr_el3
+	bic	x1, x1, #SCTLR_C_BIT
+	msr	sctlr_el3, x1
+	isb
+	ret
+endfunc generic_disable_dcache
+
+func generic_core_pwr_dwn
+	mov	x18, x30
+
+	/* ---------------------------------------------
+	 * Turn off caches.
+	 * ---------------------------------------------
+	 */
+	bl	generic_disable_dcache
+
+	/* ---------------------------------------------
+	 * Flush L1 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level1
+
+	ret	x18
+endfunc generic_core_pwr_dwn
+
+func generic_cluster_pwr_dwn
+	mov	x18, x30
+
+	/* ---------------------------------------------
+	 * Turn off caches.
+	 * ---------------------------------------------
+	 */
+	bl	generic_disable_dcache
+
+	/* ---------------------------------------------
+	 * Flush L1 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level1
+
+	/* ---------------------------------------------
+	 * Disable the optional ACP.
+	 * ---------------------------------------------
+	 */
+	bl	plat_disable_acp
+
+	/* ---------------------------------------------
+	 * Flush L2 caches.
+	 * ---------------------------------------------
+	 */
+	mov	x0, #DCCISW
+	bl	dcsw_op_level2
+
+	ret	x18
+
+endfunc generic_cluster_pwr_dwn
+
+/* ---------------------------------------------
+ * Unimplemented functions.
+ * ---------------------------------------------
+ */
+.equ	generic_errata_report,		0
+.equ	generic_cpu_reg_dump,		0
+.equ	generic_reset_func,		0
+
+declare_cpu_ops generic, AARCH64_GENERIC_MIDR, \
+	generic_reset_func, \
+	generic_core_pwr_dwn, \
+	generic_cluster_pwr_dwn