soc: rockchip: rk_fiq_debugger: use the sdei for fiq debugger.

Signed-off-by: Tony Xie <tony.xie@rock-chips.com>
Change-Id: Ie97e37cee6bf94df99dbdf50dc3ca8808c73bc01

drivers/soc/rockchip/rk_fiq_debugger.c

@@ -350,6 +350,249 @@ static void fiq_enable(struct platform_device *pdev, unsigned int irq, bool on)
 }
 
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
+#ifdef CONFIG_ARM_SDE_INTERFACE
+#include <linux/arm_sdei.h>
+#include <asm/smp_plat.h>
+#include <linux/suspend.h>
+void fiq_debugger_fiq_get_(const char *fmt, ...);
+
+static struct rk_fiq_sdei_st {
+	u32 cur_cpu;
+	u32 sw_cpu;
+	u32 cpu_can_sw;
+	int fiq_en;
+	u32 event_id;
+	u32 cpu_off_sw;
+	u32 cpu_sw_event_id;
+} rk_fiq_sdei;
+
+int sdei_fiq_debugger_is_enabled(void)
+{
+	return rk_fiq_sdei.fiq_en;
+}
+
+int fiq_sdei_event_callback(u32 event, struct pt_regs *regs, void *arg)
+{
+	int cpu_id = get_logical_index(read_cpuid_mpidr() &
+				       MPIDR_HWID_BITMASK);
+	fiq_debugger_fiq(regs, cpu_id);
+
+	return 0;
+}
+
+void rk_fiq_sdei_event_sw_cpu(int wait_disable)
+{
+	unsigned long affinity;
+	int cnt = 100000;
+	int ret = 0;
+
+	do {
+		ret = sdei_event_disable_nolock(rk_fiq_sdei.event_id);
+		if (!ret)
+			break;
+		cnt--;
+		udelay(20);
+	} while (wait_disable && cnt);
+
+	affinity = cpu_logical_map(rk_fiq_sdei.sw_cpu) & MPIDR_HWID_BITMASK;
+	ret = sdei_event_routing_set_nolock(rk_fiq_sdei.event_id,
+					    SDEI_EVENT_REGISTER_RM_PE,
+					    affinity);
+	ret = sdei_event_enable_nolock(rk_fiq_sdei.event_id);
+	rk_fiq_sdei.cur_cpu = rk_fiq_sdei.sw_cpu;
+}
+
+int fiq_sdei_sw_cpu_event_callback(u32 event, struct pt_regs *regs, void *arg)
+{
+	int cnt = 10000;
+	int ret = 0;
+	int cpu_id = event - rk_fiq_sdei.cpu_sw_event_id;
+
+	WARN_ON(cpu_id !=
+		get_logical_index(read_cpuid_mpidr() & MPIDR_HWID_BITMASK));
+
+	if (cpu_id == rk_fiq_sdei.sw_cpu) {
+		if (!rk_fiq_sdei.cpu_off_sw) {
+			rk_fiq_sdei.cpu_can_sw = 1;
+		} else {
+			rk_fiq_sdei_event_sw_cpu(1);
+			rk_fiq_sdei.cpu_off_sw = 0;
+		}
+	} else if (cpu_id == rk_fiq_sdei.cur_cpu && !rk_fiq_sdei.cpu_off_sw) {
+		while (!rk_fiq_sdei.cpu_can_sw && cnt) {
+			udelay(10);
+			cnt--;
+		};
+
+		if (rk_fiq_sdei.cpu_can_sw) {
+			rk_fiq_sdei_event_sw_cpu(0);
+			rk_fiq_sdei.cpu_can_sw = 0;
+		}
+	}
+	return ret;
+}
+
+static void _rk_fiq_dbg_sdei_switch_cpu(unsigned int cpu, int cpu_off)
+{
+	if (cpu == rk_fiq_sdei.cur_cpu)
+		return;
+	rk_fiq_sdei.sw_cpu = cpu;
+	rk_fiq_sdei.cpu_can_sw = 0;
+	rk_fiq_sdei.cpu_off_sw = cpu_off;
+	sip_fiq_debugger_sdei_switch_cpu(rk_fiq_sdei.cur_cpu, cpu, cpu_off);
+}
+
+static void rk_fiq_dbg_sdei_switch_cpu(struct platform_device *pdev,
+				       unsigned int cpu)
+{
+	_rk_fiq_dbg_sdei_switch_cpu(cpu, 0);
+}
+
+static int fiq_dbg_sdei_cpu_off_migrate_fiq(unsigned int cpu)
+{
+	unsigned int target_cpu;
+	int cnt = 10000;
+
+	if (rk_fiq_sdei.cur_cpu == cpu) {
+		target_cpu = cpumask_first(cpu_online_mask);
+		_rk_fiq_dbg_sdei_switch_cpu(target_cpu, 1);
+
+		while (rk_fiq_sdei.cur_cpu == cpu && cnt) {
+			udelay(10);
+			cnt--;
+		};
+		if (!cnt)
+			pr_err("%s: from %d to %d err!\n",
+			       __func__, cpu, target_cpu);
+	}
+
+	return 0;
+}
+
+static int fiq_dbg_sdei_pm_callback(struct notifier_block *nb,
+				    unsigned long mode, void *_unused)
+{
+	unsigned int target_cpu;
+
+	switch (mode) {
+	case PM_SUSPEND_PREPARE:
+		target_cpu = cpumask_first(cpu_online_mask);
+		if (target_cpu != 0)
+			pr_err("%s: fiq for core !\n", __func__);
+		else
+			_rk_fiq_dbg_sdei_switch_cpu(target_cpu, 1);
+		break;
+	default:
+	break;
+	}
+	return 0;
+}
+
+static struct notifier_block fiq_dbg_sdei_pm_nb = {
+	.notifier_call = fiq_dbg_sdei_pm_callback,
+};
+
+static int fiq_debugger_sdei_enable(struct rk_fiq_debugger *t)
+{
+	int ret, cpu, i;
+
+	ret = sip_fiq_debugger_sdei_get_event_id(&rk_fiq_sdei.event_id,
+						 &rk_fiq_sdei.cpu_sw_event_id,
+						 NULL);
+
+	if (ret) {
+		pr_err("%s: get event id error!\n", __func__);
+		return ret;
+	}
+
+	ret = cpuhp_setup_state_nocalls(CPUHP_AP_ONLINE_DYN,
+					"soc/rk_sdei_fiq_debugger",
+					NULL,
+					fiq_dbg_sdei_cpu_off_migrate_fiq);
+	if (ret < 0) {
+		pr_err("%s: cpuhp_setup_state_nocalls error! %d\n",
+		       __func__, ret);
+		return ret;
+	}
+
+	if (register_pm_notifier(&fiq_dbg_sdei_pm_nb)) {
+		pr_err("%s: register pm notify error: %d!\n", __func__, ret);
+		return ret;
+	}
+
+	ret = sdei_event_register(rk_fiq_sdei.event_id,
+				  fiq_sdei_event_callback, NULL);
+
+	if (ret) {
+		pr_err("%s: sdei_event_register error!\n", __func__);
+		unregister_pm_notifier(&fiq_dbg_sdei_pm_nb);
+		return ret;
+	}
+
+	rk_fiq_sdei.cur_cpu = 0;
+
+	ret = sdei_event_routing_set(rk_fiq_sdei.event_id,
+				     SDEI_EVENT_REGISTER_RM_PE,
+				     cpu_logical_map(rk_fiq_sdei.cur_cpu));
+
+	if (ret) {
+		pr_err("%s: sdei_event_routing_set error!\n", __func__);
+		goto err;
+	}
+
+	ret = sdei_event_enable(rk_fiq_sdei.event_id);
+	if (ret) {
+		pr_err("%s: sdei_event_enable error!\n", __func__);
+		goto err;
+	}
+
+	for (cpu = 0; cpu < num_possible_cpus(); cpu++) {
+		ret = sdei_event_register(rk_fiq_sdei.cpu_sw_event_id + cpu,
+					  fiq_sdei_sw_cpu_event_callback,
+					  NULL);
+		if (ret) {
+			pr_err("%s: cpu %d sdei_event_register error!\n",
+			       __func__, cpu);
+			goto cpu_sw_err;
+		}
+		ret = sdei_event_routing_set(rk_fiq_sdei.cpu_sw_event_id + cpu,
+					     SDEI_EVENT_REGISTER_RM_PE,
+					     cpu_logical_map(cpu));
+
+		if (ret) {
+			pr_err("%s:cpu %d fiq_sdei_event_routing_set error!\n",
+			       __func__, cpu);
+			goto cpu_sw_err;
+		}
+
+		ret = sdei_event_enable(rk_fiq_sdei.cpu_sw_event_id + cpu);
+		if (ret) {
+			pr_err("%s: cpu %d sdei_event_enable error!\n",
+			       __func__, cpu);
+			goto cpu_sw_err;
+		}
+	}
+
+	t->pdata.switch_cpu = rk_fiq_dbg_sdei_switch_cpu;
+	rk_fiq_sdei.fiq_en = 1;
+	return 0;
+cpu_sw_err:
+	for (i = 0; i < cpu; i++)
+		sdei_event_unregister(rk_fiq_sdei.cpu_sw_event_id + i);
+err:
+	unregister_pm_notifier(&fiq_dbg_sdei_pm_nb);
+	sdei_event_unregister(rk_fiq_sdei.event_id);
+
+	return ret;
+}
+
+#else
+static inline int fiq_debugger_sdei_enable(struct rk_fiq_debugger *t)
+{
+	return -EINVAL;
+}
+#endif
+
 static struct pt_regs fiq_pt_regs;
 
 static void rk_fiq_debugger_switch_cpu(struct platform_device *pdev,
@@ -542,8 +785,11 @@ void rk_serial_debug_init(void __iomem *base, phys_addr_t phy_base,
 	rk_fiq_read(t, UART_USR);
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
 	if ((signal_irq > 0) && (serial_hwirq > 0)) {
-		ret = fiq_debugger_bind_sip_smc(t, phy_base, serial_hwirq,
-						signal_irq, baudrate);
+		ret = fiq_debugger_sdei_enable(t);
+		if (ret)
+			ret = fiq_debugger_bind_sip_smc(t, phy_base,
+							serial_hwirq,
+							signal_irq, baudrate);
 		if (ret)
 			tf_fiq_sup = false;
 		else
@@ -761,7 +1007,12 @@ static int __init rk_fiqdbg_init(void)
 	return platform_driver_probe(&rk_fiqdbg_driver,
 				     rk_fiqdbg_probe);
 }
+
+#if defined(CONFIG_FIQ_DEBUGGER_TRUST_ZONE) && defined(CONFIG_ARM_SDE_INTERFACE)
+fs_initcall(rk_fiqdbg_init);
+#else
 subsys_initcall(rk_fiqdbg_init); /* after of_platform_default_populate_init */
+#endif
 
 static void __exit rk_fiqdbg_exit(void)
 {

drivers/staging/android/fiq_debugger/fiq_debugger.c

@@ -38,6 +38,7 @@
 
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
 #include <linux/rockchip/rockchip_sip.h>
+#include <linux/soc/rockchip/rk_fiq_debugger.h>
 #endif
 
 #ifdef CONFIG_FIQ_GLUE
@@ -568,7 +569,8 @@ static void fiq_debugger_switch_cpu(struct fiq_debugger_state *state, int cpu)
 #ifdef CONFIG_ARCH_ROCKCHIP
 	else {
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
-		if (sip_fiq_debugger_is_enabled()) {
+		if (sip_fiq_debugger_is_enabled() ||
+		    sdei_fiq_debugger_is_enabled()) {
 			if (state->pdata->switch_cpu) {
 				state->pdata->switch_cpu(state->pdev, cpu);
 				state->current_cpu = cpu;
@@ -1452,7 +1454,8 @@ static int fiq_debugger_probe(struct platform_device *pdev)
 	if (fiq_debugger_have_fiq(state)) {
 #ifdef CONFIG_FIQ_GLUE
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
-		if (sip_fiq_debugger_is_enabled()) {
+		if (sip_fiq_debugger_is_enabled() ||
+		    sdei_fiq_debugger_is_enabled()) {
 		} else
 #endif
 		{

include/linux/soc/rockchip/rk_fiq_debugger.h

@@ -4,6 +4,19 @@
 
 #ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
 void fiq_debugger_fiq(void *regs, u32 cpu);
+
+#ifdef CONFIG_ARM_SDE_INTERFACE
+int sdei_fiq_debugger_is_enabled(void);
+int fiq_sdei_event_enable(u32 event_num);
+int fiq_sdei_event_routing_set(u32 event_num, unsigned long flags,
+			       unsigned long affinity);
+int fiq_sdei_event_disable(u32 event_num);
+#else
+static inline int sdei_fiq_debugger_is_enabled(void)
+{
+	return 0;
+}
+#endif
 #endif
 
 #endif