6 files changed, 75 insertions, 47 deletions

diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index db5e47dfc303..c54e04c995ee 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -370,21 +370,21 @@ config CPU_BIG_ENDIAN
 
 config CPU_ICACHE_DISABLE
         bool "Disable I-Cache"
-        depends on CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020
+        depends on CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020 || CPU_V6
         help
           Say Y here to disable the processor instruction cache. Unless
           you have a reason not to or are unsure, say N.
 
 config CPU_DCACHE_DISABLE
         bool "Disable D-Cache"
-        depends on CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020
+        depends on CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020 || CPU_V6
         help
           Say Y here to disable the processor data cache. Unless
           you have a reason not to or are unsure, say N.
 
 config CPU_DCACHE_WRITETHROUGH
         bool "Force write through D-cache"
-        depends on (CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020) && !CPU_DCACHE_DISABLE
+        depends on (CPU_ARM920T || CPU_ARM922T || CPU_ARM925T || CPU_ARM926T || CPU_ARM1020 || CPU_V6) && !CPU_DCACHE_DISABLE
         default y if CPU_ARM925T
         help
           Say Y here to use the data cache in writethrough mode. Unless you
@@ -399,7 +399,7 @@ config CPU_CACHE_ROUND_ROBIN
 
 config CPU_BPREDICT_DISABLE
         bool "Disable branch prediction"
-        depends on CPU_ARM1020
+        depends on CPU_ARM1020 || CPU_V6
         help
           Say Y here to disable branch prediction.  If unsure, say N.
 
diff --git a/arch/arm/mm/abort-ev6.S b/arch/arm/mm/abort-ev6.S
index 8f76f3df7b4c..dbd346033122 100644
--- a/arch/arm/mm/abort-ev6.S
+++ b/arch/arm/mm/abort-ev6.S
@@ -20,6 +20,11 @@
  */
         .align  5
 ENTRY(v6_early_abort)
+#ifdef CONFIG_CPU_MPCORE
+        clrex
+#else
+        strex   r0, r1, [sp]                    @ Clear the exclusive monitor
+#endif
         mrc     p15, 0, r1, c5, c0, 0           @ get FSR
         mrc     p15, 0, r0, c6, c0, 0           @ get FAR
 /*
diff --git a/arch/arm/mm/alignment.c b/arch/arm/mm/alignment.c
index 4b39d867ac14..705c98921c37 100644
--- a/arch/arm/mm/alignment.c
+++ b/arch/arm/mm/alignment.c
@@ -111,7 +111,7 @@ proc_alignment_read(char *page, char **start, off_t off, int count, int *eof,
 }
 
 static int proc_alignment_write(struct file *file, const char __user *buffer,
-                               unsigned long count, void *data)
+                                unsigned long count, void *data)
 {
         char mode;
 
@@ -119,7 +119,7 @@ static int proc_alignment_write(struct file *file, const char __user *buffer,
                 if (get_user(mode, buffer))
                         return -EFAULT;
                 if (mode >= '0' && mode <= '5')
-                           ai_usermode = mode - '0';
+                        ai_usermode = mode - '0';
         }
         return count;
 }
@@ -262,7 +262,7 @@ union offset_union {
                         goto fault;                             \
         } while (0)
 
-#define put32_unaligned_check(val,addr)  \
+#define put32_unaligned_check(val,addr) \
         __put32_unaligned_check("strb", val, addr)
 
 #define put32t_unaligned_check(val,addr) \
@@ -306,19 +306,19 @@ do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *r
         return TYPE_LDST;
 
  user:
-        if (LDST_L_BIT(instr)) {
-                unsigned long val;
-                get16t_unaligned_check(val, addr);
+        if (LDST_L_BIT(instr)) {
+                unsigned long val;
+                get16t_unaligned_check(val, addr);
 
-                /* signed half-word? */
-                if (instr & 0x40)
-                        val = (signed long)((signed short) val);
+                /* signed half-word? */
+                if (instr & 0x40)
+                        val = (signed long)((signed short) val);
 
-                regs->uregs[rd] = val;
-        } else
-                put16t_unaligned_check(regs->uregs[rd], addr);
+                regs->uregs[rd] = val;
+        } else
+                put16t_unaligned_check(regs->uregs[rd], addr);
 
