diff --git a/src/isa/riscv64/local-include/csr.h b/src/isa/riscv64/local-include/csr.h
index 51015ab0e..4284d28ad 100644
--- a/src/isa/riscv64/local-include/csr.h
+++ b/src/isa/riscv64/local-include/csr.h
@@ -18,6 +18,7 @@
 #define __CSR_H__
 
 #include <common.h>
+#include <memory/vaddr.h>
 
 /**
  * Mapping between CSR name and addr
@@ -1041,6 +1042,8 @@ MAP(CSRS, CSRS_DECL)
 #define IDXVLENB  0xc22
 
 /** CSR satp **/
+#define SATP_MODE_BARE 0
+#define SATP_MODE_Sv39 8
 #define SATP_ASID_LEN 16 // max is 16
 #define SATP_PADDR_LEN (CONFIG_PADDRBITS-12) // max is 44
 #define SATP_ASID_MAX_LEN 16
@@ -1055,6 +1058,8 @@ MAP(CSRS, CSRS_DECL)
 
 /** CSR hgatp **/
 #ifdef CONFIG_RVH
+#define HGATP_MODE_BARE   0
+#define HGATP_MODE_Sv39x4 8
 #define HGATP_VMID_LEN 14 // max is 14
 #define HGATP_PADDR_LEN 44 // max is 44
 #define HGATP_VMID_MAX_LEN 16
@@ -1065,7 +1070,13 @@ MAP(CSRS, CSRS_DECL)
 #define HGATP_PADDR_MASK ((1L << HGATP_PADDR_MAX_LEN)-1)
 
 #define HGATP_MASK (HGATP_MODE_MASK | HGATP_VMID_MASK | HGATP_PADDR_MASK)
-#define MASKED_HGATP(x) (HGATP_MASK & x)
+#endif // CONFIG_RVH
+
+#ifdef CONFIG_RVH
+#define HGATP_Bare_GPADDR_LEN CONFIG_PADDRBITS
+#define HGATP_Sv39x4_GPADDR_LEN 41
+#define VSATP_PPN_HGATP_BARE_MASK BITMASK(HGATP_Bare_GPADDR_LEN - PAGE_SHIFT)
+#define VSATP_PPN_HGATP_Sv39x4_MASK BITMASK(HGATP_Sv39x4_GPADDR_LEN - PAGE_SHIFT)
 #endif // CONFIG_RVH
 
 /** RVH **/
diff --git a/src/isa/riscv64/system/priv.c b/src/isa/riscv64/system/priv.c
index 09f4de453..a7d8a889a 100644
--- a/src/isa/riscv64/system/priv.c
+++ b/src/isa/riscv64/system/priv.c
@@ -484,8 +484,26 @@ void disable_time_intr() {
     mie->val = mask_bitset(mie->val, MTIE_MASK, 0);
 }
 
+#ifdef CONFIG_RVH
+void update_vsatp(const vsatp_t new_val) {
+  vsatp->_64.mode = new_val._64.mode;
+  vsatp->_64.asid = new_val._64.asid;
+  switch (hgatp->mode) {
+    case HGATP_MODE_BARE:
+      vsatp->_64.ppn = new_val._64.ppn & VSATP_PPN_HGATP_BARE_MASK;
+      break;
+    case HGATP_MODE_Sv39x4:
+      vsatp->_64.ppn = new_val._64.ppn & VSATP_PPN_HGATP_Sv39x4_MASK;
+      break;
+    default:
+      panic("HGATP.mode is illegal value(%lx), when write vsatp\n", (uint64_t)hgatp->mode);
+      break;
+  }
+}
+#endif
+
 static inline void csr_write(word_t *dest, word_t src) {
-  #ifdef CONFIG_RVH
+#ifdef CONFIG_RVH
   if(cpu.v == 1 && (is_write(sstatus) || is_write(sie) || is_write(stvec) || is_write(sscratch)
         || is_write(sepc) || is_write(scause) || is_write(stval) || is_write(sip)
         || is_write(satp) || is_write(stvec))){
@@ -503,9 +521,13 @@ static inline void csr_write(word_t *dest, word_t src) {
       if (cpu.mode == MODE_S && hstatus->vtvm == 1) {
         longjmp_exception(EX_VI);
       }
-      if ((src & SATP_SV39_MASK) >> 60 == 8 || (src & SATP_SV39_MASK) >> 60 == 0)
-        vsatp->val = MASKED_SATP(src);
-    }else if( is_write(stvec))  {vstvec->val = src & ~(0x2UL);}
+      vsatp_t new_val = (vsatp_t)src;
+      // legal mode
+      if (new_val._64.mode == SATP_MODE_BARE || new_val._64.mode == SATP_MODE_Sv39) {
+        update_vsatp(new_val);
+      }
+    }
+    else if( is_write(stvec))  {vstvec->val = src & ~(0x2UL);}
   }else if (is_write(mideleg)){
     *dest = (src & 0x222) | MIDELEG_FORCED_MASK;
   }else if (is_write(hideleg)){
@@ -538,8 +560,11 @@ static inline void csr_write(word_t *dest, word_t src) {
     if (cpu.mode == MODE_S && hstatus->vtvm == 1) {
       longjmp_exception(EX_VI);
     }
-    if ((src & SATP_SV39_MASK) >> 60 == 8 || (src & SATP_SV39_MASK) >> 60 == 0)
-      vsatp->val = MASKED_SATP(src);
+    vsatp_t new_val = (vsatp_t)src;
+    // legal mode
+    if (new_val._64.mode == SATP_MODE_BARE || new_val._64.mode == SATP_MODE_Sv39) {
+      update_vsatp(new_val);
+    }
   }else if (is_write(mstatus)) { mstatus->val = mask_bitset(mstatus->val, MSTATUS_WMASK, src); }
 #else
   if (is_write(mstatus)) {
@@ -746,9 +771,16 @@ static inline void csr_write(word_t *dest, word_t src) {
   }
 #ifdef CONFIG_RVH
   else if (is_write(hgatp)) {
-    // Only support Sv39, ignore write that sets other mode
-    if ((src & SATP_SV39_MASK) >> 60 == 8 || (src & SATP_SV39_MASK) >> 60 == 0)
-      hgatp->val = MASKED_HGATP(src);
+    hgatp_t new_val = (hgatp_t)src;
+    // vmid and ppn WARL in the normal way, regardless of new_val.mode
+    hgatp->vmid = new_val.vmid;
+    // Make PPN[1:0] read only zero
+    hgatp->ppn = new_val.ppn & ~(rtlreg_t)3 & BITMASK(CONFIG_PADDRBITS - PAGE_SHIFT);
+
+    // Only support Sv39x4, ignore write that sets other mode
+    if (new_val.mode == HGATP_MODE_Sv39x4 || new_val.mode == HGATP_MODE_BARE)
+      hgatp->mode = new_val.mode;
+    // When MODE=Bare, software should set the remaining fields in hgatp to zeros, not hardware.
   }
 #endif// CONFIG_RVH
   else if (is_mhpmcounter(dest) || is_mhpmevent(dest)) {