static enum emulation_result kvm_vz_gpsi_cop0()

in kvm/vz.c [875:1072]

• 168 lines of code
• 69 McCabe index (conditional complexity)

static enum emulation_result kvm_vz_gpsi_cop0(union mips_instruction inst,
					      u32 *opc, u32 cause,
					      struct kvm_vcpu *vcpu)
{
	struct mips_coproc *cop0 = vcpu->arch.cop0;
	enum emulation_result er = EMULATE_DONE;
	u32 rt, rd, sel;
	unsigned long curr_pc;
	unsigned long val;

	/*
	 * Update PC and hold onto current PC in case there is
	 * an error and we want to rollback the PC
	 */
	curr_pc = vcpu->arch.pc;
	er = update_pc(vcpu, cause);
	if (er == EMULATE_FAIL)
		return er;

	if (inst.co_format.co) {
		switch (inst.co_format.func) {
		case wait_op:
			er = kvm_mips_emul_wait(vcpu);
			break;
		default:
			er = EMULATE_FAIL;
		}
	} else {
		rt = inst.c0r_format.rt;
		rd = inst.c0r_format.rd;
		sel = inst.c0r_format.sel;

		switch (inst.c0r_format.rs) {
		case dmfc_op:
		case mfc_op:
#ifdef CONFIG_KVM_MIPS_DEBUG_COP0_COUNTERS
			cop0->stat[rd][sel]++;
#endif
			if (rd == MIPS_CP0_COUNT &&
			    sel == 0) {			/* Count */
				val = kvm_mips_read_count(vcpu);
			} else if (rd == MIPS_CP0_COMPARE &&
				   sel == 0) {		/* Compare */
				val = read_gc0_compare();
			} else if (rd == MIPS_CP0_LLADDR &&
				   sel == 0) {		/* LLAddr */
				if (cpu_guest_has_rw_llb)
					val = read_gc0_lladdr() &
						MIPS_LLADDR_LLB;
				else
					val = 0;
			} else if (rd == MIPS_CP0_LLADDR &&
				   sel == 1 &&		/* MAAR */
				   cpu_guest_has_maar &&
				   !cpu_guest_has_dyn_maar) {
				/* MAARI must be in range */
				BUG_ON(kvm_read_sw_gc0_maari(cop0) >=
						ARRAY_SIZE(vcpu->arch.maar));
				val = vcpu->arch.maar[
					kvm_read_sw_gc0_maari(cop0)];
			} else if ((rd == MIPS_CP0_PRID &&
				    (sel == 0 ||	/* PRid */
				     sel == 2 ||	/* CDMMBase */
				     sel == 3)) ||	/* CMGCRBase */
				   (rd == MIPS_CP0_STATUS &&
				    (sel == 2 ||	/* SRSCtl */
				     sel == 3)) ||	/* SRSMap */
				   (rd == MIPS_CP0_CONFIG &&
				    (sel == 6 ||	/* Config6 */
				     sel == 7)) ||	/* Config7 */
				   (rd == MIPS_CP0_LLADDR &&
				    (sel == 2) &&	/* MAARI */
				    cpu_guest_has_maar &&
				    !cpu_guest_has_dyn_maar) ||
				   (rd == MIPS_CP0_ERRCTL &&
				    (sel == 0))) {	/* ErrCtl */
				val = cop0->reg[rd][sel];
#ifdef CONFIG_CPU_LOONGSON64
			} else if (rd == MIPS_CP0_DIAG &&
				   (sel == 0)) {	/* Diag */
				val = cop0->reg[rd][sel];
#endif
			} else {
				val = 0;
				er = EMULATE_FAIL;
			}

			if (er != EMULATE_FAIL) {
				/* Sign extend */
				if (inst.c0r_format.rs == mfc_op)
					val = (int)val;
				vcpu->arch.gprs[rt] = val;
			}

			trace_kvm_hwr(vcpu, (inst.c0r_format.rs == mfc_op) ?
					KVM_TRACE_MFC0 : KVM_TRACE_DMFC0,
				      KVM_TRACE_COP0(rd, sel), val);
			break;

		case dmtc_op:
		case mtc_op:
#ifdef CONFIG_KVM_MIPS_DEBUG_COP0_COUNTERS
			cop0->stat[rd][sel]++;
#endif
			val = vcpu->arch.gprs[rt];
			trace_kvm_hwr(vcpu, (inst.c0r_format.rs == mtc_op) ?
					KVM_TRACE_MTC0 : KVM_TRACE_DMTC0,
				      KVM_TRACE_COP0(rd, sel), val);

			if (rd == MIPS_CP0_COUNT &&
			    sel == 0) {			/* Count */
				kvm_vz_lose_htimer(vcpu);
				kvm_mips_write_count(vcpu, vcpu->arch.gprs[rt]);
			} else if (rd == MIPS_CP0_COMPARE &&
				   sel == 0) {		/* Compare */
				kvm_mips_write_compare(vcpu,
						       vcpu->arch.gprs[rt],
						       true);
			} else if (rd == MIPS_CP0_LLADDR &&
				   sel == 0) {		/* LLAddr */
				/*
				 * P5600 generates GPSI on guest MTC0 LLAddr.
				 * Only allow the guest to clear LLB.
				 */
				if (cpu_guest_has_rw_llb &&
				    !(val & MIPS_LLADDR_LLB))
					write_gc0_lladdr(0);
			} else if (rd == MIPS_CP0_LLADDR &&
				   sel == 1 &&		/* MAAR */
				   cpu_guest_has_maar &&
				   !cpu_guest_has_dyn_maar) {
				val = mips_process_maar(inst.c0r_format.rs,
							val);

				/* MAARI must be in range */
				BUG_ON(kvm_read_sw_gc0_maari(cop0) >=
						ARRAY_SIZE(vcpu->arch.maar));
				vcpu->arch.maar[kvm_read_sw_gc0_maari(cop0)] =
									val;
			} else if (rd == MIPS_CP0_LLADDR &&
				   (sel == 2) &&	/* MAARI */
				   cpu_guest_has_maar &&
				   !cpu_guest_has_dyn_maar) {
				kvm_write_maari(vcpu, val);
			} else if (rd == MIPS_CP0_CONFIG &&
				   (sel == 6)) {
				cop0->reg[rd][sel] = (int)val;
			} else if (rd == MIPS_CP0_ERRCTL &&
				   (sel == 0)) {	/* ErrCtl */
				/* ignore the written value */
#ifdef CONFIG_CPU_LOONGSON64
			} else if (rd == MIPS_CP0_DIAG &&
				   (sel == 0)) {	/* Diag */
				unsigned long flags;

				local_irq_save(flags);
				if (val & LOONGSON_DIAG_BTB) {
					/* Flush BTB */
					set_c0_diag(LOONGSON_DIAG_BTB);
				}
				if (val & LOONGSON_DIAG_ITLB) {
					/* Flush ITLB */
					set_c0_diag(LOONGSON_DIAG_ITLB);
				}
				if (val & LOONGSON_DIAG_DTLB) {
					/* Flush DTLB */
					set_c0_diag(LOONGSON_DIAG_DTLB);
				}
				if (val & LOONGSON_DIAG_VTLB) {
					/* Flush VTLB */
					kvm_loongson_clear_guest_vtlb();
				}
				if (val & LOONGSON_DIAG_FTLB) {
					/* Flush FTLB */
					kvm_loongson_clear_guest_ftlb();
				}
				local_irq_restore(flags);
#endif
			} else {
				er = EMULATE_FAIL;
			}
			break;

		default:
			er = EMULATE_FAIL;
			break;
		}
	}
	/* Rollback PC only if emulation was unsuccessful */
	if (er == EMULATE_FAIL) {
		kvm_err("[%#lx]%s: unsupported cop0 instruction 0x%08x\n",
			curr_pc, __func__, inst.word);

		vcpu->arch.pc = curr_pc;
	}

	return er;
}