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: NLu Feifei <lufeifei@wxiat.com>
Reviewed-by: NHe Sheng <hesheng@wxiat.com>
Signed-off-by: NGu Zitao <guzitao@wxiat.com>

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
 

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 */