-        return TYPE_LDST;
+        return TYPE_LDST;
 
  fault:
         return TYPE_FAULT;
@@ -330,6 +330,9 @@ do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
 {
         unsigned int rd = RD_BITS(instr);
 
+        if (((rd & 1) == 1) || (rd == 14))
+                goto bad;
+
         ai_dword += 1;
 
         if (user_mode(regs))
@@ -339,11 +342,11 @@ do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
                 unsigned long val;
                 get32_unaligned_check(val, addr);
                 regs->uregs[rd] = val;
-                get32_unaligned_check(val, addr+4);
-                regs->uregs[rd+1] = val;
+                get32_unaligned_check(val, addr + 4);
+                regs->uregs[rd + 1] = val;
         } else {
                 put32_unaligned_check(regs->uregs[rd], addr);
-                put32_unaligned_check(regs->uregs[rd+1], addr+4);
+                put32_unaligned_check(regs->uregs[rd + 1], addr + 4);
         }
 
         return TYPE_LDST;
@@ -353,15 +356,16 @@ do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
                 unsigned long val;
                 get32t_unaligned_check(val, addr);
                 regs->uregs[rd] = val;
-                get32t_unaligned_check(val, addr+4);
-                regs->uregs[rd+1] = val;
+                get32t_unaligned_check(val, addr + 4);
+                regs->uregs[rd + 1] = val;
         } else {
                 put32t_unaligned_check(regs->uregs[rd], addr);
-                put32t_unaligned_check(regs->uregs[rd+1], addr+4);
+                put32t_unaligned_check(regs->uregs[rd + 1], addr + 4);
         }
 
         return TYPE_LDST;
-
+ bad:
+        return TYPE_ERROR;
  fault:
         return TYPE_FAULT;
 }
@@ -439,7 +443,7 @@ do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *reg
         if (LDST_P_EQ_U(instr)) /* U = P */
                 eaddr += 4;
 
-        /* 
+        /*
          * For alignment faults on the ARM922T/ARM920T the MMU  makes
          * the FSR (and hence addr) equal to the updated base address
          * of the multiple access rather than the restored value.
@@ -566,7 +570,7 @@ thumb2arm(u16 tinstr)
         /* 6.5.1 Format 3: */
         case 0x4800 >> 11:                              /* 7.1.28 LDR(3) */
                 /* NOTE: This case is not technically possible. We're
-                 *       loading 32-bit memory data via PC relative
+                 *       loading 32-bit memory data via PC relative
                  *       addressing mode. So we can and should eliminate
                  *       this case. But I'll leave it here for now.
                  */
@@ -638,7 +642,7 @@ do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 
         if (fault) {
                 type = TYPE_FAULT;
-                goto bad_or_fault;
+                goto bad_or_fault;
         }
 
         if (user_mode(regs))
@@ -663,6 +667,8 @@ do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
                 else if ((instr & 0x001000f0) == 0x000000d0 || /* LDRD */
                          (instr & 0x001000f0) == 0x000000f0)   /* STRD */
                         handler = do_alignment_ldrdstrd;
+                else if ((instr & 0x01f00ff0) == 0x01000090) /* SWP */
+                        goto swp;
                 else
                         goto bad;
                 break;
@@ -733,6 +739,9 @@ do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
         do_bad_area(current, current->mm, addr, fsr, regs);
         return 0;
 
