3 files changed, 92 insertions, 5 deletions

diff --git a/arch/sparc64/kernel/sun4v_tlb_miss.S b/arch/sparc64/kernel/sun4v_tlb_miss.S
index df65d712dcc6..244d50de8499 100644
--- a/arch/sparc64/kernel/sun4v_tlb_miss.S
+++ b/arch/sparc64/kernel/sun4v_tlb_miss.S
@@ -84,8 +84,9 @@ sun4v_itlb_load:
         mov     %g3, %o2                ! PTE
         mov     HV_MMU_IMMU, %o3        ! flags
         ta      HV_MMU_MAP_ADDR_TRAP
+        brnz,pn %o0, sun4v_itlb_error
+         mov    %g2, %o1                ! restore %o1
         mov     %g1, %o0                ! restore %o0
-        mov     %g2, %o1                ! restore %o1
         mov     %g5, %o2                ! restore %o2
         mov     %g7, %o3                ! restore %o3
 
@@ -126,8 +127,9 @@ sun4v_dtlb_load:
         mov     %g3, %o2                ! PTE
         mov     HV_MMU_DMMU, %o3        ! flags
         ta      HV_MMU_MAP_ADDR_TRAP
+        brnz,pn %o0, sun4v_dtlb_error
+         mov    %g2, %o1                ! restore %o1
         mov     %g1, %o0                ! restore %o0
-        mov     %g2, %o1                ! restore %o1
         mov     %g5, %o2                ! restore %o2
         mov     %g7, %o3                ! restore %o3
 
@@ -154,6 +156,7 @@ sun4v_itsb_miss:
         ldxa    [%g1] ASI_SCRATCHPAD, %g1
         brz,pn  %g5, kvmap_itlb_4v
          mov    FAULT_CODE_ITLB, %g3
+        ba,a,pt %xcc, sun4v_tsb_miss_common
 
         /* Called from trap table with TAG TARGET placed into
          * %g6 and SCRATCHPAD_UTSBREG1 contents in %g1.
@@ -182,6 +185,56 @@ sun4v_tsb_miss_common:
         ba,pt   %xcc, tsb_miss_page_table_walk_sun4v_fastpath
          ldx    [%g2 + TRAP_PER_CPU_PGD_PADDR], %g7
 
+sun4v_itlb_error:
+        sethi   %hi(sun4v_err_itlb_vaddr), %g1
+        stx     %g4, [%g1 + %lo(sun4v_err_itlb_vaddr)]
+        sethi   %hi(sun4v_err_itlb_ctx), %g1
+        srlx    %g6, 48, %o1            ! ctx
+        stx     %o1, [%g1 + %lo(sun4v_err_itlb_ctx)]
+        sethi   %hi(sun4v_err_itlb_pte), %g1
+        stx     %g3, [%g1 + %lo(sun4v_err_itlb_pte)]
+        sethi   %hi(sun4v_err_itlb_error), %g1
+        stx     %o0, [%g1 + %lo(sun4v_err_itlb_error)]
+
+        rdpr    %tl, %g4
+        cmp     %g4, 1
+        ble,pt  %icc, 1f
+         sethi  %hi(2f), %g7
+        ba,pt   %xcc, etraptl1
+         or     %g7, %lo(2f), %g7
+
+1:      ba,pt   %xcc, etrap
+2:       or     %g7, %lo(2b), %g7
+        call    sun4v_itlb_error_report
+         add    %sp, PTREGS_OFF, %o0
+
+        /* NOTREACHED */
+
+sun4v_dtlb_error:
+        sethi   %hi(sun4v_err_dtlb_vaddr), %g1
+        stx     %g4, [%g1 + %lo(sun4v_err_dtlb_vaddr)]
+        sethi   %hi(sun4v_err_dtlb_ctx), %g1
+        srlx    %g6, 48, %o1            ! ctx
+        stx     %o1, [%g1 + %lo(sun4v_err_dtlb_ctx)]
+        sethi   %hi(sun4v_err_dtlb_pte), %g1
+        stx     %g3, [%g1 + %lo(sun4v_err_dtlb_pte)]
+        sethi   %hi(sun4v_err_dtlb_error), %g1
+        stx     %o0, [%g1 + %lo(sun4v_err_dtlb_error)]
+
+        rdpr    %tl, %g4
+        cmp     %g4, 1
+        ble,pt  %icc, 1f
+         sethi  %hi(2f), %g7
+        ba,pt   %xcc, etraptl1
+         or     %g7, %lo(2f), %g7
+
+1:      ba,pt   %xcc, etrap
+2:       or     %g7, %lo(2b), %g7
+        call    sun4v_dtlb_error_report
+         add    %sp, PTREGS_OFF, %o0
+
+        /* NOTREACHED */
+
         /* Instruction Access Exception, tl0. */
 sun4v_iacc:
         ldxa    [%g0] ASI_SCRATCHPAD, %g2
