lib: sbi_tlb: Fix remote TLB HFENCE VVMA implementation

The HFENCE VVMA instructions flushes TLB based on the VMID
present in HGATP CSR. To handle this, we get the current
VMID for SBI HFENCE VVMA call and we use this current VMID
to do remote TLB HFENCE VVMA on desired set of HARTs.

Signed-off-by: Anup Patel <anup.patel@wdc.com>
Reviewed-by: Atish Patra <atish.patra@wdc.com>

diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 674e9b9..fd96b0f 100644 (file)
--- 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 6a07ee2..6ee64a9 100644 (file)
--- 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 9f7a5b6..3bfe749 100644 (file)
--- 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 88a6f08..e460c30 100644 (file)
--- 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 d857162..c6ca7b1 100644 (file)
--- 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) {