diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 674e9b93..fd96b0fd 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -111,6 +111,21 @@
 #define SATP_MODE_SV57			_UL(10)
 #define SATP_MODE_SV64			_UL(11)
 
+#define HGATP_MODE_OFF			_UL(0)
+#define HGATP_MODE_SV32X4		_UL(1)
+#define HGATP_MODE_SV39X4		_UL(8)
+#define HGATP_MODE_SV48X4		_UL(9)
+
+#define HGATP32_MODE_SHIFT		31
+#define HGATP32_VMID_SHIFT		22
+#define HGATP32_VMID_MASK		_UL(0x1FC00000)
+#define HGATP32_PPN			_UL(0x003FFFFF)
+
+#define HGATP64_MODE_SHIFT		60
+#define HGATP64_VMID_SHIFT		44
+#define HGATP64_VMID_MASK		_ULL(0x03FFF00000000000)
+#define HGATP64_PPN			_ULL(0x00000FFFFFFFFFFF)
+
 #define PMP_R				_UL(0x01)
 #define PMP_W				_UL(0x02)
 #define PMP_X				_UL(0x04)
@@ -127,10 +142,20 @@
 #define MSTATUS_SD			MSTATUS64_SD
 #define SSTATUS_SD			SSTATUS64_SD
 #define SATP_MODE			SATP64_MODE
+
+#define HGATP_PPN			HGATP64_PPN
+#define HGATP_VMID_SHIFT		HGATP64_VMID_SHIFT
+#define HGATP_VMID_MASK			HGATP64_VMID_MASK
+#define HGATP_MODE_SHIFT		HGATP64_MODE_SHIFT
 #else
 #define MSTATUS_SD			MSTATUS32_SD
 #define SSTATUS_SD			SSTATUS32_SD
 #define SATP_MODE			SATP32_MODE
+
+#define HGATP_PPN			HGATP32_PPN
+#define HGATP_VMID_SHIFT		HGATP32_VMID_SHIFT
+#define HGATP_VMID_MASK			HGATP32_VMID_MASK
+#define HGATP_MODE_SHIFT		HGATP32_MODE_SHIFT
 #endif
 
 #define CSR_USTATUS			0x0
diff --git a/include/sbi/sbi_tlb.h b/include/sbi/sbi_tlb.h
index 6a07ee2c..6ee64a90 100644
--- a/include/sbi/sbi_tlb.h
+++ b/include/sbi/sbi_tlb.h
@@ -38,17 +38,19 @@ struct sbi_tlb_info {
 	unsigned long start;
 	unsigned long size;
 	unsigned long asid;
+	unsigned long vmid;
 	unsigned long type;
 	struct sbi_hartmask smask;
 };
 
-#define SBI_TLB_INFO_INIT(__ptr, __start, __size, __asid, __type, __src_hart) \
+#define SBI_TLB_INFO_INIT(__p, __start, __size, __asid, __vmid, __type, __src) \
 do { \
-	(__ptr)->start = (__start); \
-	(__ptr)->size = (__size); \
-	(__ptr)->asid = (__asid); \
-	(__ptr)->type = (__type); \
-	SBI_HARTMASK_INIT_EXCEPT(&(__ptr)->smask, (__src_hart)); \
+	(__p)->start = (__start); \
+	(__p)->size = (__size); \
+	(__p)->asid = (__asid); \
+	(__p)->vmid = (__vmid); \
+	(__p)->type = (__type); \
+	SBI_HARTMASK_INIT_EXCEPT(&(__p)->smask, (__src)); \
 } while (0)
 
 #define SBI_TLB_INFO_SIZE		sizeof(struct sbi_tlb_info)
diff --git a/lib/sbi/sbi_ecall_legacy.c b/lib/sbi/sbi_ecall_legacy.c
index 9f7a5b6d..3bfe7496 100644
--- a/lib/sbi/sbi_ecall_legacy.c
+++ b/lib/sbi/sbi_ecall_legacy.c
@@ -76,7 +76,7 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 		ret = sbi_load_hart_mask_unpriv((ulong *)args[0],
 						&hmask, out_trap);
 		if (ret != SBI_ETRAP) {
-			SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0,
+			SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0, 0,
 					  SBI_ITLB_FLUSH, source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		}
