imx: mx7: psci: use C code exclusively

There is no need for assembly in the platform specific part of
the PSCI implementation.

Note that this does not make it a complete PSCI 1.0 implementation
yet but aids to do so in upcoming patches.

Signed-off-by: Stefan Agner <stefan.agner@toradex.com>

arch/arm/mach-imx/mx7/Makefile

@@ -4,7 +4,4 @@
 #
 
 obj-y	:= soc.o clock.o clock_slice.o ddr.o snvs.o
-
-ifdef CONFIG_ARMV7_PSCI
-obj-y  += psci-mx7.o psci.o
-endif
+obj-$(CONFIG_ARMV7_PSCI)  += psci-mx7.o

arch/arm/mach-imx/mx7/psci-mx7.c

@@ -67,23 +67,34 @@ __secure void imx_enable_cpu_ca7(int cpu, bool enable)
 	writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
 }
 
-__secure int imx_cpu_on(int fn, int cpu, int pc)
+__secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep,
+			 u32 context_id)
 {
-	writel(pc, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+	u32 cpu = (mpidr & 0x1);
+
+	psci_save(cpu, ep, context_id);
+
+	writel((u32)psci_cpu_entry, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
 	imx_gpcv2_set_core1_power(true);
 	imx_enable_cpu_ca7(cpu, true);
 	return 0;
 }
 
-__secure int imx_cpu_off(int cpu)
+__secure s32 psci_cpu_off(void)
 {
+	int cpu;
+
+	psci_cpu_off_common();
+	cpu = psci_get_cpu_id();
 	imx_enable_cpu_ca7(cpu, false);
 	imx_gpcv2_set_core1_power(false);
 	writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
-	return 0;
+
+	while (1)
+		wfi();
 }
 
-__secure void imx_system_reset(void)
+__secure void psci_system_reset(void)
 {
 	struct wdog_regs *wdog = (struct wdog_regs *)WDOG1_BASE_ADDR;
 
@@ -91,9 +102,12 @@ __secure void imx_system_reset(void)
 	writel(0x1 << 28, CCM_BASE_ADDR + CCM_ROOT_WDOG);
 	writel(0x3, CCM_BASE_ADDR + CCM_CCGR_WDOG1);
 	writew(WCR_WDE, &wdog->wcr);
+
+	while (1)
+		wfi();
 }
 
-__secure void imx_system_off(void)
+__secure void psci_system_off(void)
 {
 	u32 val;
 
@@ -103,4 +117,7 @@ __secure void imx_system_off(void)
 	val = readl(SNVS_BASE_ADDR + SNVS_LPCR);
 	val |= BP_SNVS_LPCR_DP_EN | BP_SNVS_LPCR_TOP;
 	writel(val, SNVS_BASE_ADDR + SNVS_LPCR);
+
+	while (1)
+		wfi();
 }

arch/arm/mach-imx/mx7/psci.S

@@ -1,60 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ */
-/*
- * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
- * Copyright 2017 NXP
- */
-
-#include <config.h>
-#include <linux/linkage.h>
-
-#include <asm/armv7.h>
-#include <asm/arch-armv7/generictimer.h>
-#include <asm/psci.h>
-
-	.pushsection ._secure.text, "ax"
-
-	.arch_extension sec
-
-.globl psci_cpu_on
-psci_cpu_on:
-	push	{r4, r5, lr}
-
-	mov	r4, r0
-	mov	r5, r1
-	mov	r0, r1
-	mov	r1, r2
-	mov	r2, r3
-	bl	psci_save
-
-	mov	r0, r4
-	mov	r1, r5
-	ldr	r2, =psci_cpu_entry
-	bl	imx_cpu_on
-
-	pop	{r4, r5, pc}
-
-.globl psci_cpu_off
-psci_cpu_off:
-
-	bl	psci_cpu_off_common
-	bl	psci_get_cpu_id
-	bl	imx_cpu_off
-
-1: 	wfi
-	b 1b
-
-.globl psci_system_reset
-psci_system_reset:
-	bl	imx_system_reset
-
-2: 	wfi
-	b 2b
-
-.globl psci_system_off
-psci_system_off:
-	bl	imx_system_off
-
-3: 	wfi
-	b 3b
-
-	.popsection