From f96cdf076bcc4a12af7670c5cb3ff584d2adf988 Mon Sep 17 00:00:00 2001
From: Tobias Schramm <t.schramm@manjaro.org>
Date: Thu, 28 May 2020 14:11:05 +0200
Subject: [PATCH 02/22] firmware: Add Rockchip SIP driver

Used exclusively for suspend signaling. Drop for mainline and
use PSCI

Signed-off-by: Tobias Schramm <t.schramm@manjaro.org>
---
 drivers/firmware/Kconfig              |   7 +
 drivers/firmware/Makefile             |   1 +
 drivers/firmware/rockchip_sip.c       | 262 ++++++++++++++++++++++++++
 include/linux/rockchip/rockchip_sip.h | 149 +++++++++++++++
 4 files changed, 419 insertions(+)
 create mode 100644 drivers/firmware/rockchip_sip.c
 create mode 100644 include/linux/rockchip/rockchip_sip.h

diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 3315e3c21586..1dfa18dde097 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -250,6 +250,13 @@ config QCOM_SCM_DOWNLOAD_MODE_DEFAULT
 
 	  Say Y here to enable "download mode" by default.
 
+config ROCKCHIP_SIP
+	bool "Rockchip SIP interface"
+	depends on ARM64 && ARM_PSCI_FW
+	help
+	  Say Y here if you want to enable SIP callbacks for Rockchip platforms
+	  This option enables support for communicating with the ATF.
+
 config TI_SCI_PROTOCOL
 	tristate "TI System Control Interface (TISCI) Message Protocol"
 	depends on TI_MESSAGE_MANAGER
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index 5e013b6a3692..85c40483e5fa 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -28,6 +28,7 @@ obj-y				+= meson/
 obj-$(CONFIG_GOOGLE_FIRMWARE)	+= google/
 obj-$(CONFIG_EFI)		+= efi/
 obj-$(CONFIG_UEFI_CPER)		+= efi/
+obj-$(CONFIG_ROCKCHIP_SIP)	+= rockchip_sip.o
 obj-y				+= imx/
 obj-y				+= psci/
 obj-y				+= smccc/
