[PATCH v3 3/3] 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_USES2TGID is set. Enable
this if S2TGID feature is detected from cpucfg8.

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              | 6 ++++--
 3 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/arch/loongarch/include/asm/loongarch.h b/arch/loongarch/include/asm/loongarch.h
index 2a6bc99177d8..88e3bdfd4971 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_S2TGID			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_USES2TGID_SHIFT	14
+#define  CSR_GTLBC_USES2TGID		(_ULCAST_(0x1) << CSR_GTLBC_USES2TGID_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..e849ebacd268 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_S2TGID)
+			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 0099d0f0b803..e43ed4c11d31 100644
--- a/arch/loongarch/kvm/main.c
+++ b/arch/loongarch/kvm/main.c
@@ -358,7 +358,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_USES2TGID);
 
 	/*
 	 * Enable virtualization features granting guest direct control of
@@ -381,6 +381,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_USES2TGID);
 	kvm_debug("GCFG:%lx GSTAT:%lx GINTC:%lx GTLBC:%lx",
 		  read_csr_gcfg(), read_csr_gstat(), read_csr_gintc(), read_csr_gtlbc());
 
@@ -399,7 +401,7 @@ void kvm_arch_disable_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_USES2TGID);
 
 	/* Flush any remaining guest TLB entries */
 	kvm_flush_tlb_all();
-- 
2.39.3