From 102b0d2daa97dae68d3eed54d8fe37a9cc38a892 Mon Sep 17 00:00:00 2001
From: Daniel Baumann <daniel.baumann@progress-linux.org>
Date: Sun, 28 Apr 2024 11:13:47 +0200
Subject: Adding upstream version 2.8.0+dfsg.

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>
---
 plat/arm/board/morello/aarch64/morello_helper.S | 55 +++++++++++++++++++++++++
 1 file changed, 55 insertions(+)
 create mode 100644 plat/arm/board/morello/aarch64/morello_helper.S

(limited to 'plat/arm/board/morello/aarch64/morello_helper.S')

diff --git a/plat/arm/board/morello/aarch64/morello_helper.S b/plat/arm/board/morello/aarch64/morello_helper.S
new file mode 100644
index 0000000..60470a8
--- /dev/null
+++ b/plat/arm/board/morello/aarch64/morello_helper.S
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <cpu_macros.S>
+#include <rainier.h>
+
+#include <platform_def.h>
+
+	.globl	plat_arm_calc_core_pos
+	.globl	plat_reset_handler
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *
+	 * Helper function to calculate the core position.
+	 * ((ChipId * MORELLO_MAX_CLUSTERS_PER_CHIP + ClusterId) *
+	 * MORELLO_MAX_CPUS_PER_CLUSTER * MORELLO_MAX_PE_PER_CPU) +
+	 * (CPUId * MORELLO_MAX_PE_PER_CPU) + ThreadId
+	 *
+	 * which can be simplified as:
+	 *
+	 * (((ChipId * MORELLO_MAX_CLUSTERS_PER_CHIP + ClusterId) *
+	 * MORELLO_MAX_CPUS_PER_CLUSTER + CPUId) * MORELLO_MAX_PE_PER_CPU) +
+	 * ThreadId
+	 * ------------------------------------------------------
+	 */
+
+func plat_arm_calc_core_pos
+	mov	x4, x0
+
+	/*
+	 * The MT bit in MPIDR is always set for morello and the
+	 * affinity level 0 corresponds to thread affinity level.
+	 */
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx	x0, x4, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x1, x4, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x2, x4, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx	x3, x4, #MPIDR_AFF3_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov	x4, #MORELLO_MAX_CLUSTERS_PER_CHIP
+	madd	x2, x3, x4, x2
+	mov	x4, #MORELLO_MAX_CPUS_PER_CLUSTER
+	madd	x1, x2, x4, x1
+	mov	x4, #MORELLO_MAX_PE_PER_CPU
+	madd	x0, x1, x4, x0
+	ret
+endfunc plat_arm_calc_core_pos
-- 
cgit v1.2.3