5 files changed, 206 insertions, 0 deletions
diff --git a/lib/utils/reset/fdt_reset.c b/lib/utils/reset/fdt_reset.c
index dead8a3..82532c2 100644
--- a/lib/utils/reset/fdt_reset.c
+++ b/lib/utils/reset/fdt_reset.c
@@ -13,10 +13,12 @@
 
 extern struct fdt_reset fdt_reset_sifive;
 extern struct fdt_reset fdt_reset_htif;
+extern struct fdt_reset fdt_reset_thead;
 
 static struct fdt_reset *reset_drivers[] = {
 	&fdt_reset_sifive,
 	&fdt_reset_htif,
+	&fdt_reset_thead,
 };
 
 static struct fdt_reset *current_driver = NULL;
diff --git a/lib/utils/reset/fdt_reset_thead.c b/lib/utils/reset/fdt_reset_thead.c
new file mode 100644
index 0000000..ea81fc9
--- /dev/null
+++ b/lib/utils/reset/fdt_reset_thead.c
@@ -0,0 +1,132 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#include <libfdt.h>
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/reset/fdt_reset.h>
+#include "fdt_reset_thead.h"
+
+struct custom_csr custom_csr[MAX_CUSTOM_CSR];
+
+#define CSR_OPCODE 0x39073
+static void clone_csrs(int cnt)
+{
+	unsigned long i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Write csr BIT[31 - 20] to stub */
+		__reset_thead_csr_stub[3*i + 1] =
+				CSR_OPCODE | (custom_csr[i].index << 20);
+
+		/* Mask csr BIT[31 - 20] */
+		*(u32 *)&__fdt_reset_thead_csrr &= BIT(20) - 1;
+		smp_mb();
+
+		/* Write csr BIT[31 - 20] to __fdt_reset_thead_csrr */
+		*(u32 *)&__fdt_reset_thead_csrr |= custom_csr[i].index << 20;
+		smp_mb();
+
+		RISCV_FENCE_I;
+
+		custom_csr[i].value = __fdt_reset_thead_csrr();
+	}
+}
+
+extern void __thead_pre_start_warm(void);
+static int thead_reset_init(void *fdt, int nodeoff,
+				 const struct fdt_match *match)
+{
+	void *p;
+	const fdt64_t *val;
+	const fdt32_t *val_w;
+	int len, i, cnt;
+	u32 t, tmp = 0;
+
+	/* Prepare clone csrs */
+	val_w = fdt_getprop(fdt, nodeoff, "csr-copy", &len);
+	if (len > 0 && val_w) {
+		cnt = len / sizeof(fdt32_t);
+
+		if (cnt > MAX_CUSTOM_CSR)
+			sbi_hart_hang();
+
+		for (i = 0; i < cnt; i++) {
+			custom_csr[i].index = fdt32_to_cpu(val_w[i]);
+		}
+	}
+
+	if (cnt)
+		clone_csrs(cnt);
+
+	/* Delegate plic enable regs for S-mode */
+	val = fdt_getprop(fdt, nodeoff, "plic-delegate", &len);
+	if (len > 0 && val) {
+		p = (void *)(ulong)fdt64_to_cpu(*val);
+		writel(BIT(0), p);
+	}
+
+	/* Old reset method for secondary harts */
+	if (fdt_getprop(fdt, nodeoff, "using-csr-reset", &len)) {
+		csr_write(0x7c7, (ulong)&__thead_pre_start_warm);
+		csr_write(0x7c6, -1);
+	}
+
+	/* Custom reset method for secondary harts */
+	val = fdt_getprop(fdt, nodeoff, "entry-reg", &len);
+	if (len > 0 && val) {
+		p = (void *)(ulong)fdt64_to_cpu(*val);
+
+		val_w = fdt_getprop(fdt, nodeoff, "entry-cnt", &len);
+		if (len > 0 && val_w) {
+			tmp = fdt32_to_cpu(*val_w);
+
+			for (i = 0; i < tmp; i++) {
+				t = (ulong)&__thead_pre_start_warm;
+				writel(t, p + (8 * i));
+				t = (u64)(ulong)&__thead_pre_start_warm >> 32;
+				writel(t, p + (8 * i) + 4);
+			}
+		}
+
+		val = fdt_getprop(fdt, nodeoff, "control-reg", &len);
+		if (len > 0 && val) {
+			p = (void *)(ulong)fdt64_to_cpu(*val);
+
+			val_w = fdt_getprop(fdt, nodeoff, "control-val", &len);
+			if (len > 0 && val_w) {
+				tmp = fdt32_to_cpu(*val_w);
+				tmp |= readl(p);
+				writel(tmp, p);
+			}
+		}
+	}
+
+	return 0;
+}
+
+int thead_system_reset_check(u32 type, u32 reason)
+{
+	return 1;
+}
+
+void thead_system_reset(u32 type, u32 reason)
+{
+	ebreak();
+}
+
+static const struct fdt_match thead_reset_match[] = {
+	{ .compatible = "thead,reset-sample" },
+	{ },
+};
+
+struct fdt_reset fdt_reset_thead = {
+	.match_table = thead_reset_match,
+	.init = thead_reset_init,
+	.system_reset_check = thead_system_reset_check,
+	.system_reset = thead_system_reset
+};
diff --git a/lib/utils/reset/fdt_reset_thead.h b/lib/utils/reset/fdt_reset_thead.h
new file mode 100644
index 0000000..d7a686e
--- /dev/null
+++ b/lib/utils/reset/fdt_reset_thead.h
@@ -0,0 +1,23 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#ifndef __FDT_RESET_THEAD_H__
+#define __FDT_RESET_THEAD_H__
+
+#define MAX_CUSTOM_CSR	32
+
+#ifndef __ASSEMBLER__
+struct custom_csr {
+	unsigned long index;
+	unsigned long value;
+};
+
+u64  __fdt_reset_thead_csrr(void);
+
+extern struct custom_csr custom_csr[MAX_CUSTOM_CSR];
+extern u32 __reset_thead_csr_stub[];
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* __FDT_RESET_THEAD_H__ */
diff --git a/lib/utils/reset/fdt_reset_thead_asm.S b/lib/utils/reset/fdt_reset_thead_asm.S
new file mode 100644
index 0000000..8237951
--- /dev/null
+++ b/lib/utils/reset/fdt_reset_thead_asm.S
@@ -0,0 +1,47 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#include <sbi/riscv_asm.h>
+#include "fdt_reset_thead.h"
+
+/*
+ * csrrs rd, csr, rs1
+ * |31   20|19   15|14   12|11  7|6       0|
+ *    csr     rs1     010     rd   1110011
+ */
+#define CSR_STUB	addi    x0, x0, 0
+
+	.option norvc
+	.align 3
+	.global __fdt_reset_thead_csrr
+__fdt_reset_thead_csrr:
+	csrrs a0, 0, x0
+	ret
+
+	.align 3
+	.global __thead_pre_start_warm
+__thead_pre_start_warm:
+	/*
+	 * Clear L1 cache & BTB & BHT ...
+	 */
+	li	t1, 0x70013
+	csrw	0x7c2, t1
+	fence rw,rw
+
+	lla	t1, custom_csr
+
+	.global __reset_thead_csr_stub
+__reset_thead_csr_stub:
+.rept	MAX_CUSTOM_CSR
+	REG_L	t2, 8(t1)
+	CSR_STUB
+	addi	t1, t1, 16
+.endr
+	/*
+	 * Clear L1 cache & BTB & BHT ...
+	 */
+	li	t1, 0x70013
+	csrw	0x7c2, t1
+	fence rw,rw
+	j _start_warm
diff --git a/lib/utils/reset/objects.mk b/lib/utils/reset/objects.mk
index b447261..b6619f4 100644
--- a/lib/utils/reset/objects.mk
+++ b/lib/utils/reset/objects.mk
@@ -10,3 +10,5 @@
 libsbiutils-objs-y += reset/fdt_reset.o
 libsbiutils-objs-y += reset/fdt_reset_htif.o
 libsbiutils-objs-y += reset/fdt_reset_sifive.o
+libsbiutils-objs-y += reset/fdt_reset_thead.o
+libsbiutils-objs-y += reset/fdt_reset_thead_asm.o