[PATCH v2 5/5] LoongArch: KVM: Enable separate vmid feature

[Bibo Mao]

With CSR GTLBC shortname for Guest TLB Control Register, separate vmid
feature will be enabled if bit 14 CSR_GTLBC_USEVMID is set. Enable
this feature if cpu_has_guestid is true when LVZ is enabled and the LVZ
version is 2.

Signed-off-by: Bibo Mao <maobibo@xxxxxxxxxxx>
---
 arch/loongarch/include/asm/loongarch.h | 5 +++++
 arch/loongarch/kernel/cpu-probe.c      | 6 ++++++
 arch/loongarch/kvm/main.c              | 4 +++-
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/arch/loongarch/include/asm/loongarch.h b/arch/loongarch/include/asm/loongarch.h
index 2a6bc99177d8..a20fb68d8ac4 100644
--- a/arch/loongarch/include/asm/loongarch.h
+++ b/arch/loongarch/include/asm/loongarch.h
@@ -138,6 +138,9 @@
 #define  CPUCFG6_PMBITS_SHIFT		8
 #define  CPUCFG6_UPM			BIT(14)
 
+#define LOONGARCH_CPUCFG8		0x8
+#define  CPUCFG8_VMID			BIT(8)
+
 #define LOONGARCH_CPUCFG16		0x10
 #define  CPUCFG16_L1_IUPRE		BIT(0)
 #define  CPUCFG16_L1_IUUNIFY		BIT(1)
@@ -350,6 +353,8 @@
 #define  CSR_GTLBC_TGID_WIDTH		8
 #define  CSR_GTLBC_TGID_SHIFT_END	(CSR_GTLBC_TGID_SHIFT + CSR_GTLBC_TGID_WIDTH - 1)
 #define  CSR_GTLBC_TGID			(_ULCAST_(0xff) << CSR_GTLBC_TGID_SHIFT)
+#define  CSR_GTLBC_USEVMID_SHIFT	14
+#define  CSR_GTLBC_USEVMID		(_ULCAST_(0x1) << CSR_GTLBC_USEVMID_SHIFT)
 #define  CSR_GTLBC_TOTI_SHIFT		13
 #define  CSR_GTLBC_TOTI			(_ULCAST_(0x1) << CSR_GTLBC_TOTI_SHIFT)
 #define  CSR_GTLBC_USETGID_SHIFT	12
diff --git a/arch/loongarch/kernel/cpu-probe.c b/arch/loongarch/kernel/cpu-probe.c
index 74d31f260dfd..fc68f695d49c 100644
--- a/arch/loongarch/kernel/cpu-probe.c
+++ b/arch/loongarch/kernel/cpu-probe.c
@@ -241,6 +241,12 @@ static void cpu_probe_common(struct cpuinfo_loongarch *c)
 	if (config & CPUCFG6_PMP)
 		c->options |= LOONGARCH_CPU_PMP;
 
+	if (c->options & LOONGARCH_CPU_LVZ) {
+		config = read_cpucfg(LOONGARCH_CPUCFG8);
+		if (config & CPUCFG8_VMID)
+			c->options |= LOONGARCH_CPU_GUESTID;
+	}
+
 	config = csr_read32(LOONGARCH_CSR_ASID);
 	config = (config & CSR_ASID_BIT) >> CSR_ASID_BIT_SHIFT;
 	asid_mask = GENMASK(config - 1, 0);
diff --git a/arch/loongarch/kvm/main.c b/arch/loongarch/kvm/main.c
index 72c1d23156e5..712b078db0d5 100644
--- a/arch/loongarch/kvm/main.c
+++ b/arch/loongarch/kvm/main.c
@@ -359,7 +359,7 @@ int kvm_arch_enable_virtualization_cpu(void)
 	write_csr_gcfg(0);
 	write_csr_gstat(0);
 	write_csr_gintc(0);
-	clear_csr_gtlbc(CSR_GTLBC_USETGID | CSR_GTLBC_TOTI);
+	clear_csr_gtlbc(CSR_GTLBC_USETGID | CSR_GTLBC_TOTI | CSR_GTLBC_USEVMID);
 
 	/*
 	 * Enable virtualization features granting guest direct control of
@@ -382,6 +382,8 @@ int kvm_arch_enable_virtualization_cpu(void)
 
 	/* Enable using TGID  */
 	set_csr_gtlbc(CSR_GTLBC_USETGID);
+	if (cpu_has_guestid)
+		set_csr_gtlbc(CSR_GTLBC_USEVMID);
 	kvm_debug("GCFG:%lx GSTAT:%lx GINTC:%lx GTLBC:%lx",
 		  read_csr_gcfg(), read_csr_gstat(), read_csr_gintc(), read_csr_gtlbc());
 
-- 
2.39.3