diff --git a/arch/sparc64/kernel/traps.c b/arch/sparc64/kernel/traps.c
index c9484ae5bb8f..5a157e92bfc7 100644
--- a/arch/sparc64/kernel/traps.c
+++ b/arch/sparc64/kernel/traps.c
@@ -1928,6 +1928,40 @@ void sun4v_nonresum_overflow(struct pt_regs *regs)
         atomic_inc(&sun4v_nonresum_oflow_cnt);
 }
 
+unsigned long sun4v_err_itlb_vaddr;
+unsigned long sun4v_err_itlb_ctx;
+unsigned long sun4v_err_itlb_pte;
+unsigned long sun4v_err_itlb_error;
+
+void sun4v_itlb_error_report(struct pt_regs *regs, int tl)
+{
+        if (tl > 1)
+                dump_tl1_traplog((struct tl1_traplog *)(regs + 1));
+
+        printk("SUN4V-ITLB: Error at TPC[%lx], tl %d\n", regs->tpc, tl);
+        printk("SUN4V-ITLB: vaddr[%lx] ctx[%lx] pte[%lx] error[%lx]\n",
+               sun4v_err_itlb_vaddr, sun4v_err_itlb_ctx,
+               sun4v_err_itlb_pte, sun4v_err_itlb_error);
+        prom_halt();
+}
+
+unsigned long sun4v_err_dtlb_vaddr;
+unsigned long sun4v_err_dtlb_ctx;
+unsigned long sun4v_err_dtlb_pte;
+unsigned long sun4v_err_dtlb_error;
+
+void sun4v_dtlb_error_report(struct pt_regs *regs, int tl)
+{
+        if (tl > 1)
+                dump_tl1_traplog((struct tl1_traplog *)(regs + 1));
+
+        printk("SUN4V-DTLB: Error at TPC[%lx], tl %d\n", regs->tpc, tl);
+        printk("SUN4V-DTLB: vaddr[%lx] ctx[%lx] pte[%lx] error[%lx]\n",
+               sun4v_err_dtlb_vaddr, sun4v_err_dtlb_ctx,
+               sun4v_err_dtlb_pte, sun4v_err_dtlb_error);
+        prom_halt();
+}
+
 void do_fpe_common(struct pt_regs *regs)
 {
         if (regs->tstate & TSTATE_PRIV) {
diff --git a/arch/sparc64/kernel/tsb.S b/arch/sparc64/kernel/tsb.S
index 7996c9d66702..a17259cf34b8 100644
--- a/arch/sparc64/kernel/tsb.S
+++ b/arch/sparc64/kernel/tsb.S
@@ -135,8 +135,8 @@ tsb_do_fault:
         wrpr            %g5, PSTATE_AG | PSTATE_MG, %pstate
         .section        .sun4v_2insn_patch, "ax"
         .word           661b
-        nop
-        nop
+        SET_GL(1)
+        ldxa            [%g0] ASI_SCRATCHPAD, %g2
         .previous
 
         bne,pn          %xcc, tsb_do_itlb_fault
@@ -150,7 +150,7 @@ tsb_do_dtlb_fault:
         ldxa    [%g4] ASI_DMMU, %g5
         .section .sun4v_2insn_patch, "ax"
         .word   661b
-        mov     %g4, %g5
+        ldx     [%g2 + HV_FAULT_D_ADDR_OFFSET], %g5
         nop
         .previous