[PATCH 2/2] platform: thead: restore platform CSR when suspend

[Xiang W]

Some CSRs in Thead need to be backed up and restored during HART
suspend.

Signed-off-by: David Li <davidli.li at linux.alibaba.com>
Signed-off-by: Xiang W <wangxiang at iscas.ac.cn>
---
 .../generic/include/thead/c9xx_encoding.h     |  1 +
 platform/generic/thead/thead-generic.c        | 53 +++++++++++++++++++
 2 files changed, 54 insertions(+)

diff --git a/platform/generic/include/thead/c9xx_encoding.h b/platform/generic/include/thead/c9xx_encoding.h
index a45b171e..bcbd9e24 100644
--- a/platform/generic/include/thead/c9xx_encoding.h
+++ b/platform/generic/include/thead/c9xx_encoding.h
@@ -16,6 +16,7 @@
 #define THEAD_C9XX_CSR_MCOUNTEROF	0x7cb
 #define THEAD_C9XX_CSR_MHINT2		0x7cc
 #define THEAD_C9XX_CSR_MHINT3		0x7cd
+#define THEAD_C9XX_CSR_MHINT4		0x7ce
 #define THEAD_C9XX_CSR_MRADDR		0x7e0
 #define THEAD_C9XX_CSR_MEXSTATUS	0x7e1
 #define THEAD_C9XX_CSR_MNMICAUSE	0x7e2
diff --git a/platform/generic/thead/thead-generic.c b/platform/generic/thead/thead-generic.c
index ddb4f0bf..63e33849 100644
--- a/platform/generic/thead/thead-generic.c
+++ b/platform/generic/thead/thead-generic.c
@@ -7,6 +7,7 @@
  */
 
 #include <platform_override.h>
+#include <thead/c9xx_encoding.h>
 #include <thead/c9xx_errata.h>
 #include <thead/c9xx_pmu.h>
 #include <sbi/sbi_const.h>
@@ -15,10 +16,61 @@
 #include <sbi/sbi_string.h>
 #include <sbi_utils/fdt/fdt_helper.h>
 
+/* redefine CSR register */
+#define CSR_MXSTATUS	THEAD_C9XX_CSR_MXSTATUS
+#define CSR_MHCR	THEAD_C9XX_CSR_MHCR
+#define CSR_MCCR2	THEAD_C9XX_CSR_MCCR2
+#define CSR_MHINT	THEAD_C9XX_CSR_MHINT
+#define CSR_MHINT2_E	THEAD_C9XX_CSR_MHINT2
+#define CSR_MHINT4	THEAD_C9XX_CSR_MHINT4
+#define CSR_MSMPR	THEAD_C9XX_CSR_MSMPR
+#define CSR_SMPEN	CSR_MSMPR
+
 struct thead_generic_quirks {
 	u64	errata;
 };
 
+extern struct sbi_platform platform;
+
+static inline bool thead_has_hotplug(void)
+{
+	return platform.hart_count > 1;
+}
+
+static void thead_suspend_non_ret_save(unsigned long *data)
+{
+	data[0] = csr_read(CSR_SMPEN);
+	data[1] = csr_read(CSR_MCCR2);
+	data[2] = csr_read(CSR_MXSTATUS);
+	data[3] = csr_read(CSR_MHINT);
+	data[4] = csr_read(CSR_MHCR);
+	data[5] = csr_read(CSR_MHINT2_E);
+	data[6] = csr_read(CSR_MHINT4);
+}
+
+static void thead_suspend_non_ret_restore(unsigned long *data)
+{
+	csr_write(CSR_SMPEN, data[0]);
+	csr_write(CSR_MCCR2, data[1]);
+	csr_write(CSR_MXSTATUS, data[2]);
+	csr_write(CSR_MHINT, data[3]);
+	csr_write(CSR_MHCR, data[4]);
+	csr_write(CSR_MHINT2_E, data[5]);
+	csr_write(CSR_MHINT4, data[6]);
+}
+
+static int thead_nascent_init(void)
+{
+
+	if (thead_has_hotplug()) {
+		platform.suspend_backup_csr_count = 7;
+		generic_platform_ops.suspend_non_ret_save = thead_suspend_non_ret_save;
+		generic_platform_ops.suspend_non_ret_restore = thead_suspend_non_ret_restore;
+	}
+
+	return generic_nascent_init();
+}
+
 static int thead_tlb_flush_early_init(bool cold_boot)
 {
 	thead_register_tlb_flush_trap_handler();
@@ -44,6 +96,7 @@ static int thead_generic_platform_init(const void *fdt, int nodeoff,
 {
 	const struct thead_generic_quirks *quirks = match->data;
 
+	generic_platform_ops.nascent_init = thead_nascent_init;
 	if (quirks->errata & THEAD_QUIRK_ERRATA_TLB_FLUSH)
 		generic_platform_ops.early_init = thead_tlb_flush_early_init;
 	if (quirks->errata & THEAD_QUIRK_ERRATA_THEAD_PMU)
-- 
2.47.3