6 files changed, 83 insertions, 22 deletions

diff --git a/arch/microblaze/include/asm/mmu.h b/arch/microblaze/include/asm/mmu.h
index 5198de8b1224..1f9edddf7f4b 100644
--- a/arch/microblaze/include/asm/mmu.h
+++ b/arch/microblaze/include/asm/mmu.h
@@ -56,6 +56,12 @@ typedef struct _SEGREG {
 
 extern void _tlbie(unsigned long va);   /* invalidate a TLB entry */
 extern void _tlbia(void);               /* invalidate all TLB entries */
+
+/*
+ * tlb_skip size stores actual number skipped TLBs from TLB0 - every directy TLB
+ * mapping has to increase tlb_skip size.
+ */
+extern u32 tlb_skip;
 #   endif /* __ASSEMBLY__ */
 
 /*
@@ -68,7 +74,12 @@ extern void _tlbia(void);		/* invalidate all TLB entries */
  */
 
 #  define MICROBLAZE_TLB_SIZE 64
-#  define MICROBLAZE_TLB_SKIP 2
+
+/* For cases when you want to skip some TLB entries */
+#  define MICROBLAZE_TLB_SKIP 0
+
+/* Use the last TLB for temporary access to LMB */
+#  define MICROBLAZE_LMB_TLB_ID 63
 
 /*
  * TLB entries are defined by a "high" tag portion and a "low" data
diff --git a/arch/microblaze/kernel/early_printk.c b/arch/microblaze/kernel/early_printk.c
index 742c247792ce..ec485876d0d0 100644
--- a/arch/microblaze/kernel/early_printk.c
+++ b/arch/microblaze/kernel/early_printk.c
@@ -175,6 +175,20 @@ void __init remap_early_printk(void)
                                                                 base_addr);
         base_addr = (u32) ioremap(base_addr, PAGE_SIZE);
         printk(KERN_CONT "0x%x\n", base_addr);
+
+        /*
+         * Early console is on the top of skipped TLB entries
+         * decrease tlb_skip size ensure that hardcoded TLB entry will be
+         * used by generic algorithm
+         * FIXME check if early console mapping is on the top by rereading
+         * TLB entry and compare baseaddr
+         *  mts  rtlbx, (tlb_skip - 1)
+         *  nop
+         *  mfs  rX, rtlblo
+         *  nop
+         *  cmp rX, orig_base_addr
+         */
+        tlb_skip -= 1;
 }
 
 void __init disable_early_printk(void)
diff --git a/arch/microblaze/kernel/head.S b/arch/microblaze/kernel/head.S
index 49dd48f9e6ec..98b17f9f904b 100644
--- a/arch/microblaze/kernel/head.S
+++ b/arch/microblaze/kernel/head.S
@@ -149,6 +149,7 @@ _copy_bram:
 _invalidate:
         mts     rtlbx, r3
         mts     rtlbhi, r0                      /* flush: ensure V is clear   */
+        mts     rtlblo, r0
         bgtid   r3, _invalidate         /* loop for all entries       */
         addik   r3, r3, -1
         /* sync */
@@ -224,8 +225,14 @@ tlb_end:
         andi    r4,r4,0xfffffc00        /* Mask off the real page number */
         ori     r4,r4,(TLB_WR | TLB_EX) /* Set the write and execute bits */
 
-        /* TLB0 can be zeroes that's why we not setup it */
-        beqi    r9, jump_over
+        /*
+         * TLB0 is always used - check if is not zero (r9 stores TLB0 value)
+         * if is use TLB1 value and clear it (r10 stores TLB1 value)
+         */
+        bnei    r9, tlb0_not_zero
+        add     r9, r10, r0
+        add     r10, r0, r0
+tlb0_not_zero:
 
         /* look at the code below */
         ori     r30, r0, 0x200
@@ -239,18 +246,21 @@ tlb_end:
         bneid   r29, 1f
         addik   r30, r30, 0x80
 1:
-        ori r11, r30, 0
-
         andi    r3,r3,0xfffffc00        /* Mask off the effective page number */
         ori     r3,r3,(TLB_VALID)
-        or      r3, r3, r11
+        or      r3, r3, r30
 
-        mts     rtlbx,r0                /* TLB slow 0 */
+        /* Load tlb_skip size value which is index to first unused TLB entry */
+        lwi     r11, r0, TOPHYS(tlb_skip)
+        mts     rtlbx,r11               /* TLB slow 0 */
 
         mts     rtlblo,r4               /* Load the data portion of the entry */
         mts     rtlbhi,r3               /* Load the tag portion of the entry */
 
-jump_over:
+        /* Increase tlb_skip size */
+        addik   r11, r11, 1
+        swi     r11, r0, TOPHYS(tlb_skip)
+
         /* TLB1 can be zeroes that's why we not setup it */
         beqi    r10, jump_over2
 
@@ -266,27 +276,30 @@ jump_over:
         bneid   r29, 1f
         addik   r30, r30, 0x80
 1:
-        ori r12, r30, 0
-
         addk    r4, r4, r9      /* previous addr + TLB0 size */
         addk    r3, r3, r9
 
         andi    r3,r3,0xfffffc00        /* Mask off the effective page number */
         ori     r3,r3,(TLB_VALID)
-        or      r3, r3, r12
+        or      r3, r3, r30
 
