From 3860d52fa5d2c47a4449022163cb7e8f174e0f58 Mon Sep 17 00:00:00 2001
From: Lu Feifei <lufeifei@wxiat.com>
Date: Wed, 8 Feb 2023 17:43:38 +0800
Subject: [PATCH] sw64: add SIMD emulation for guest IO access

Sunway inclusion
category: feature
bugzilla: https://gitee.com/openeuler/kernel/issues/I56WV8

--------------------------------

To support SIMD IO access in guest, an exception type IF_SIMDEMU has
been added in do_entIF(), which is used to emualte SIMD load/store.

Currently, only vldd and vstd are supported for emulation.

Signed-off-by: Lu Feifei <lufeifei@wxiat.com>
Reviewed-by: He Sheng <hesheng@wxiat.com>
Signed-off-by: Gu Zitao <guzitao@wxiat.com>
---
 arch/sw_64/kernel/entry.S |  2 +-
 arch/sw_64/kernel/traps.c | 28 +++++++++++++++++++++++++++-
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/arch/sw_64/kernel/entry.S b/arch/sw_64/kernel/entry.S
index 750bd634dde3..988ce3352e8b 100644
--- a/arch/sw_64/kernel/entry.S
+++ b/arch/sw_64/kernel/entry.S
@@ -121,7 +121,7 @@ entMM:
 entIF:
 	SAVE_ALL
 	ldi	$26, ret_from_sys_call
-	mov	$sp, $17
+	mov	$sp, $18
 	call	$31, do_entIF
 	.end entIF
 
diff --git a/arch/sw_64/kernel/traps.c b/arch/sw_64/kernel/traps.c
index 38a013129008..bc5a2291f29d 100644
--- a/arch/sw_64/kernel/traps.c
+++ b/arch/sw_64/kernel/traps.c
@@ -38,6 +38,7 @@ enum SW64_IF_TYPES {
 	IF_GENTRAP,
 	IF_FEN,
 	IF_OPDEC,
+	IF_SIMDEMU,
 };
 
 void show_regs(struct pt_regs *regs)
@@ -165,12 +166,32 @@ do_entArith(unsigned long summary, unsigned long write_mask,
 	force_sig_fault(SIGFPE, si_code, (void __user *)regs->pc, 0);
 }
 
+void simd_emulate(unsigned int inst, unsigned long va)
+{
+	unsigned long *fp;
+	int instr_opc, reg;
+
+	instr_opc = (inst >> 26) & 0x3f;
+	reg = (inst >> 21) & 0x1f;
+	fp = (unsigned long *) va;
+
+	switch (instr_opc) {
+	case 0x0d: /* vldd */
+		sw64_write_simd_fp_reg_d(reg, fp[0], fp[1], fp[2], fp[3]);
+		return;
+
+	case 0x0f: /* vstd */
+		sw64_read_simd_fp_m_d(reg, fp);
+		return;
+	}
+}
+
 /*
  * BPT/GENTRAP/OPDEC make regs->pc = exc_pc + 4. debugger should
  * do something necessary to handle it correctly.
  */
 asmlinkage void
-do_entIF(unsigned long inst_type, struct pt_regs *regs)
+do_entIF(unsigned long inst_type, unsigned long va, struct pt_regs *regs)
 {
 	int signo, code;
 	unsigned int inst, type;
@@ -178,6 +199,11 @@ do_entIF(unsigned long inst_type, struct pt_regs *regs)
 	type = inst_type & 0xffffffff;
 	inst = inst_type >> 32;
 
+	if (type == IF_SIMDEMU) {
+		simd_emulate(inst, va);
+		return;
+	}
+
 	if (!user_mode(regs) && type != IF_OPDEC) {
 		if (type == IF_BREAKPOINT) {
 			/* support kgdb */
-- 
GitLab