3 files changed, 82 insertions, 19 deletions

diff --git a/arch/arm/include/asm/memory.h b/arch/arm/include/asm/memory.h
index d9b96c65e594..6748d6295a1a 100644
--- a/arch/arm/include/asm/memory.h
+++ b/arch/arm/include/asm/memory.h
@@ -172,9 +172,14 @@
  * so that all we need to do is modify the 8-bit constant field.
  */
 #define __PV_BITS_31_24 0x81000000
+#define __PV_BITS_7_0   0x81
 
 extern phys_addr_t (*arch_virt_to_idmap) (unsigned long x);
-extern unsigned long __pv_phys_offset;
+extern u64 __pv_phys_offset;
+extern u64 __pv_offset;
+extern void fixup_pv_table(const void *, unsigned long);
+extern const void *__pv_table_begin, *__pv_table_end;
+
 #define PHYS_OFFSET __pv_phys_offset
 
 #define __pv_stub(from,to,instr,type)                   \
@@ -186,10 +191,36 @@ extern unsigned long __pv_phys_offset;
         : "=r" (to)                                     \
         : "r" (from), "I" (type))
 
+#define __pv_stub_mov_hi(t)                             \
+        __asm__ volatile("@ __pv_stub_mov\n"            \
+        "1:     mov     %R0, %1\n"                      \
+        "       .pushsection .pv_table,\"a\"\n"         \
+        "       .long   1b\n"                           \
+        "       .popsection\n"                          \
+        : "=r" (t)                                      \
+        : "I" (__PV_BITS_7_0))
+
+#define __pv_add_carry_stub(x, y)                       \
+        __asm__ volatile("@ __pv_add_carry_stub\n"      \
+        "1:     adds    %Q0, %1, %2\n"                  \
+        "       adc     %R0, %R0, #0\n"                 \
+        "       .pushsection .pv_table,\"a\"\n"         \
+        "       .long   1b\n"                           \
+        "       .popsection\n"                          \
+        : "+r" (y)                                      \
+        : "r" (x), "I" (__PV_BITS_31_24)                \
+        : "cc")
+
 static inline phys_addr_t __virt_to_phys(unsigned long x)
 {
-        unsigned long t;
-        __pv_stub(x, t, "add", __PV_BITS_31_24);
+        phys_addr_t t;
+
+        if (sizeof(phys_addr_t) == 4) {
+                __pv_stub(x, t, "add", __PV_BITS_31_24);
+        } else {
+                __pv_stub_mov_hi(t);
+                __pv_add_carry_stub(x, t);
+        }
         return t;
 }
 
diff --git a/arch/arm/kernel/armksyms.c b/arch/arm/kernel/armksyms.c
index 60d3b738d420..1f031ddd0667 100644
--- a/arch/arm/kernel/armksyms.c
+++ b/arch/arm/kernel/armksyms.c
@@ -155,4 +155,5 @@ EXPORT_SYMBOL(__gnu_mcount_nc);
 
 #ifdef CONFIG_ARM_PATCH_PHYS_VIRT
 EXPORT_SYMBOL(__pv_phys_offset);
+EXPORT_SYMBOL(__pv_offset);
 #endif
diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index 2c7cc1e03473..54547947a4e9 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -536,6 +536,14 @@ ENTRY(fixup_smp)
         ldmfd   sp!, {r4 - r6, pc}
 ENDPROC(fixup_smp)
 
