[PATCH v2 2/2] riscv64: Fix huge page translation in vtop_riscv64()

[Rui Qi]

When a page table entry has _PAGE_LEAF set at PGD/P4D/PUD/PMD level,
the current code incorrectly falls through to the next level instead
of computing the correct physical address. Fix this by calculating
the paddr with the proper level shift and returning directly.

Co-developed-by: Rui Qi <qirui.001 at bytedance.com>
Signed-off-by: Shuan He <heshuan at bytedance.com>
---
 arch/riscv64.c | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/arch/riscv64.c b/arch/riscv64.c
index 28365fa0cc5b..7b640b6f7d29 100644
--- a/arch/riscv64.c
+++ b/arch/riscv64.c
@@ -107,8 +107,12 @@ vtop_riscv64(pgd_t * pgd, unsigned long vaddr, long va_bits)
 
 	pt_phys = (pt_val >> _PAGE_PFN_SHIFT) << PAGESHIFT();
 
-	if (pt_val & _PAGE_LEAF)
-		goto out;
+	if (pt_val & _PAGE_LEAF) {
+		unsigned long shift = (va_bits == VA_BITS_SV57) ? PGD_SHIFT_L5 :
+				      (va_bits == VA_BITS_SV48) ? PGD_SHIFT_L4 : PGD_SHIFT_L3;
+		paddr = (pt_phys & ~((1ULL << shift) - 1)) + (vaddr & ((1ULL << shift) - 1));
+		return paddr;
+	}
 
 	if (va_bits == VA_BITS_SV57)
 		goto p4d;
@@ -133,8 +137,10 @@ p4d:
 
 	pt_phys = (pt_val >> _PAGE_PFN_SHIFT) << PAGESHIFT();
 
-	if (pt_val & _PAGE_LEAF)
-		goto out;
+	if (pt_val & _PAGE_LEAF) {
+		paddr = (pt_phys & ~((1ULL << P4D_SHIFT) - 1)) + (vaddr & ((1ULL << P4D_SHIFT) - 1));
+		return paddr;
+	}
 pud:
 	/* PUD */
 	puda = (pud_t *)(pt_phys) + pud_index(vaddr);
@@ -152,8 +158,10 @@ pud:
 
 	pt_phys = (pt_val >> _PAGE_PFN_SHIFT) << PAGESHIFT();
 
-	if(pt_val & _PAGE_LEAF)
-		goto out;
+	if (pt_val & _PAGE_LEAF) {
+		paddr = (pt_phys & ~((1ULL << PUD_SHIFT) - 1)) + (vaddr & ((1ULL << PUD_SHIFT) - 1));
+		return paddr;
+	}
 pmd:
 	/* PMD */
 	pmda = (pmd_t *)(pt_phys) + pmd_index(vaddr);
@@ -171,8 +179,10 @@ pmd:
 
 	pt_phys = (pt_val >> _PAGE_PFN_SHIFT) << PAGESHIFT();
 
-	if (pt_val & _PAGE_LEAF)
-		goto out;
+	if (pt_val & _PAGE_LEAF) {
+		paddr = (pt_phys & ~((1ULL << PMD_SHIFT) - 1)) + (vaddr & ((1ULL << PMD_SHIFT) - 1));
+		return paddr;
+	}
 
 	/* PTE */
 	ptea = (pte_t *)(pt_phys) + pte_index(vaddr);
@@ -190,8 +200,7 @@ pmd:
 
 	pt_phys = (pt_val >> _PAGE_PFN_SHIFT) << PAGESHIFT();
 
-out:
-	paddr = pt_phys + PAGEOFFSET(vaddr);
+	return pt_phys + PAGEOFFSET(vaddr);
 invalid:
 	return paddr;
 }
-- 
2.20.1