diff --git a/drivers/firmware/rockchip_sip.c b/drivers/firmware/rockchip_sip.c
new file mode 100644
index 000000000000..6ed780c587e1
--- /dev/null
+++ b/drivers/firmware/rockchip_sip.c
@@ -0,0 +1,262 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Copyright (C) 2016, Fuzhou Rockchip Electronics Co., Ltd
+ */
+
+#include <linux/arm-smccc.h>
+#include <linux/io.h>
+#include <linux/rockchip/rockchip_sip.h>
+#include <asm/cputype.h>
+#include <asm/smp_plat.h>
+#include <uapi/linux/psci.h>
+#include <linux/ptrace.h>
+
+#ifdef CONFIG_64BIT
+#define PSCI_FN_NATIVE(version, name)	PSCI_##version##_FN64_##name
+#else
+#define PSCI_FN_NATIVE(version, name)	PSCI_##version##_FN_##name
+#endif
+
+#define SIZE_PAGE(n)	((n) << 12)
+
+static struct arm_smccc_res __invoke_sip_fn_smc(unsigned long function_id,
+						unsigned long arg0,
+						unsigned long arg1,
+						unsigned long arg2)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+	return res;
+}
+
+struct arm_smccc_res sip_smc_ddr_cfg(u32 arg0, u32 arg1, u32 arg2)
+{
+	return __invoke_sip_fn_smc(SIP_DDR_CFG, arg0, arg1, arg2);
+}
+
+struct arm_smccc_res sip_smc_get_atf_version(void)
+{
+	return __invoke_sip_fn_smc(SIP_ATF_VERSION, 0, 0, 0);
+}
+
+struct arm_smccc_res sip_smc_get_sip_version(void)
+{
+	return __invoke_sip_fn_smc(SIP_SIP_VERSION, 0, 0, 0);
+}
+
+int sip_smc_set_suspend_mode(u32 ctrl, u32 config1, u32 config2)
+{
+	struct arm_smccc_res res;
+
+	res = __invoke_sip_fn_smc(SIP_SUSPEND_MODE, ctrl, config1, config2);
+	return res.a0;
+}
+
+int sip_smc_virtual_poweroff(void)
+{
+	struct arm_smccc_res res;
+
+	res = __invoke_sip_fn_smc(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND), 0, 0, 0);
+	return res.a0;
+}
+
+struct arm_smccc_res sip_smc_request_share_mem(u32 page_num,
+					       share_page_type_t page_type)
+{
+	struct arm_smccc_res res;
+	unsigned long share_mem_phy;
+
+	res = __invoke_sip_fn_smc(SIP_SHARE_MEM, page_num, page_type, 0);
+	if (IS_SIP_ERROR(res.a0))
+		goto error;
+
+	share_mem_phy = res.a1;
+	res.a1 = (unsigned long)ioremap(share_mem_phy, SIZE_PAGE(page_num));
+
+error:
+	return res;
+}
+
+struct arm_smccc_res sip_smc_mcu_el3fiq(u32 arg0, u32 arg1, u32 arg2)
+{
+	return __invoke_sip_fn_smc(SIP_MCU_EL3FIQ_CFG, arg0, arg1, arg2);
+}
+
+/************************** fiq debugger **************************************/
+#ifdef CONFIG_ARM64
+#define SIP_UARTDBG_FN		SIP_UARTDBG_CFG64
+#else
+#define SIP_UARTDBG_FN		SIP_UARTDBG_CFG
+#endif
+
+static int fiq_sip_enabled;
+static int fiq_target_cpu;
+static phys_addr_t ft_fiq_mem_phy;
+static void __iomem *ft_fiq_mem_base;
+static void (*sip_fiq_debugger_uart_irq_tf)(struct pt_regs _pt_regs,
+					    unsigned long cpu);
+int sip_fiq_debugger_is_enabled(void)
+{
+	return fiq_sip_enabled;
+}
+
+static struct pt_regs sip_fiq_debugger_get_pt_regs(void *reg_base,
+						   unsigned long sp_el1)
+{
+	struct pt_regs fiq_pt_regs;
+
+#ifdef CONFIG_ARM64
+	/* copy cpu context */
+	memcpy(&fiq_pt_regs, reg_base, 8 * 31);
+
+	/* copy pstate */
+	memcpy(&fiq_pt_regs.pstate, reg_base + 0x110, 8);
+
+	/* EL1 mode */
+	if (fiq_pt_regs.pstate & 0x10)
+		memcpy(&fiq_pt_regs.sp, reg_base + 0xf8, 8);
+	/* EL0 mode */
+	else
+		fiq_pt_regs.sp = sp_el1;
+
+	/* copy pc */
+	memcpy(&fiq_pt_regs.pc, reg_base + 0x118, 8);
+#else
+	struct sm_nsec_ctx *nsec_ctx = reg_base;
+
+	fiq_pt_regs.ARM_r0 = nsec_ctx->r0;
+	fiq_pt_regs.ARM_r1 = nsec_ctx->r1;
+	fiq_pt_regs.ARM_r2 = nsec_ctx->r2;
+	fiq_pt_regs.ARM_r3 = nsec_ctx->r3;
+	fiq_pt_regs.ARM_r4 = nsec_ctx->r4;
+	fiq_pt_regs.ARM_r5 = nsec_ctx->r5;
+	fiq_pt_regs.ARM_r6 = nsec_ctx->r6;
+	fiq_pt_regs.ARM_r7 = nsec_ctx->r7;
+	fiq_pt_regs.ARM_r8 = nsec_ctx->r8;
+	fiq_pt_regs.ARM_r9 = nsec_ctx->r9;
+	fiq_pt_regs.ARM_r10 = nsec_ctx->r10;
+	fiq_pt_regs.ARM_fp = nsec_ctx->r11;
+	fiq_pt_regs.ARM_ip = nsec_ctx->r12;
+	fiq_pt_regs.ARM_sp = nsec_ctx->svc_sp;
+	fiq_pt_regs.ARM_lr = nsec_ctx->svc_lr;
+	fiq_pt_regs.ARM_pc = nsec_ctx->mon_lr;
+	fiq_pt_regs.ARM_cpsr = nsec_ctx->mon_spsr;
+#endif
+
+	return fiq_pt_regs;
+}
+
+static void sip_fiq_debugger_uart_irq_tf_cb(unsigned long sp_el1,
+					    unsigned long offset,
+					    unsigned long cpu)
+{
+	struct pt_regs fiq_pt_regs;
+	char *cpu_context;
+
+	/* calling fiq handler */
+	if (ft_fiq_mem_base) {
+		cpu_context = (char *)ft_fiq_mem_base + offset;
+		fiq_pt_regs = sip_fiq_debugger_get_pt_regs(cpu_context, sp_el1);
+		sip_fiq_debugger_uart_irq_tf(fiq_pt_regs, cpu);
+	}
+
+	/* fiq handler done, return to EL3(then EL3 return to EL1 entry) */
+	__invoke_sip_fn_smc(SIP_UARTDBG_FN, 0, 0, UARTDBG_CFG_OSHDL_TO_OS);
+}
+
+int sip_fiq_debugger_uart_irq_tf_init(u32 irq_id, void *callback_fn)
+{
+	struct arm_smccc_res res;
+
+	fiq_target_cpu = 0;
+
+	/* init fiq debugger callback */
+	sip_fiq_debugger_uart_irq_tf = callback_fn;
+	res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, irq_id,
+				  (unsigned long)sip_fiq_debugger_uart_irq_tf_cb,
+				  UARTDBG_CFG_INIT);
+	if (IS_SIP_ERROR(res.a0)) {
+		pr_err("%s error: %d\n", __func__, (int)res.a0);
+		return res.a0;
+	}
+
+	/* share memory ioremap */
+	if (!ft_fiq_mem_base) {
+		ft_fiq_mem_phy = res.a1;
+		ft_fiq_mem_base = ioremap(ft_fiq_mem_phy,
+					  FIQ_UARTDBG_SHARE_MEM_SIZE);
+		if (!ft_fiq_mem_base) {
+			pr_err("%s: share memory ioremap failed\n", __func__);
+			return -ENOMEM;
+		}
+	}
+
+	fiq_sip_enabled = 1;
+
+	return SIP_RET_SUCCESS;
+}
+
+int sip_fiq_debugger_switch_cpu(u32 cpu)
+{
+	struct arm_smccc_res res;
+
+	fiq_target_cpu = cpu;
+	res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, cpu_logical_map(cpu),
+				  0, UARTDBG_CFG_OSHDL_CPUSW);
+	return res.a0;
+}
+
+void sip_fiq_debugger_enable_debug(bool enable)
+{
+	unsigned long val;
+
+	val = enable ? UARTDBG_CFG_OSHDL_DEBUG_ENABLE :
+		       UARTDBG_CFG_OSHDL_DEBUG_DISABLE;
+
+	__invoke_sip_fn_smc(SIP_UARTDBG_FN, 0, 0, val);
+}
+
+int sip_fiq_debugger_set_print_port(u32 port_phyaddr, u32 baudrate)
+{
+	struct arm_smccc_res res;
+
+	res = __invoke_sip_fn_smc(SIP_UARTDBG_FN, port_phyaddr, baudrate,
+				  UARTDBG_CFG_PRINT_PORT);
+	return res.a0;
+}
+
+int sip_fiq_debugger_request_share_memory(void)
+{
+	struct arm_smccc_res res;
+
+	/* request page share memory */
+	res = sip_smc_request_share_mem(FIQ_UARTDBG_PAGE_NUMS,
+					SHARE_PAGE_TYPE_UARTDBG);
+	if (IS_SIP_ERROR(res.a0))
+		return res.a0;
+
+	return SIP_RET_SUCCESS;
+}
+
+int sip_fiq_debugger_get_target_cpu(void)
+{
+	return fiq_target_cpu;
+}
+
+void sip_fiq_debugger_enable_fiq(bool enable, uint32_t tgt_cpu)
+{
+	u32 en;
+
+	fiq_target_cpu = tgt_cpu;
+	en = enable ? UARTDBG_CFG_FIQ_ENABEL : UARTDBG_CFG_FIQ_DISABEL;
+	__invoke_sip_fn_smc(SIP_UARTDBG_FN, tgt_cpu, 0, en);
+}
diff --git a/include/linux/rockchip/rockchip_sip.h b/include/linux/rockchip/rockchip_sip.h
new file mode 100644
index 000000000000..b19f64ede981
--- /dev/null
+++ b/include/linux/rockchip/rockchip_sip.h
@@ -0,0 +1,149 @@
+/* Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef __ROCKCHIP_SIP_H
+#define __ROCKCHIP_SIP_H
+
+#include <linux/arm-smccc.h>
+#include <linux/io.h>
+
+/* SMC function IDs for SiP Service queries, compatible with kernel-3.10 */
+#define SIP_ATF_VERSION			0x82000001
+#define SIP_ACCESS_REG			0x82000002
+#define SIP_SUSPEND_MODE		0x82000003
+#define SIP_PENDING_CPUS		0x82000004
+#define SIP_UARTDBG_CFG			0x82000005
+#define SIP_UARTDBG_CFG64		0xc2000005
+#define SIP_MCU_EL3FIQ_CFG		0x82000006
+#define SIP_ACCESS_CHIP_STATE64		0xc2000006
+#define SIP_SECURE_MEM_CONFIG		0x82000007
+#define SIP_ACCESS_CHIP_EXTRA_STATE64	0xc2000007
+#define SIP_DDR_CFG			0x82000008
+#define SIP_SHARE_MEM			0x82000009
+#define SIP_SIP_VERSION			0x8200000a
+#define SIP_REMOTECTL_CFG		0x8200000b
+
+/* Trust firmware version */
+#define ATF_VER_MAJOR(ver)		(((ver) >> 16) & 0xffff)
+#define ATF_VER_MINOR(ver)		(((ver) >> 0) & 0xffff)
+
+/* SIP_ACCESS_REG: read or write */
+#define SECURE_REG_RD			0x0
+#define SECURE_REG_WR			0x1
+
+/* Fiq debugger share memory: 8KB enough */
+#define FIQ_UARTDBG_PAGE_NUMS		2
+#define FIQ_UARTDBG_SHARE_MEM_SIZE	((FIQ_UARTDBG_PAGE_NUMS) * 4096)
+
+/* Error return code */
+#define IS_SIP_ERROR(x)			(!!(x))
+
+#define SIP_RET_SUCCESS			0
+#define SIP_RET_SMC_UNKNOWN		-1
+#define SIP_RET_NOT_SUPPORTED		-2
+#define SIP_RET_INVALID_PARAMS		-3
+#define SIP_RET_INVALID_ADDRESS		-4
+#define SIP_RET_DENIED			-5
+
+/* SIP_UARTDBG_CFG64 call types */
+#define UARTDBG_CFG_INIT		0xf0
+#define UARTDBG_CFG_OSHDL_TO_OS		0xf1
+#define UARTDBG_CFG_OSHDL_CPUSW		0xf3
+#define UARTDBG_CFG_OSHDL_DEBUG_ENABLE	0xf4
+#define UARTDBG_CFG_OSHDL_DEBUG_DISABLE	0xf5
+#define UARTDBG_CFG_PRINT_PORT		0xf7
+#define UARTDBG_CFG_FIQ_ENABEL		0xf8
+#define UARTDBG_CFG_FIQ_DISABEL		0xf9
+
+/* SIP_SUSPEND_MODE32 call types */
+#define SUSPEND_MODE_CONFIG		0x01
+#define WKUP_SOURCE_CONFIG		0x02
+#define PWM_REGULATOR_CONFIG		0x03
+#define GPIO_POWER_CONFIG		0x04
+#define SUSPEND_DEBUG_ENABLE		0x05
+#define APIOS_SUSPEND_CONFIG		0x06
+#define VIRTUAL_POWEROFF		0x07
+
+/* SIP_REMOTECTL_CFG call types */
+#define	REMOTECTL_SET_IRQ		0xf0
+#define REMOTECTL_SET_PWM_CH		0xf1
+#define REMOTECTL_SET_PWRKEY		0xf2
+#define REMOTECTL_GET_WAKEUP_STATE	0xf3
+#define REMOTECTL_ENABLE		0xf4
+/* wakeup state */
+#define REMOTECTL_PWRKEY_WAKEUP		0xdeadbeaf
+
+/* Share mem page types */
+typedef enum {
+	SHARE_PAGE_TYPE_INVALID = 0,
+	SHARE_PAGE_TYPE_UARTDBG,
+	SHARE_PAGE_TYPE_MAX,
+} share_page_type_t;
+
+/*
+ * Rules: struct arm_smccc_res contains result and data, details:
+ *
+ * a0: error code(0: success, !0: error);
+ * a1~a3: data
+ */
+struct arm_smccc_res sip_smc_get_atf_version(void);
+struct arm_smccc_res sip_smc_get_sip_version(void);
+struct arm_smccc_res sip_smc_ddr_cfg(u32 arg0, u32 arg1, u32 arg2);
+struct arm_smccc_res sip_smc_request_share_mem(u32 page_num,
+					       share_page_type_t page_type);
+struct arm_smccc_res sip_smc_mcu_el3fiq(u32 arg0, u32 arg1, u32 arg2);
+
+int sip_smc_set_suspend_mode(u32 ctrl, u32 config1, u32 config2);
+int sip_smc_virtual_poweroff(void);
+/***************************fiq debugger **************************************/
+void sip_fiq_debugger_enable_fiq(bool enable, uint32_t tgt_cpu);
+void sip_fiq_debugger_enable_debug(bool enable);
+int sip_fiq_debugger_uart_irq_tf_init(u32 irq_id, void *callback_fn);
+int sip_fiq_debugger_set_print_port(u32 port_phyaddr, u32 baudrate);
+int sip_fiq_debugger_request_share_memory(void);
+int sip_fiq_debugger_get_target_cpu(void);
+int sip_fiq_debugger_switch_cpu(u32 cpu);
+int sip_fiq_debugger_is_enabled(void);
+
+/* optee cpu_context */
+struct sm_nsec_ctx {
+	u32 usr_sp;
+	u32 usr_lr;
+	u32 irq_spsr;
+	u32 irq_sp;
+	u32 irq_lr;
+	u32 svc_spsr;
+	u32 svc_sp;
+	u32 svc_lr;
+	u32 abt_spsr;
+	u32 abt_sp;
+	u32 abt_lr;
+	u32 und_spsr;
+	u32 und_sp;
+	u32 und_lr;
+	u32 mon_lr;
+	u32 mon_spsr;
+	u32 r4;
+	u32 r5;
+	u32 r6;
+	u32 r7;
+	u32 r8;
+	u32 r9;
+	u32 r10;
+	u32 r11;
+	u32 r12;
+	u32 r0;
+	u32 r1;
+	u32 r2;
+	u32 r3;
+};
+
+#endif
-- 
2.30.0