+ swp:
+        printk(KERN_ERR "Alignment trap: not handling swp instruction\n");
+
  bad:
         /*
          * Oops, we didn't handle the instruction.
diff --git a/arch/arm/mm/cache-v6.S b/arch/arm/mm/cache-v6.S
index 85c10a71e7c6..72966d90e956 100644
--- a/arch/arm/mm/cache-v6.S
+++ b/arch/arm/mm/cache-v6.S
@@ -18,6 +18,7 @@
 #define HARVARD_CACHE
 #define CACHE_LINE_SIZE         32
 #define D_CACHE_LINE_SIZE       32
+#define BTB_FLUSH_SIZE          8
 
 /*
  *      v6_flush_cache_all()
@@ -98,7 +99,13 @@ ENTRY(v6_coherent_user_range)
         mcr     p15, 0, r0, c7, c5, 1           @ invalidate I line
 #endif
         mcr     p15, 0, r0, c7, c5, 7           @ invalidate BTB entry
-        add     r0, r0, #CACHE_LINE_SIZE
+        add     r0, r0, #BTB_FLUSH_SIZE
+        mcr     p15, 0, r0, c7, c5, 7           @ invalidate BTB entry
+        add     r0, r0, #BTB_FLUSH_SIZE
+        mcr     p15, 0, r0, c7, c5, 7           @ invalidate BTB entry
+        add     r0, r0, #BTB_FLUSH_SIZE
+        mcr     p15, 0, r0, c7, c5, 7           @ invalidate BTB entry
+        add     r0, r0, #BTB_FLUSH_SIZE
         cmp     r0, r1
         blo     1b
 #ifdef HARVARD_CACHE
diff --git a/arch/arm/mm/flush.c b/arch/arm/mm/flush.c
index b0208c992576..c9a03981b785 100644
--- a/arch/arm/mm/flush.c
+++ b/arch/arm/mm/flush.c
@@ -17,6 +17,24 @@
 
 #ifdef CONFIG_CPU_CACHE_VIPT
 
+#define ALIAS_FLUSH_START       0xffff4000
+
+#define TOP_PTE(x)      pte_offset_kernel(top_pmd, x)
+
+static void flush_pfn_alias(unsigned long pfn, unsigned long vaddr)
+{
+        unsigned long to = ALIAS_FLUSH_START + (CACHE_COLOUR(vaddr) << PAGE_SHIFT);
+
+        set_pte(TOP_PTE(to), pfn_pte(pfn, PAGE_KERNEL));
+        flush_tlb_kernel_page(to);
+
+        asm(    "mcrr   p15, 0, %1, %0, c14\n"
+        "       mcrr    p15, 0, %1, %0, c5\n"
+            :
+            : "r" (to), "r" (to + PAGE_SIZE - L1_CACHE_BYTES)
+            : "cc");
+}
+
 void flush_cache_mm(struct mm_struct *mm)
 {
         if (cache_is_vivt()) {
@@ -67,24 +85,6 @@ void flush_cache_page(struct vm_area_struct *vma, unsigned long user_addr, unsig
         if (cache_is_vipt_aliasing())
                 flush_pfn_alias(pfn, user_addr);
 }
-
-#define ALIAS_FLUSH_START       0xffff4000
-
-#define TOP_PTE(x)      pte_offset_kernel(top_pmd, x)
-
-static void flush_pfn_alias(unsigned long pfn, unsigned long vaddr)
-{
-        unsigned long to = ALIAS_FLUSH_START + (CACHE_COLOUR(vaddr) << PAGE_SHIFT);
-
-        set_pte(TOP_PTE(to), pfn_pte(pfn, PAGE_KERNEL));
-        flush_tlb_kernel_page(to);
-
-        asm(    "mcrr   p15, 0, %1, %0, c14\n"
-        "       mcrr    p15, 0, %1, %0, c5\n"
-            :
-            : "r" (to), "r" (to + PAGE_SIZE - L1_CACHE_BYTES)
-            : "cc");
-}
 #else
 #define flush_pfn_alias(pfn,vaddr)      do { } while (0)
 #endif
diff --git a/arch/arm/mm/proc-v6.S b/arch/arm/mm/proc-v6.S
index caf3b19b167f..9bb5fff406fb 100644
--- a/arch/arm/mm/proc-v6.S
+++ b/arch/arm/mm/proc-v6.S
@@ -55,7 +55,14 @@ ENTRY(cpu_v6_proc_init)
         mov     pc, lr
 
 ENTRY(cpu_v6_proc_fin)
-        mov     pc, lr
+        stmfd   sp!, {lr}
+        cpsid   if                              @ disable interrupts
+        bl      v6_flush_kern_cache_all
+        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
+        bic     r0, r0, #0x1000                 @ ...i............
+        bic     r0, r0, #0x0006                 @ .............ca.
+        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
+        ldmfd   sp!, {pc}
 
 /*
  *      cpu_v6_reset(loc)