1 files changed, 69 insertions, 20 deletions

diff --git a/include/asm-sparc64/cpudata.h b/include/asm-sparc64/cpudata.h
index f83768883e98..da54b4f35403 100644
--- a/include/asm-sparc64/cpudata.h
+++ b/include/asm-sparc64/cpudata.h
@@ -60,9 +60,18 @@ struct trap_per_cpu {
 } __attribute__((aligned(64)));
 extern struct trap_per_cpu trap_block[NR_CPUS];
 extern void init_cur_cpu_trap(void);
-extern void per_cpu_patch(void);
 extern void setup_tba(void);
 
+#ifdef CONFIG_SMP
+struct cpuid_patch_entry {
+        unsigned int    addr;
+        unsigned int    cheetah_safari[4];
+        unsigned int    cheetah_jbus[4];
+        unsigned int    starfire[4];
+};
+extern struct cpuid_patch_entry __cpuid_patch, __cpuid_patch_end;
+#endif
+
 #endif /* !(__ASSEMBLY__) */
 
 #define TRAP_PER_CPU_THREAD     0x00
@@ -70,35 +79,58 @@ extern void setup_tba(void);
 
 #define TRAP_BLOCK_SZ_SHIFT     6
 
-/* Clobbers %g1, loads %g6 with local processor's cpuid */
-#define __GET_CPUID                     \
-        ba,pt   %xcc, __get_cpu_id;     \
-         rd     %pc, %g1;
+#ifdef CONFIG_SMP
+
+#define __GET_CPUID(REG)                                \
+        /* Spitfire implementation (default). */        \
+661:    ldxa            [%g0] ASI_UPA_CONFIG, REG;      \
+        srlx            REG, 17, REG;                   \
+         and            REG, 0x1f, REG;                 \
+        nop;                                            \
+        .section        .cpuid_patch, "ax";             \
+        /* Instruction location. */                     \
+        .word           661b;                           \
+        /* Cheetah Safari implementation. */            \
+        ldxa            [%g0] ASI_SAFARI_CONFIG, REG;   \
+        srlx            REG, 17, REG;                   \
+        and             REG, 0x3ff, REG;                \
+        nop;                                            \
+        /* Cheetah JBUS implementation. */              \
+        ldxa            [%g0] ASI_JBUS_CONFIG, REG;     \
+        srlx            REG, 17, REG;                   \
+        and             REG, 0x1f, REG;                 \
+        nop;                                            \
+        /* Starfire implementation. */                  \
+        sethi           %hi(0x1fff40000d0 >> 9), REG;   \
+        sllx            REG, 9, REG;                    \
+        or              REG, 0xd0, REG;                 \
+        lduwa           [REG] ASI_PHYS_BYPASS_EC_E, REG;\
+        .previous;
 
 /* Clobbers %g1, current address space PGD phys address into %g7.  */
 #define TRAP_LOAD_PGD_PHYS                      \
-        __GET_CPUID                             \
-        sllx    %g6, TRAP_BLOCK_SZ_SHIFT, %g6;  \
+        __GET_CPUID(%g1)                        \
         sethi   %hi(trap_block), %g7;           \
+        sllx    %g1, TRAP_BLOCK_SZ_SHIFT, %g1;  \
         or      %g7, %lo(trap_block), %g7;      \
-        add     %g7, %g6, %g7;                  \
+        add     %g7, %g1, %g7;                  \
         ldx     [%g7 + TRAP_PER_CPU_PGD_PADDR], %g7;
 
 /* Clobbers %g1, loads local processor's IRQ work area into %g6.  */
 #define TRAP_LOAD_IRQ_WORK                      \
-        __GET_CPUID                             \
-        sethi   %hi(__irq_work), %g1;           \
-        sllx    %g6, 6, %g6;                    \
-        or      %g1, %lo(__irq_work), %g1;      \
-        add     %g1, %g6, %g6;
+        __GET_CPUID(%g1)                        \
+        sethi   %hi(__irq_work), %g6;           \
+        sllx    %g1, 6, %g1;                    \
+        or      %g6, %lo(__irq_work), %g6;      \
+        add     %g6, %g1, %g6;
 
 /* Clobbers %g1, loads %g6 with current thread info pointer.  */
 #define TRAP_LOAD_THREAD_REG                    \
-        __GET_CPUID                             \
-        sllx    %g6, TRAP_BLOCK_SZ_SHIFT, %g6;  \
-        sethi   %hi(trap_block), %g1;           \
-        or      %g1, %lo(trap_block), %g1;      \
-        ldx     [%g1 + %g6], %g6;
+        __GET_CPUID(%g1)                        \
+        sethi   %hi(trap_block), %g6;           \
+        sllx    %g1, TRAP_BLOCK_SZ_SHIFT, %g1;  \
+        or      %g6, %lo(trap_block), %g6;      \
+        ldx     [%g6 + %g1], %g6;
 
 /* Given the current thread info pointer in %g6, load the per-cpu
  * area base of the current processor into %g5.  REG1, REG2, and REG3 are
@@ -109,7 +141,6 @@ extern void setup_tba(void);
  * trap will load the fully resolved %g5 per-cpu base.  This can corrupt
  * the calculations done by the macro mid-stream.
  */
-#ifdef CONFIG_SMP
 #define LOAD_PER_CPU_BASE(REG1, REG2, REG3)             \
         ldub    [%g6 + TI_CPU], REG1;                   \
         sethi   %hi(__per_cpu_shift), REG3;             \
@@ -118,8 +149,26 @@ extern void setup_tba(void);
         ldx     [REG2 + %lo(__per_cpu_base)], REG2;     \
         sllx    REG1, REG3, REG3;                       \
         add     REG3, REG2, %g5;
+
 #else
+
+/* Uniprocessor versions, we know the cpuid is zero.  */
+#define TRAP_LOAD_PGD_PHYS                      \
+        sethi   %hi(trap_block), %g7;           \
+        or      %g7, %lo(trap_block), %g7;      \
+        ldx     [%g7 + TRAP_PER_CPU_PGD_PADDR], %g7;
+
+#define TRAP_LOAD_IRQ_WORK                      \
+        sethi   %hi(__irq_work), %g6;           \
+        or      %g6, %lo(__irq_work), %g6;
+
+#define TRAP_LOAD_THREAD_REG                    \
+        sethi   %hi(trap_block), %g6;           \
+        ldx     [%g6 + %lo(trap_block)], %g6;
+
+/* No per-cpu areas on uniprocessor, so no need to load %g5.  */
 #define LOAD_PER_CPU_BASE(REG1, REG2, REG3)
-#endif
+
+#endif /* !(CONFIG_SMP) */
 
 #endif /* _SPARC64_CPUDATA_H */