From c31b97d8ed79b33ca93f011d09722817d09d7d77 Mon Sep 17 00:00:00 2001
From: Tony Xie <tony.xie@rock-chips.com>
Date: Sun, 8 Nov 2020 20:16:30 +0800
Subject: [PATCH] firmware: arm_sdei: add some functions for fiq debugger.

1. add sdei_event_routing_set()
2. add sdei_event_routing_set_nolock()
3. add sdei_event_enable_nolock() and sdei_event_disable_nolock()

Signed-off-by: Tony Xie <tony.xie@rock-chips.com>
Change-Id: Ia2471a3ac6a9189888a465a529875957ab9661b6
---
 drivers/firmware/arm_sdei.c | 39 +++++++++++++++++++++++++++++++++++++
 include/linux/arm_sdei.h    |  9 +++++++++
 2 files changed, 48 insertions(+)

diff --git a/drivers/firmware/arm_sdei.c b/drivers/firmware/arm_sdei.c
index e809f4d9a9e9..08e828660e81 100644
--- a/drivers/firmware/arm_sdei.c
+++ b/drivers/firmware/arm_sdei.c
@@ -887,6 +887,45 @@ static void sdei_smccc_hvc(unsigned long function_id,
 	arm_smccc_hvc(function_id, arg0, arg1, arg2, arg3, arg4, 0, 0, res);
 }
 
+#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
+int sdei_event_enable_nolock(u32 event_num)
+{
+	return sdei_api_event_enable(event_num);
+}
+
+int sdei_event_disable_nolock(u32 event_num)
+{
+	return sdei_api_event_disable(event_num);
+}
+
+int sdei_event_routing_set_nolock(u32 event_num, unsigned long flags,
+				  unsigned long affinity)
+{
+	return invoke_sdei_fn(SDEI_1_0_FN_SDEI_EVENT_ROUTING_SET, event_num,
+			      (unsigned long)flags, (unsigned long)affinity,
+			      0, 0, 0);
+}
+
+int sdei_event_routing_set(u32 event_num, unsigned long flags,
+			   unsigned long affinity)
+{
+	int err = -EINVAL;
+	struct sdei_event *event;
+
+	mutex_lock(&sdei_events_lock);
+	event = sdei_event_find(event_num);
+	if (!event) {
+		mutex_unlock(&sdei_events_lock);
+		return -ENOENT;
+	}
+
+	err = sdei_event_routing_set_nolock(event_num, flags, affinity);
+	mutex_unlock(&sdei_events_lock);
+
+	return err;
+}
+#endif
+
 static int sdei_get_conduit(struct platform_device *pdev)
 {
 	const char *method;
diff --git a/include/linux/arm_sdei.h b/include/linux/arm_sdei.h
index 942afbd544b7..d3f1d27aba64 100644
--- a/include/linux/arm_sdei.h
+++ b/include/linux/arm_sdei.h
@@ -39,6 +39,15 @@ int sdei_event_unregister(u32 event_num);
 int sdei_event_enable(u32 event_num);
 int sdei_event_disable(u32 event_num);
 
+#ifdef CONFIG_FIQ_DEBUGGER_TRUST_ZONE
+int sdei_event_enable_nolock(u32 event_num);
+int sdei_event_disable_nolock(u32 event_num);
+int sdei_event_routing_set_nolock(u32 event_num, unsigned long flags,
+				  unsigned long affinity);
+int sdei_event_routing_set(u32 event_num, unsigned long flags,
+			   unsigned long affinity);
+#endif
+
 #ifdef CONFIG_ARM_SDE_INTERFACE
 /* For use by arch code when CPU hotplug notifiers are not appropriate. */
 int sdei_mask_local_cpu(void);