@@ -85,7 +85,7 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 		ret = sbi_load_hart_mask_unpriv((ulong *)args[0],
 						&hmask, out_trap);
 		if (ret != SBI_ETRAP) {
-			SBI_TLB_INFO_INIT(&tlb_info, args[1], args[2], 0,
+			SBI_TLB_INFO_INIT(&tlb_info, args[1], args[2], 0, 0,
 					  SBI_TLB_FLUSH_VMA, source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		}
@@ -95,7 +95,8 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 						&hmask, out_trap);
 		if (ret != SBI_ETRAP) {
 			SBI_TLB_INFO_INIT(&tlb_info, args[1], args[2], args[3],
-					  SBI_TLB_FLUSH_VMA_ASID, source_hart);
+					  0, SBI_TLB_FLUSH_VMA_ASID,
+					  source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		}
 		break;
diff --git a/lib/sbi/sbi_ecall_replace.c b/lib/sbi/sbi_ecall_replace.c
index 88a6f08c..e460c305 100644
--- a/lib/sbi/sbi_ecall_replace.c
+++ b/lib/sbi/sbi_ecall_replace.c
@@ -46,6 +46,7 @@ static int sbi_ecall_rfence_handler(unsigned long extid, unsigned long funcid,
 				    struct sbi_trap_info *out_trap)
 {
 	int ret = 0;
+	unsigned long vmid;
 	struct sbi_tlb_info tlb_info;
 	u32 source_hart = current_hartid();
 
@@ -56,37 +57,41 @@ static int sbi_ecall_rfence_handler(unsigned long extid, unsigned long funcid,
 
 	switch (funcid) {
 	case SBI_EXT_RFENCE_REMOTE_FENCE_I:
-		SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0,
+		SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0, 0,
 				  SBI_ITLB_FLUSH, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_HFENCE_GVMA:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0,
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0, 0,
 				  SBI_TLB_FLUSH_GVMA, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_HFENCE_GVMA_VMID:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], args[4],
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0, args[4],
 				  SBI_TLB_FLUSH_GVMA_VMID, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_HFENCE_VVMA:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0,
+		vmid = (csr_read(CSR_HGATP) & HGATP_VMID_MASK);
+		vmid = vmid >> HGATP_VMID_SHIFT;
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0, vmid,
 				  SBI_TLB_FLUSH_VVMA, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_HFENCE_VVMA_ASID:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], args[4],
+		vmid = (csr_read(CSR_HGATP) & HGATP_VMID_MASK);
+		vmid = vmid >> HGATP_VMID_SHIFT;
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], args[4], vmid,
 				  SBI_TLB_FLUSH_VVMA_ASID, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_SFENCE_VMA:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0,
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], 0, 0,
 				  SBI_TLB_FLUSH_VMA, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
 	case SBI_EXT_RFENCE_REMOTE_SFENCE_VMA_ASID:
-		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], args[4],
+		SBI_TLB_INFO_INIT(&tlb_info, args[2], args[3], args[4], 0,
 				  SBI_TLB_FLUSH_VMA_ASID, source_hart);
 		ret = sbi_tlb_request(args[0], args[1], &tlb_info);
 		break;
diff --git a/lib/sbi/sbi_tlb.c b/lib/sbi/sbi_tlb.c
index d857162f..c6ca7b19 100644
--- a/lib/sbi/sbi_tlb.c
+++ b/lib/sbi/sbi_tlb.c
@@ -36,16 +36,23 @@ static void sbi_tlb_hfence_vvma(struct sbi_tlb_info *tinfo)
 {
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
-	unsigned long i;
+	unsigned long vmid  = tinfo->vmid;
+	unsigned long i, hgatp;
+
+	hgatp = csr_swap(CSR_HGATP,
+			 (vmid << HGATP_VMID_SHIFT) & HGATP_VMID_MASK);
 
 	if ((start == 0 && size == 0) || (size == SBI_TLB_FLUSH_ALL)) {
 		__sbi_hfence_vvma_all();
-		return;
+		goto done;
 	}
 
 	for (i = 0; i < size; i += PAGE_SIZE) {
 		__sbi_hfence_vvma_va(start+i);
 	}
+
+done:
+	csr_write(CSR_HGATP, hgatp);
 }
 
 static void sbi_tlb_hfence_gvma(struct sbi_tlb_info *tinfo)
@@ -88,28 +95,35 @@ static void sbi_tlb_hfence_vvma_asid(struct sbi_tlb_info *tinfo)
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
 	unsigned long asid  = tinfo->asid;
-	unsigned long i;
+	unsigned long vmid  = tinfo->vmid;
+	unsigned long i, hgatp;
+
+	hgatp = csr_swap(CSR_HGATP,
+			 (vmid << HGATP_VMID_SHIFT) & HGATP_VMID_MASK);
 
 	if (start == 0 && size == 0) {
 		__sbi_hfence_vvma_all();
-		return;
+		goto done;
 	}
 
 	if (size == SBI_TLB_FLUSH_ALL) {
 		__sbi_hfence_vvma_asid(asid);
-		return;
+		goto done;
 	}
 
 	for (i = 0; i < size; i += PAGE_SIZE) {
 		__sbi_hfence_vvma_asid_va(asid, start + i);
 	}
+
+done:
+	csr_write(CSR_HGATP, hgatp);
 }
 
 static void sbi_tlb_hfence_gvma_vmid(struct sbi_tlb_info *tinfo)
 {
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
-	unsigned long vmid  = tinfo->asid;
+	unsigned long vmid  = tinfo->vmid;
 	unsigned long i;
 
 	if (start == 0 && size == 0) {