[RFC PATCH 2/2] lib: sbi: Preserve supervisor CTR records across domain switches

[whensun]

From: Theodorus Wensan Februanto <theodoruswensanfebruanto025 at gmail.com>

Save supervisor CTR control, depth, status, and logical record
entries when Ssctr and Sscsrind are available. This experimental
implementation freezes recording before saving and restores
status before logical records.

IMSIC and other indirect CSR state remain outside this change.

Link: https://github.com/riscv-software-src/opensbi/issues/433
Signed-off-by: Theodorus Wensan Februanto <theodoruswensanfebruanto025 at gmail.com>
---
 lib/sbi/sbi_domain_context.c | 95 ++++++++++++++++++++++++++++++++++++
 1 file changed, 95 insertions(+)

diff --git a/lib/sbi/sbi_domain_context.c b/lib/sbi/sbi_domain_context.c
index 14927ff5..af180409 100644
--- a/lib/sbi/sbi_domain_context.c
+++ b/lib/sbi/sbi_domain_context.c
@@ -21,6 +21,24 @@
 #include <sbi/sbi_vector.h>
 #include <sbi/sbi_fp.h>
 
+#define HART_CTR_MAX_ENTRIES 256
+#define HART_CTR_FIRST_SELECTOR 0x200
+#define HART_CTR_FROZEN (1UL << 31)
+
+struct hart_ctr_entry {
+	unsigned long source;
+	unsigned long target;
+	unsigned long data;
+};
+
+struct hart_ctr_context {
+	unsigned long ctl;
+	unsigned long depth;
+	unsigned long status;
+	unsigned long vsctl;
+	struct hart_ctr_entry entries[HART_CTR_MAX_ENTRIES];
+};
+
 /** Context representation for a hart within a domain */
 struct hart_context {
 	/** Trap-related states such as GPRs, mepc, and mstatus */
@@ -77,6 +95,8 @@ struct hart_context {
 	unsigned long siselect;
 	/** AIA supervisor interrupt priority register bank */
 	unsigned long siprio[16];
+	/** Supervisor control transfer records */
+	struct hart_ctr_context *ctr_ctx;
 
 	/** Float context state */
 	struct sbi_fp_context fp_ctx;
@@ -142,6 +162,59 @@ static void hart_aia_iprio_restore(const struct hart_context *ctx)
 	}
 }
 
+#if __riscv_xlen == 64
+static unsigned int hart_ctr_entry_count(unsigned long depth)
+{
+	unsigned int encoding = depth & 7;
+
+	if (encoding > 4)
+		return 0;
+	return 16U << encoding;
+}
+
+static void hart_ctr_save(struct hart_ctr_context *ctx)
+{
+	unsigned int i, count;
+
+	ctx->status = csr_read_set(CSR_SCTRSTATUS, HART_CTR_FROZEN);
+	ctx->ctl = csr_read(CSR_SCTRCTL);
+	ctx->depth = csr_read(CSR_SCTRDEPTH);
+	if (misa_extension('H'))
+		ctx->vsctl = csr_read(CSR_VSCTRCTL);
+
+	count = hart_ctr_entry_count(ctx->depth);
+	for (i = 0; i < count; i++) {
+		csr_write(CSR_SISELECT, HART_CTR_FIRST_SELECTOR + i);
+		ctx->entries[i].source = csr_read(CSR_SIREG);
+		ctx->entries[i].target = csr_read(CSR_SIREG2);
+		ctx->entries[i].data = csr_read(CSR_SIREG3);
+	}
+}
+
+static void hart_ctr_restore(const struct hart_ctr_context *ctx)
+{
+	unsigned int i, count;
+
+	csr_write(CSR_SCTRCTL, 0);
+	csr_write(CSR_SCTRDEPTH, ctx->depth);
+	__asm__ __volatile__(".4byte 0x10400073" ::: "memory");
+	csr_write(CSR_SCTRSTATUS, ctx->status | HART_CTR_FROZEN);
+
+	count = hart_ctr_entry_count(ctx->depth);
+	for (i = 0; i < count; i++) {
+		csr_write(CSR_SISELECT, HART_CTR_FIRST_SELECTOR + i);
+		csr_write(CSR_SIREG2, ctx->entries[i].target);
+		csr_write(CSR_SIREG3, ctx->entries[i].data);
+		csr_write(CSR_SIREG, ctx->entries[i].source);
+	}
+
+	if (misa_extension('H'))
+		csr_write(CSR_VSCTRCTL, ctx->vsctl);
+	csr_write(CSR_SCTRCTL, ctx->ctl);
+	csr_write(CSR_SCTRSTATUS, ctx->status);
+}
+#endif
+
 /**
  * Switches the HART context from the current domain to the target domain.
  * This includes changing domain assignments and reconfiguring PMP, as well
@@ -239,6 +312,13 @@ static int switch_to_next_domain_context(struct hart_context *ctx,
 		hart_aia_iprio_restore(dom_ctx);
 		csr_write(CSR_SISELECT, dom_ctx->siselect);
 	}
+#if __riscv_xlen == 64
+	if (ctx->ctr_ctx && dom_ctx->ctr_ctx) {
+		hart_ctr_save(ctx->ctr_ctx);
+		hart_ctr_restore(dom_ctx->ctr_ctx);
+		csr_write(CSR_SISELECT, dom_ctx->siselect);
+	}
+#endif
 
 	/* Eager context switch for float */
 	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_F) ||
@@ -302,6 +382,20 @@ static int hart_context_init(u32 hartindex)
 
 		ctx->stimecmp = ~0ULL;
 
+#if __riscv_xlen == 64
+		if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
+					   SBI_HART_EXT_SSCTR) &&
+		    sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
+					   SBI_HART_EXT_SSCSRIND)) {
+			ctx->ctr_ctx = sbi_zalloc(sizeof(*ctx->ctr_ctx));
+			if (!ctx->ctr_ctx) {
+				sbi_free(ctx);
+				return SBI_ENOMEM;
+			}
+			ctx->ctr_ctx->depth = csr_read(CSR_SCTRDEPTH);
+		}
+#endif
+
 		if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
 					   SBI_HART_EXT_V)) {
 			vec_size = sbi_vector_context_size();
@@ -309,6 +403,7 @@ static int hart_context_init(u32 hartindex)
 			/* Allocate the vector context pointer */
 			ctx->vec_ctx = sbi_zalloc(vec_size);
 			if (!ctx->vec_ctx) {
+				sbi_free(ctx->ctr_ctx);
 				sbi_free(ctx);
 				return SBI_ENOMEM;
 			}
-- 
2.43.0