-        ori     r6,r0,1                 /* TLB slot 1 */
-        mts     rtlbx,r6
+        lwi     r11, r0, TOPHYS(tlb_skip)
+        mts     rtlbx, r11              /* r11 is used from TLB0 */
 
         mts     rtlblo,r4               /* Load the data portion of the entry */
         mts     rtlbhi,r3               /* Load the tag portion of the entry */
 
+        /* Increase tlb_skip size */
+        addik   r11, r11, 1
+        swi     r11, r0, TOPHYS(tlb_skip)
+
 jump_over2:
         /*
          * Load a TLB entry for LMB, since we need access to
          * the exception vectors, using a 4k real==virtual mapping.
          */
-        ori     r6,r0,3                 /* TLB slot 3 */
+        /* Use temporary TLB_ID for LMB - clear this temporary mapping later */
+        ori     r6, r0, MICROBLAZE_LMB_TLB_ID
         mts     rtlbx,r6
 
         ori     r4,r0,(TLB_WR | TLB_EX)
@@ -355,8 +368,7 @@ start_here:
 
         /* Load up the kernel context */
 kernel_load_context:
-        # Keep entry 0 and 1 valid. Entry 3 mapped to LMB can go away.
-        ori     r5,r0,3
+        ori     r5, r0, MICROBLAZE_LMB_TLB_ID
         mts     rtlbx,r5
         nop
         mts     rtlbhi,r0
diff --git a/arch/microblaze/kernel/hw_exception_handler.S b/arch/microblaze/kernel/hw_exception_handler.S
index b7249f4215a2..aa510f450ac6 100644
--- a/arch/microblaze/kernel/hw_exception_handler.S
+++ b/arch/microblaze/kernel/hw_exception_handler.S
@@ -820,9 +820,15 @@ ex_handler_done:
  *      Upon exit, we reload everything and RFI.
  * A common place to load the TLB.
  */
+.section .data
+.align 4
+.global tlb_skip
+        tlb_skip:
+                .long   MICROBLAZE_TLB_SKIP
         tlb_index:
                 /* MS: storing last used tlb index */
-                .long   (MICROBLAZE_TLB_SKIP - 1)
+                .long   MICROBLAZE_TLB_SIZE/2
+.previous
         finish_tlb_load:
                 /* MS: load the last used TLB index. */
                 lwi     r5, r0, TOPHYS(tlb_index)
@@ -833,7 +839,7 @@ ex_handler_done:
                 ori     r6, r0, 1
                 cmp     r31, r5, r6
                 blti    r31, ex12
-                addik   r5, r6, MICROBLAZE_TLB_SKIP - 1
+                lwi     r5, r0, TOPHYS(tlb_skip)
         ex12:
                 /* MS: save back current TLB index */
                 swi     r5, r0, TOPHYS(tlb_index)
diff --git a/arch/microblaze/kernel/misc.S b/arch/microblaze/kernel/misc.S
index c9090d7973f6..1dafddeb8a0b 100644
--- a/arch/microblaze/kernel/misc.S
+++ b/arch/microblaze/kernel/misc.S
@@ -29,16 +29,16 @@
 .type  _tlbia, @function
 .align 4;
 _tlbia:
-        addik   r12, r0, MICROBLAZE_TLB_SIZE - 1 /* flush all entries (63 - 3) */
+        lwi     r12, r0, tlb_skip;
         /* isync */
 _tlbia_1:
         mts     rtlbx, r12
         nop
         mts     rtlbhi, r0 /* flush: ensure V is clear */
         nop
-        addik   r11, r12, -MICROBLAZE_TLB_SKIP
+        rsubi   r11, r12, MICROBLAZE_TLB_SIZE - 1
         bneid   r11, _tlbia_1 /* loop for all entries */
-        addik   r12, r12, -1
+        addik   r12, r12, 1
         /* sync */
         rtsd    r15, 8
         nop
@@ -75,7 +75,7 @@ early_console_reg_tlb_alloc:
          * Load a TLB entry for the UART, so that microblaze_progress() can use
          * the UARTs nice and early.  We use a 4k real==virtual mapping.
          */
-        ori     r4, r0, 63
+        lwi     r4, r0, tlb_skip
         mts     rtlbx, r4 /* TLB slot 63 */
 
         or      r4,r5,r0
@@ -89,6 +89,11 @@ early_console_reg_tlb_alloc:
         nop
         mts     rtlbhi,r5 /* Load the tag portion of the entry */
         nop
+
+        lwi     r5, r0, tlb_skip
+        addik   r5, r5, 1
+        swi     r5, r0, tlb_skip
+
         rtsd    r15, 8
         nop
 
diff --git a/arch/microblaze/kernel/setup.c b/arch/microblaze/kernel/setup.c
index a1fa2a5813bf..e4f5956ee13c 100644
--- a/arch/microblaze/kernel/setup.c
+++ b/arch/microblaze/kernel/setup.c
@@ -208,6 +208,19 @@ static int microblaze_debugfs_init(void)
         return of_debugfs_root == NULL;
 }
 arch_initcall(microblaze_debugfs_init);
+
+static int __init debugfs_tlb(void)
+{
+        struct dentry *d;
+
+        if (!of_debugfs_root)
+                return -ENODEV;
+
+        d = debugfs_create_u32("tlb_skip", S_IRUGO, of_debugfs_root, &tlb_skip);
+        if (!d)
+                return -ENOMEM;
+}
+device_initcall(debugfs_tlb);
 #endif
 
 static int dflt_bus_notify(struct notifier_block *nb,