From 4bb9b1f44b72b7e8d8f7f758f0b69beec13cccdd Mon Sep 17 00:00:00 2001
From: Yan Wang <yan.wang@amlogic.com>
Date: Wed, 26 Jun 2024 16:23:41 +0800
Subject: [PATCH] MTE: debug: update flag according mte status. [1/1]

PD#SWPL-185970

Problem:
check mte enable/disable,
if mte disable, update flag to notify AO CPU.

Solution:
if mte disable, update flag to notify AO CPU.

Verify:
BL201

Change-Id: I8b15c80da1dcd5fee1d0682bd5e3a3a6fee1e201
Signed-off-by: Yan Wang <yan.wang@amlogic.com>
---
 drivers/debug/Makefile    |  1 +
 drivers/debug/debug_mte.c | 60 +++++++++++++++++++++++++++++++++++++++
 drivers/debug/main.c      |  6 ++++
 drivers/debug/main.h      |  5 ++++
 4 files changed, 72 insertions(+)
 create mode 100644 drivers/debug/debug_mte.c

diff --git a/drivers/debug/Makefile b/drivers/debug/Makefile
index 8884f29ab..62f44abbd 100644
--- a/drivers/debug/Makefile
+++ b/drivers/debug/Makefile
@@ -18,6 +18,7 @@ $(MODULE_NAME)-$(CONFIG_AMLOGIC_DEBUG_ATRACE) += atrace.o
 $(MODULE_NAME)-$(CONFIG_AMLOGIC_DEBUG_FILE) += debug_file.o
 $(MODULE_NAME)-$(CONFIG_AMLOGIC_GKI_CONFIG) += gki_config.o
 $(MODULE_NAME)-$(CONFIG_AMLOGIC_DEBUG_HLD) += watchdog_hld.o
+$(MODULE_NAME)-$(CONFIG_ARM64_MTE) += debug_mte.o
 ifeq ($(CONFIG_AMLOGIC_DEBUG_PRINTK),m)
 	$(MODULE_NAME)-$(CONFIG_AMLOGIC_DEBUG_PRINTK) += printk.o
 else
diff --git a/drivers/debug/debug_mte.c b/drivers/debug/debug_mte.c
new file mode 100644
index 000000000..7eace3dcc
--- /dev/null
+++ b/drivers/debug/debug_mte.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
+/*
+ * Copyright (c) 2019 Amlogic, Inc. All rights reserved.
+ */
+
+#if IS_ENABLED(CONFIG_ARM64_MTE)
+#include <linux/irqflags.h>
+#include <linux/moduleparam.h>
+#include <linux/module.h>
+#include <linux/cpuset.h>
+#include <linux/sched/isolation.h>
+#include <linux/amlogic/gki_module.h>
+#include <linux/sched.h>
+#include <asm/cpufeature.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/errno.h>
+#include <linux/of_address.h>
+
+static void __iomem *debug_mte_reg;
+
+/* SYSCTRL_DEBUG_REG4[0]=0b1: stop writing MTE clock */
+#define MTE_CLK_STOP BIT(0)
+
+int aml_debug_mte_init(void)
+{
+	struct device_node *node;
+	unsigned int val;
+
+	if (!system_supports_mte()) {
+		node = of_find_node_by_path("/mte_debug");
+
+		if (!node) {
+			pr_info("no mte_debug node\n");
+			return -EINVAL;
+		}
+
+		debug_mte_reg = of_iomap(node, 0);
+		if (!debug_mte_reg) {
+			pr_info("mte_debug map fail\n");
+			return -ENOMEM;
+		}
+
+		val = readl_relaxed(debug_mte_reg);
+		val |= MTE_CLK_STOP;
+		writel_relaxed(val, debug_mte_reg);
+		pr_info("mte disabled\n");
+	} else {
+		pr_info("mte enabled\n");
+	}
+	return 0;
+}
+
+void aml_debug_mte_exit(void)
+{
+	if (debug_mte_reg)
+		iounmap(debug_mte_reg);
+}
+
+#endif
diff --git a/drivers/debug/main.c b/drivers/debug/main.c
index 39efb25e9..36e3e102c 100644
--- a/drivers/debug/main.c
+++ b/drivers/debug/main.c
@@ -24,6 +24,9 @@ static int __init debug_main_init(void)
 	call_sub_init(aml_sched_init);
 	call_sub_init(aml_kprobes_init);
 	call_sub_init(aml_isolcpus_init);
+#if IS_ENABLED(CONFIG_ARM64_MTE)
+	call_sub_init(aml_debug_mte_init);
+#endif
 	pr_debug("### %s() end\n", __func__);
 	return 0;
 }
@@ -34,6 +37,9 @@ static void __exit debug_main_exit(void)
 #if IS_MODULE(CONFIG_AMLOGIC_DEBUG_PRINTK)
 	printk_vendor_hook_exit();
 #endif
+#if IS_ENABLED(CONFIG_ARM64_MTE)
+	aml_debug_mte_exit();
+#endif
 }
 
 module_init(debug_main_init);
diff --git a/drivers/debug/main.h b/drivers/debug/main.h
index a2c0f8a52..4ef354f0e 100644
--- a/drivers/debug/main.h
+++ b/drivers/debug/main.h
@@ -71,4 +71,9 @@ int printk_vendor_hook_init(void);
 void printk_vendor_hook_exit(void);
 #endif
 
+#if IS_ENABLED(CONFIG_ARM64_MTE)
+int aml_debug_mte_init(void);
+void aml_debug_mte_exit(void);
+#endif
+
 #endif /*_DEBUG_MAIN_H__*/