1 files changed, 88 insertions, 0 deletions
diff --git a/core/arch/arm/plat-hisilicon/psci.c b/core/arch/arm/plat-hisilicon/psci.c
new file mode 100644
index 00000000..dad87030
--- /dev/null
+++ b/core/arch/arm/plat-hisilicon/psci.c
@@ -0,0 +1,88 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2019, HiSilicon Technologies Co., Ltd.
+ */
+
+#include <console.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+
+#define REG_CPU_SUSSYS_RESET	0xcc
+#define REG_CPU_START_COMMAND	0x0
+#define REG_CPU_START_ADDR	0x4
+#define REG_SYSCTRL_RESET	0x4
+#define RELEASE_CORE_MASK	(BIT32(25) | BIT32(1))
+
+int psci_features(uint32_t psci_fid)
+{
+	switch (psci_fid) {
+	case PSCI_PSCI_FEATURES:
+	case PSCI_VERSION:
+	case PSCI_SYSTEM_RESET:
+#ifdef CFG_BOOT_SECONDARY_REQUEST
+	case PSCI_CPU_ON:
+#endif
+		return PSCI_RET_SUCCESS;
+	default:
+		return PSCI_RET_NOT_SUPPORTED;
+	}
+}
+
+uint32_t psci_version(void)
+{
+	return PSCI_VERSION_1_0;
+}
+
+void psci_system_reset(void)
+{
+	vaddr_t sysctrl = core_mmu_get_va(SYS_CTRL_BASE, MEM_AREA_IO_SEC);
+
+	if (!sysctrl) {
+		EMSG("no sysctrl mapping, hang here");
+		panic();
+	}
+
+	io_write32(sysctrl + REG_SYSCTRL_RESET, 0xdeadbeef);
+}
+
+#ifdef CFG_BOOT_SECONDARY_REQUEST
+int psci_cpu_on(uint32_t core_idx, uint32_t entry,
+		uint32_t context_id)
+{
+	uint32_t val = 0;
+	size_t pos = get_core_pos_mpidr(core_idx);
+	vaddr_t bootsram = core_mmu_get_va(BOOTSRAM_BASE, MEM_AREA_IO_SEC);
+	vaddr_t crg = core_mmu_get_va(CPU_CRG_BASE, MEM_AREA_IO_SEC);
+
+	if (!bootsram || !crg) {
+		EMSG("No bootsram or crg mapping");
+		return PSCI_RET_INVALID_PARAMETERS;
+	}
+
+	if ((pos == 0) || (pos >= CFG_TEE_CORE_NB_CORE))
+		return PSCI_RET_INVALID_PARAMETERS;
+
+	/* set secondary core's NS entry addresses */
+	generic_boot_set_core_ns_entry(pos, entry, context_id);
+
+	val = virt_to_phys((void *)TEE_TEXT_VA_START);
+	io_write32(bootsram + REG_CPU_START_ADDR, val);
+	io_write32(bootsram + REG_CPU_START_COMMAND, 0xe51ff004);
+
+	/* release secondary core */
+	io_clrbits32(crg + REG_CPU_SUSSYS_RESET, RELEASE_CORE_MASK);
+
+	return PSCI_RET_SUCCESS;
+}
+#endif