+#ifdef __ARMEB_
+#define LOW_OFFSET      0x4
+#define HIGH_OFFSET     0x0
+#else
+#define LOW_OFFSET      0x0
+#define HIGH_OFFSET     0x4
+#endif
+
 #ifdef CONFIG_ARM_PATCH_PHYS_VIRT
 
 /* __fixup_pv_table - patch the stub instructions with the delta between
@@ -546,17 +554,20 @@ ENDPROC(fixup_smp)
         __HEAD
 __fixup_pv_table:
         adr     r0, 1f
-        ldmia   r0, {r3-r5, r7}
-        sub     r3, r0, r3      @ PHYS_OFFSET - PAGE_OFFSET
+        ldmia   r0, {r3-r7}
+        mvn     ip, #0
+        subs    r3, r0, r3      @ PHYS_OFFSET - PAGE_OFFSET
         add     r4, r4, r3      @ adjust table start address
         add     r5, r5, r3      @ adjust table end address
-        add     r7, r7, r3      @ adjust __pv_phys_offset address
-        str     r8, [r7]        @ save computed PHYS_OFFSET to __pv_phys_offset
+        add     r6, r6, r3      @ adjust __pv_phys_offset address
+        add     r7, r7, r3      @ adjust __pv_offset address
+        str     r8, [r6, #LOW_OFFSET]   @ save computed PHYS_OFFSET to __pv_phys_offset
+        strcc   ip, [r7, #HIGH_OFFSET]  @ save to __pv_offset high bits
         mov     r6, r3, lsr #24 @ constant for add/sub instructions
         teq     r3, r6, lsl #24 @ must be 16MiB aligned
 THUMB(  it      ne              @ cross section branch )
         bne     __error
-        str     r6, [r7, #4]    @ save to __pv_offset
+        str     r3, [r7, #LOW_OFFSET]   @ save to __pv_offset low bits
         b       __fixup_a_pv_table
 ENDPROC(__fixup_pv_table)
 
@@ -565,10 +576,19 @@ ENDPROC(__fixup_pv_table)
         .long   __pv_table_begin
         .long   __pv_table_end
 2:      .long   __pv_phys_offset
+        .long   __pv_offset
 
         .text
 __fixup_a_pv_table:
+        adr     r0, 3f
+        ldr     r6, [r0]
+        add     r6, r6, r3
+        ldr     r0, [r6, #HIGH_OFFSET]  @ pv_offset high word
+        ldr     r6, [r6, #LOW_OFFSET]   @ pv_offset low word
+        mov     r6, r6, lsr #24
+        cmn     r0, #1
 #ifdef CONFIG_THUMB2_KERNEL
+        moveq   r0, #0x200000   @ set bit 21, mov to mvn instruction
         lsls    r6, #24
         beq     2f
         clz     r7, r6
@@ -582,18 +602,28 @@ __fixup_a_pv_table:
         b       2f
 1:      add     r7, r3
         ldrh    ip, [r7, #2]
-        and     ip, 0x8f00
-        orr     ip, r6  @ mask in offset bits 31-24
+        tst     ip, #0x4000
+        and     ip, #0x8f00
+        orrne   ip, r6  @ mask in offset bits 31-24
+        orreq   ip, r0  @ mask in offset bits 7-0
         strh    ip, [r7, #2]
+        ldrheq  ip, [r7]
+        biceq   ip, #0x20
+        orreq   ip, ip, r0, lsr #16
+        strheq  ip, [r7]
 2:      cmp     r4, r5
         ldrcc   r7, [r4], #4    @ use branch for delay slot
         bcc     1b
         bx      lr
 #else
+        moveq   r0, #0x400000   @ set bit 22, mov to mvn instruction
         b       2f
 1:      ldr     ip, [r7, r3]
         bic     ip, ip, #0x000000ff
-        orr     ip, ip, r6      @ mask in offset bits 31-24
+        tst     ip, #0xf00      @ check the rotation field
+        orrne   ip, ip, r6      @ mask in offset bits 31-24
+        biceq   ip, ip, #0x400000       @ clear bit 22
+        orreq   ip, ip, r0      @ mask in offset bits 7-0
         str     ip, [r7, r3]
 2:      cmp     r4, r5
         ldrcc   r7, [r4], #4    @ use branch for delay slot
@@ -602,28 +632,29 @@ __fixup_a_pv_table:
 #endif
 ENDPROC(__fixup_a_pv_table)
 
+3:      .long __pv_offset
+
 ENTRY(fixup_pv_table)
         stmfd   sp!, {r4 - r7, lr}
-        ldr     r2, 2f                  @ get address of __pv_phys_offset
         mov     r3, #0                  @ no offset
         mov     r4, r0                  @ r0 = table start
         add     r5, r0, r1              @ r1 = table size
-        ldr     r6, [r2, #4]            @ get __pv_offset
         bl      __fixup_a_pv_table
         ldmfd   sp!, {r4 - r7, pc}
 ENDPROC(fixup_pv_table)
 
-        .align
-2:      .long   __pv_phys_offset
-
         .data
         .globl  __pv_phys_offset
         .type   __pv_phys_offset, %object
 __pv_phys_offset:
-        .long   0
-        .size   __pv_phys_offset, . - __pv_phys_offset
+        .quad   0
+        .size   __pv_phys_offset, . -__pv_phys_offset
+
+        .globl  __pv_offset
+        .type   __pv_offset, %object
 __pv_offset:
-        .long   0
+        .quad   0
+        .size   __pv_offset, . -__pv_offset
 #endif
 
 #include "head-common.S"