Hotplug: Introduce functionality to KFSCB trap handling.

When Linux writes to the RST_HOLD0 register to hotplug a cpu, the
virtualizer saves the context and then ask the secure world to
shutdown this particular cpu. All other accesses to the KFSCB are
simply passed to the KFSCB.

Signed-off-by: Dietmar Eggemann <dietmar.eggemann@arm.com>

1 files changed, 38 insertions, 4 deletions

diff --git a/big-little/virtualisor/kfscb_trap_handler.c b/big-little/virtualisor/kfscb_trap_handler.c
index f6c61f1..7813cce 100644
--- a/big-little/virtualisor/kfscb_trap_handler.c
+++ b/big-little/virtualisor/kfscb_trap_handler.c
@@ -21,6 +21,8 @@
  */
 
 #include "misc.h"
+#include "virt_helpers.h"
+#include "context.h"
 
 /*
  * Whether A15 or A7, the KFSCB accesses are virtualised in exactly the same
@@ -28,10 +30,42 @@
  */
 void handle_kfscb_abort(unsigned pa, unsigned *data, unsigned write)
 {
-	if (write) {
-		write32(pa, *data);
-	} else {
-		*data = read32(pa);
+	unsigned reg_offset = pa & 0xfff;
+	unsigned cluster_id = read_clusterid();
+
+	/*
+	 * The OS always asks for the Eagle resets.
+	 * We have to be more intelligent and return
+	 * the apt value.
+	 */
+	if (cluster_id == KFC)
+		pa += 4;
+
+	switch (reg_offset) {
+
+	/* Access to KFSCB registers */
+	case (RST_HOLD0):
+		if (write) {
+			/* Entry */
+			if (*data & (*data ^ read32(pa + 0xc))) {
+				save_context(find_first_cpu(), OP_TYPE_HP);
+				smc(SMC_SEC_SHUTDOWN, *data, OP_TYPE_HP);
+			} else {
+				/* Exit */
+				write32(pa, *data);
+			}
+		} else {
+			*data = read32(pa);
+		}
+
+		break;
+
+	default:
+		if (write) {
+			write32(pa, *data);
+		} else {
+			*data = read32(pa);
+		}
 	}
 
 	return;