3 files changed, 95 insertions, 11 deletions

diff --git a/include/asm-sparc64/cpudata.h b/include/asm-sparc64/cpudata.h
index f7c0faede8b8..6c57cbb9a7d1 100644
--- a/include/asm-sparc64/cpudata.h
+++ b/include/asm-sparc64/cpudata.h
@@ -1,12 +1,15 @@
 /* cpudata.h: Per-cpu parameters.
  *
- * Copyright (C) 2003, 2005 David S. Miller (davem@redhat.com)
+ * Copyright (C) 2003, 2005, 2006 David S. Miller (davem@davemloft.net)
  */
 
 #ifndef _SPARC64_CPUDATA_H
 #define _SPARC64_CPUDATA_H
 
+#ifndef __ASSEMBLY__
+
 #include <linux/percpu.h>
+#include <linux/threads.h>
 
 typedef struct {
         /* Dcache line 1 */
@@ -32,4 +35,85 @@ DECLARE_PER_CPU(cpuinfo_sparc, __cpu_data);
 #define cpu_data(__cpu)         per_cpu(__cpu_data, (__cpu))
 #define local_cpu_data()        __get_cpu_var(__cpu_data)
 
+/* Trap handling code needs to get at a few critical values upon
+ * trap entry and to process TSB misses.  These cannot be in the
+ * per_cpu() area as we really need to lock them into the TLB and
+ * thus make them part of the main kernel image.  As a result we
+ * try to make this as small as possible.
+ *
+ * This is padded out and aligned to 64-bytes to avoid false sharing
+ * on SMP.
+ */
+
+/* If you modify the size of this structure, please update
+ * TRAP_BLOCK_SZ_SHIFT below.
+ */
+struct thread_info;
+struct trap_per_cpu {
+/* D-cache line 1 */
+        struct thread_info      *thread;
+        unsigned long           pgd_paddr;
+        unsigned long           __pad1[2];
+
+/* D-cache line 2 */
+        unsigned long           __pad2[4];
+} __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);
+
+#endif /* !(__ASSEMBLY__) */
+
+#define TRAP_PER_CPU_THREAD     0x00
+#define TRAP_PER_CPU_PGD_PADDR  0x08
+
+#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;
+
+/* Clobbers %g1, current address space PGD phys address into %g7.  */
+#define TRAP_LOAD_PGD_PHYS                      \
+        __GET_CPUID                             \
+        sllx    %g6, TRAP_BLOCK_SZ_SHIFT, %g6;  \
+        sethi   %hi(trap_block), %g7;           \
+        or      %g7, %lo(trap_block), %g7;      \
+        add     %g7, %g6, %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;
+
+/* 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;
+
+/* Given the current thread info pointer in %g6, load the per-cpu
+ * area base of the current processor into %g5.  REG1 and REG2 are
+ * clobbered.
+ */
+#ifdef CONFIG_SMP
+#define LOAD_PER_CPU_BASE(REG1, REG2)                   \
+        ldub    [%g6 + TI_CPU], REG1;                   \
+        sethi   %hi(__per_cpu_shift), %g5;              \
+        sethi   %hi(__per_cpu_base), REG2;              \
+        ldx     [%g5 + %lo(__per_cpu_shift)], %g5;      \
+        ldx     [REG2 + %lo(__per_cpu_base)], REG2;     \
+        sllx    REG1, %g5, %g5;                         \
+        add     %g5, REG2, %g5;
+#else
+#define LOAD_PER_CPU_BASE(REG1, REG2)
+#endif
+
 #endif /* _SPARC64_CPUDATA_H */
diff --git a/include/asm-sparc64/system.h b/include/asm-sparc64/system.h
index af254e581834..26c0807af3e4 100644
--- a/include/asm-sparc64/system.h
+++ b/include/asm-sparc64/system.h
@@ -209,6 +209,8 @@ do {	if (test_thread_flag(TIF_PERFCTR)) {				\
         /* so that ASI is only written if it changes, think again. */   \
         __asm__ __volatile__("wr %%g0, %0, %%asi"                       \
         : : "r" (__thread_flag_byte_ptr(task_thread_info(next))[TI_FLAG_BYTE_CURRENT_DS]));\
+        trap_block[current_thread_info()->cpu].thread =                 \
+                task_thread_info(next);                                 \
         __asm__ __volatile__(                                           \
         "mov    %%g4, %%g7\n\t"                                         \
         "wrpr   %%g0, 0x95, %%pstate\n\t"                               \
diff --git a/include/asm-sparc64/ttable.h b/include/asm-sparc64/ttable.h
index 2784f80094c3..f557db4faf84 100644
--- a/include/asm-sparc64/ttable.h
+++ b/include/asm-sparc64/ttable.h
@@ -109,14 +109,14 @@
         nop;nop;nop;
         
 #define TRAP_UTRAP(handler,lvl)                         \
-        ldx     [%g6 + TI_UTRAPS], %g1;                 \
-        sethi   %hi(109f), %g7;                         \
-        brz,pn  %g1, utrap;                             \
-         or     %g7, %lo(109f), %g7;                    \
-        ba,pt   %xcc, utrap;                            \
-109:     ldx    [%g1 + handler*8], %g1;                 \
-        ba,pt   %xcc, utrap_ill;                        \
-         mov    lvl, %o1;
+        mov     handler, %g3;                           \
+        ba,pt   %xcc, utrap_trap;                       \
+         mov    lvl, %g4;                               \
+        nop;                                            \
+        nop;                                            \
+        nop;                                            \
+        nop;                                            \
+        nop;
 
 #ifdef CONFIG_SUNOS_EMUL
 #define SUNOS_SYSCALL_TRAP SYSCALL_TRAP(linux_sparc_syscall32, sunos_sys_table)
@@ -136,8 +136,6 @@
 #else
 #define SOLARIS_SYSCALL_TRAP TRAP(solaris_syscall)
 #endif
-/* FIXME: Write these actually */       
-#define NETBSD_SYSCALL_TRAP TRAP(netbsd_syscall)
 #define BREAKPOINT_TRAP TRAP(breakpoint_trap)
 
 #define TRAP_IRQ(routine, level)                        \