lib/sbi/illegal_insn.c: add emulation for fence.tso

firmware/fw_base.S

@@ -713,6 +713,7 @@ fw_platform_init:
 	.globl _trap_handler
 	.globl _trap_exit
 _trap_handler:
+	sfence.vma zero, t0
 	TRAP_SAVE_AND_SETUP_SP_T0
 
 	TRAP_SAVE_MEPC_MSTATUS 0

include/sbi/riscv_encoding.h

@@ -625,6 +625,9 @@
 #define INSN_MASK_WFI			0xffffff00
 #define INSN_MATCH_WFI			0x10500000
 
+#define INSN_MASK_FENCE_TSO		0xffffffff
+#define INSN_MATCH_FENCE_TSO		0x8330000f
+
 #define INSN_16BIT_MASK			0x3
 #define INSN_32BIT_MASK			0x1c
 

lib/sbi/sbi_illegal_insn.c

@@ -8,6 +8,7 @@
  */
 
 #include <sbi/riscv_asm.h>
+#include <sbi/riscv_barrier.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_emulate_csr.h>
@@ -31,6 +32,18 @@ static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
 	return sbi_trap_redirect(regs, &trap);
 }
 
+static int misc_mem_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	/* Errata workaround: emulate `fence.tso` as `fence rw, rw`. */
+	if ((insn & INSN_MASK_FENCE_TSO) == INSN_MATCH_FENCE_TSO) {
+		smp_mb();
+		regs->mepc += 4;
+		return 0;
+	}
+
+	return truly_illegal_insn(insn, regs);
+}
+
 static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
 {
 	int do_write, rs1_num = (insn >> 15) & 0x1f;
@@ -83,7 +96,7 @@ static illegal_insn_func illegal_insn_table[32] = {
 	truly_illegal_insn, /* 0 */
 	truly_illegal_insn, /* 1 */
 	truly_illegal_insn, /* 2 */
-	truly_illegal_insn, /* 3 */
+	misc_mem_opcode_insn, /* 3 */
 	truly_illegal_insn, /* 4 */
 	truly_illegal_insn, /* 5 */
 	truly_illegal_insn, /* 6 */

lib/sbi/sbi_scratch.c

@@ -14,7 +14,7 @@
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_string.h>
 
-u32 last_hartid_having_scratch = SBI_HARTMASK_MAX_BITS;
+u32 last_hartid_having_scratch = SBI_HARTMASK_MAX_BITS - 1;
 struct sbi_scratch *hartid_to_scratch_table[SBI_HARTMASK_MAX_BITS] = { 0 };
 
 static spinlock_t extra_lock = SPIN_LOCK_INITIALIZER;
@@ -74,7 +74,7 @@ done:
 	spin_unlock(&extra_lock);
 
 	if (ret) {
-		for (i = 0; i < sbi_scratch_last_hartid(); i++) {
+		for (i = 0; i <= sbi_scratch_last_hartid(); i++) {
 			rscratch = sbi_hartid_to_scratch(i);
 			if (!rscratch)
 				continue;

lib/sbi/sbi_tlb.c

@@ -29,7 +29,11 @@ static unsigned long tlb_range_flush_limit;
 
 static void sbi_tlb_flush_all(void)
 {
+#ifdef FW_NO_SFENCE_VMA
+	csr_write(0x9c3, 1 << 26);
+#else
 	__asm__ __volatile("sfence.vma");
+#endif
 }
 
 void sbi_tlb_local_hfence_vvma(struct sbi_tlb_info *tinfo)
@@ -73,6 +77,9 @@ void sbi_tlb_local_hfence_gvma(struct sbi_tlb_info *tinfo)
 
 void sbi_tlb_local_sfence_vma(struct sbi_tlb_info *tinfo)
 {
+#ifdef FW_NO_SFENCE_VMA
+	csr_write(0x9c3, 1 << 26);
+#else
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
 	unsigned long i;
@@ -88,6 +95,7 @@ void sbi_tlb_local_sfence_vma(struct sbi_tlb_info *tinfo)
 				     : "r"(start + i)
 				     : "memory");
 	}
+#endif
 }
 
 void sbi_tlb_local_hfence_vvma_asid(struct sbi_tlb_info *tinfo)
@@ -143,6 +151,9 @@ void sbi_tlb_local_hfence_gvma_vmid(struct sbi_tlb_info *tinfo)
 
 void sbi_tlb_local_sfence_vma_asid(struct sbi_tlb_info *tinfo)
 {
+#ifdef FW_NO_SFENCE_VMA
+	csr_write(0x9c3, 1 << 26);
+#else
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
 	unsigned long asid  = tinfo->asid;
@@ -168,6 +179,7 @@ void sbi_tlb_local_sfence_vma_asid(struct sbi_tlb_info *tinfo)
 				     : "r"(start + i), "r"(asid)
 				     : "memory");
 	}
+#endif
 }
 
 void sbi_tlb_local_fence_i(struct sbi_tlb_info *tinfo)

lib/sbi/sbi_trap.c

@@ -225,6 +225,15 @@ struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
 
 	if (mcause & (1UL << (__riscv_xlen - 1))) {
 		mcause &= ~(1UL << (__riscv_xlen - 1));
+
+		/*
+		 * DCACHE.CALL:
+		 * | 31 - 25 | 24 - 20 | 19 - 15 | 14 - 12 | 11 - 7 | 6 - 0 |
+		 *   0000000    00001     00000      000      00000  0001011
+		 */
+		if ((regs->mepc & 0x7f) == 4)
+			asm volatile(".long 0x0010000b\n");
+
 		switch (mcause) {
 		case IRQ_M_TIMER:
 			sbi_timer_process();