Change the way pcpu and curthread are stored per-core:

the old way was to store pcpu in a register, and get curthread from pcpu,
which is not very atomic, and led to issues if the thread was migrated
to another core between the time we got the pcpu address and the time we
got curthread.
Instead, we now store curthread where pcpu used to be store, and we
calculate the pcpu address based on the cpu id.

11 files changed, 69 insertions, 37 deletions

diff --git a/sys/arm/arm/bcopyinout.S b/sys/arm/arm/bcopyinout.S
index 68fdf20..ce6a430 100644
--- a/sys/arm/arm/bcopyinout.S
+++ b/sys/arm/arm/bcopyinout.S
@@ -57,7 +57,7 @@ __FBSDID("$FreeBSD$");
 #ifdef _ARM_ARCH_6
 #define GET_PCB(tmp) \
 	mrc p15, 0, tmp, c13, c0, 4; \
-	add	tmp, tmp, #(PC_CURPCB)
+	add	tmp, tmp, #(TD_PCB)
 #else
 .Lcurpcb:
 	.word	_C_LABEL(__pcpu) + PC_CURPCB
diff --git a/sys/arm/arm/bcopyinout_xscale.S b/sys/arm/arm/bcopyinout_xscale.S
index 2cb98d9..45cfe70 100644
--- a/sys/arm/arm/bcopyinout_xscale.S
+++ b/sys/arm/arm/bcopyinout_xscale.S
@@ -44,7 +44,7 @@ __FBSDID("$FreeBSD$");
 #ifdef _ARM_ARCH_6
 #define GET_PCB(tmp) \
 	mrc p15, 0, tmp, c13, c0, 4; \
-	add	tmp, tmp, #(PC_CURPCB)
+	add	tmp, tmp, #(TD_PCB)
 #else
 .Lcurpcb:
 	.word	_C_LABEL(__pcpu) + PC_CURPCB
diff --git a/sys/arm/arm/copystr.S b/sys/arm/arm/copystr.S
index 83b7ec7..7cd3571 100644
--- a/sys/arm/arm/copystr.S
+++ b/sys/arm/arm/copystr.S
@@ -53,7 +53,7 @@ __FBSDID("$FreeBSD$");
 #ifdef _ARM_ARCH_6
 #define GET_PCB(tmp) \
 	mrc p15, 0, tmp, c13, c0, 4; \
-	add	tmp, tmp, #(PC_CURPCB)
+	add	tmp, tmp, #(TD_PCB)
 #else
 .Lpcb:
 	.word	_C_LABEL(__pcpu) + PC_CURPCB
diff --git a/sys/arm/arm/fusu.S b/sys/arm/arm/fusu.S
index f298bc4..8561295 100644
--- a/sys/arm/arm/fusu.S
+++ b/sys/arm/arm/fusu.S
@@ -42,7 +42,7 @@ __FBSDID("$FreeBSD$");
 #ifdef _ARM_ARCH_6
 #define GET_PCB(tmp) \
 	mrc p15, 0, tmp, c13, c0, 4; \
-	add	tmp, tmp, #(PC_CURPCB)
+	add	tmp, tmp, #(TD_PCB)
 #else
 .Lcurpcb:
 	.word	_C_LABEL(__pcpu) + PC_CURPCB
diff --git a/sys/arm/arm/genassym.c b/sys/arm/arm/genassym.c
index 8ddb05e..029529a 100644
--- a/sys/arm/arm/genassym.c
+++ b/sys/arm/arm/genassym.c
@@ -140,3 +140,4 @@ ASSYM(TRAPFRAMESIZE, sizeof(struct trapframe));
 
 ASSYM(MAXCOMLEN, MAXCOMLEN);
 ASSYM(NIRQ, NIRQ);
+ASSYM(PCPU_SIZE, sizeof(struct pcpu));
diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 1a85d60..e4b755e 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -874,7 +874,7 @@ void
 pcpu0_init(void)
 {
 #if ARM_ARCH_6 || ARM_ARCH_7A || defined(CPU_MV_PJ4B)
-	set_pcpu(pcpup);
+	set_curthread(&thread0);
 #endif
 	pcpu_init(pcpup, 0, sizeof(struct pcpu));
 	PCPU_SET(curthread, &thread0);
diff --git a/sys/arm/arm/mp_machdep.c b/sys/arm/arm/mp_machdep.c
index c62e311..e8e6eee 100644
--- a/sys/arm/arm/mp_machdep.c
+++ b/sys/arm/arm/mp_machdep.c
@@ -177,7 +177,6 @@ init_secondary(int cpu)
 	cpu_tlb_flushID();
 
 	pc = &__pcpu[cpu];
-	set_pcpu(pc);
 
 	/*
 	 * pcpu_init() updates queue, so it should not be executed in parallel
@@ -203,6 +202,7 @@ init_secondary(int cpu)
 	KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread"));
 	pc->pc_curthread = pc->pc_idlethread;
 	pc->pc_curpcb = pc->pc_idlethread->td_pcb;
+	set_curthread(pc->pc_idlethread);
 #ifdef VFP
 	pc->pc_cpu = cpu;
 
diff --git a/sys/arm/arm/swtch.S b/sys/arm/arm/swtch.S
index bff20fb..f1371a1 100644
--- a/sys/arm/arm/swtch.S
+++ b/sys/arm/arm/swtch.S
@@ -89,16 +89,22 @@ __FBSDID("$FreeBSD$");
 #define DOMAIN_CLIENT	0x01
 
 #ifdef _ARM_ARCH_6
-#define GET_PCPU(tmp) \
-	mrc p15, 0, tmp, c13, c0, 4;
+#define GET_PCPU(tmp, tmp2) \
+	mrc 	p15, 0, tmp, c0, c0, 5;	\
+	and	tmp, tmp, #0xf;		\
+	ldr 	tmp2, .Lcurpcpu+4;	\
+	mul 	tmp, tmp, tmp2;		\
+	ldr	tmp2, .Lcurpcpu;	\
+	add	tmp, tmp, tmp2;
 #else
-.Lcurpcpu:
-	.word	_C_LABEL(__pcpu)
 
-#define GET_PCPU(tmp) \
+#define GET_PCPU(tmp, tmp2) \
 	ldr	tmp, .Lcurpcpu
 #endif
 
+.Lcurpcpu:
+        .word   _C_LABEL(__pcpu)
+	.word	PCPU_SIZE
 .Lcpufuncs:	
 	.word	_C_LABEL(cpufuncs)
 .Lblocked_lock:
@@ -112,7 +118,7 @@ ENTRY(cpu_throw)
 	 * r5 = newtd
 	 */
 
-	GET_PCPU(r7)
+	GET_PCPU(r7, r9)
 
 #ifdef VFP
 	/*
@@ -191,10 +197,15 @@ ENTRY(cpu_throw)
 	ldr	r13, [r7, #(PCB_SP)]
 #endif
 
+	GET_PCPU(r6, r4)
+	/* Hook in a new pcb */
+	str	r7, [r6, #PC_CURPCB]
 	/* We have a new curthread now so make a note it */
-	GET_CURTHREAD_PTR(r6)
+	add	r6, r6, #PC_CURTHREAD
 	str	r5, [r6]
-
+#ifndef ARM_TP_ADDRESS
+	mcr	p15, 0, r5, c13, c0, 4
+#endif
 	/* Set the new tp */
 	ldr	r6, [r5, #(TD_MD + MD_TP)]
 #ifdef ARM_TP_ADDRESS
@@ -207,9 +218,6 @@ ENTRY(cpu_throw)
 #else
 	mcr p15, 0, r6, c13, c0, 3
 #endif
-	/* Hook in a new pcb */
-	GET_PCPU(r6)
-	str	r7, [r6, #PC_CURPCB]
 
 	add	sp, sp, #4;
 	ldmfd	sp!, {r4-r7, pc}
@@ -231,11 +239,14 @@ ENTRY(cpu_switch)
 
 	/* Process is now on a processor. */
 	/* We have a new curthread now so make a note it */
-	GET_CURTHREAD_PTR(r7)
+	GET_PCPU(r7, r2)
+	add	r7, r7, #PC_CURTHREAD
 	str	r1, [r7]
+#ifndef ARM_TP_ADDRESS
+	mcr	p15, 0, r1, c13, c0, 4
+#endif
 
 	/* Hook in a new pcb */
-	GET_PCPU(r7)
 	ldr	r2, [r1, #TD_PCB]
 	str	r2, [r7, #PC_CURPCB]
 
@@ -315,7 +326,7 @@ ENTRY(cpu_switch)
 	 * a future exception will bounce the backup settings in the fp unit.
 	 * XXX vfp_store can't change r4
 	 */
-	GET_PCPU(r7)
+	GET_PCPU(r7, r8)
 	ldr	r8, [r7, #(PC_VFPCTHREAD)]
 	cmp	r4, r8				/* old thread used vfp? */
 	bne	1f				/* no, don't save */
@@ -440,7 +451,6 @@ ENTRY(cpu_switch)
 #if defined(SCHED_ULE) && defined(SMP)
 	ldr	r6, .Lblocked_lock
 	GET_CURTHREAD_PTR(r3)
-
 1:
 	ldr	r4, [r3, #TD_LOCK]
 	cmp	r4, r6
@@ -516,7 +526,7 @@ ENTRY(savectx)
 	 * registers and state, and modify the control as needed.
 	 * a future exception will bounce the backup settings in the fp unit.
 	 */
-	GET_PCPU(r7)
+	GET_PCPU(r7, r4)
 	ldr	r4, [r7, #(PC_VFPCTHREAD)]      /* vfp thread */
 	ldr	r2, [r7, #(PC_CURTHREAD)]       /* current thread */
 	cmp	r4, r2
diff --git a/sys/arm/arm/vfp.c b/sys/arm/arm/vfp.c
index b78371a..e6c02bc 100644
--- a/sys/arm/arm/vfp.c
+++ b/sys/arm/arm/vfp.c
@@ -140,9 +140,11 @@ vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 	u_int fpexc;
 	struct pcb *curpcb;
 	struct thread *vfptd;
+	int i;
 
 	if (!vfp_exists)
 		return 1;		/* vfp does not exist */
+	i = disable_interrupts(I32_bit|F32_bit);
 	fpexc = fmrx(VFPEXC);		/* read the vfp exception reg */
 	if (fpexc & VFPEXC_EN) {
 		vfptd = PCPU_GET(vfpcthread);
@@ -164,6 +166,7 @@ vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 		fmxr(VFPEXC, fpexc);	/* turn vfp hardware off */
 		if (vfptd == curthread) {
 			/* kill the process - we do not handle emulation */
+			restore_interrupts(i);
 			killproc(curthread->td_proc, "vfp emulation");
 			return 1;
 		}
@@ -173,7 +176,7 @@ vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 	}
 	fpexc |= VFPEXC_EN;
 	fmxr(VFPEXC, fpexc);	/* enable the vfp and repeat command */
-	curpcb = PCPU_GET(curpcb);
+	curpcb = curthread->td_pcb;
 	/* If we were the last process to use the VFP, the process did not
 	 * use a VFP on another processor, then the registers in the VFP
 	 * will still be ours and are current. Eventually, we will make the
@@ -183,7 +186,8 @@ vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 #ifdef SMP
 	curpcb->pcb_vfpcpu = PCPU_GET(cpu);
 #endif
-	PCPU_SET(vfpcthread, PCPU_GET(curthread));
+	PCPU_SET(vfpcthread, curthread);
+	restore_interrupts(i);
 	return 0;
 }
 
@@ -218,7 +222,6 @@ vfp_restore(struct vfp_state *vfpsave)
 			"ldr	%0, [%1]\n"		/* set old vfpscr */
 			"mcr	p10, 7, %0, cr1, c0, 0\n"
 			: "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc");
-		PCPU_SET(vfpcthread, PCPU_GET(curthread));
 	}
 }
 
@@ -237,7 +240,7 @@ vfp_store(struct vfp_state *vfpsave)
 	u_int tmp, vfpscr = 0;
 
 	tmp = fmrx(VFPEXC);		/* Is the vfp enabled? */
-	if (vfpsave && tmp & VFPEXC_EN) {
+	if (vfpsave && (tmp & VFPEXC_EN)) {
 		__asm __volatile("stc	p11, c0, [%1], #128\n" /* d0-d15 */
 			"cmp	%2, #0\n"		/* -D16 or -D32? */
 			stclne"	p11, c0, [%1], #128\n"	/* d16-d31 */
@@ -265,6 +268,12 @@ vfp_discard()
 {
 	u_int tmp = 0;
 
+	/*
+	 * No need to protect the access to vfpcthread by disabling
+	 * interrupts, since it's called from cpu_throw(), who is called
+	 * with interrupts disabled.
+	 */
+	  
 	PCPU_SET(vfpcthread, 0);	/* permanent forget about reg */
 	tmp = fmrx(VFPEXC);
 	tmp &= ~VFPEXC_EN;		/* turn off VFP hardware */
diff --git a/sys/arm/include/asmacros.h b/sys/arm/include/asmacros.h
index ff92cc6..e688689 100644
--- a/sys/arm/include/asmacros.h
+++ b/sys/arm/include/asmacros.h
@@ -241,15 +241,15 @@ name:
 #ifdef _ARM_ARCH_6
 #define	AST_LOCALS
 #define GET_CURTHREAD_PTR(tmp) \
-	mrc p15, 0, tmp, c13, c0, 4; \
-	add	tmp, tmp, #(PC_CURTHREAD)
+    	mrc	p15, 0, tmp, c13, c0, 4
 #else
 #define	AST_LOCALS							;\
 .Lcurthread:								;\
 	.word	_C_LABEL(__pcpu) + PC_CURTHREAD
 
 #define GET_CURTHREAD_PTR(tmp) \
-	ldr	tmp, .Lcurthread
+	ldr	tmp, .Lcurthread     \
+	ldr	tmp, [tmp]
 #endif
 
 #define	DO_AST								\
@@ -262,7 +262,6 @@ name:
 	bne	2f			/* Nope, get out now */		;\
 	bic	r4, r4, #(I32_bit|F32_bit)				;\
 1:	GET_CURTHREAD_PTR(r5)						;\
-	ldr	r5, [r5]						;\
 	ldr	r1, [r5, #(TD_FLAGS)]					;\
 	and	r1, r1, #(TDF_ASTPENDING|TDF_NEEDRESCHED)		;\
 	teq	r1, #0x00000000						;\
diff --git a/sys/arm/include/pcpu.h b/sys/arm/include/pcpu.h
index 6e0eb06..94ce6b9 100644
--- a/sys/arm/include/pcpu.h
+++ b/sys/arm/include/pcpu.h
@@ -62,22 +62,32 @@ struct pcpu;
 extern struct pcpu *pcpup;
 #if ARM_ARCH_6 || ARM_ARCH_7A
 /* or ARM_TP_ADDRESS 	mark REMOVE ME NOTE */
-static inline struct pcpu *
-get_pcpu(void)
+
+#define CPU_MASK (0xf)
+
+#define get_pcpu() __extension__ ({			  		\
+    	int id;								\
+        __asm __volatile("mrc p15, 0, %0, c0, c0, 5" : "=r" (id));	\
+    	(pcpup + (id & CPU_MASK));					\
+    })
+	
+static inline struct thread *
+get_curthread(void)
 {
-	void *pcpu;
+	void *ret;
 
-	__asm __volatile("mrc p15, 0, %0, c13, c0, 4" : "=r" (pcpu));
-	return (pcpu);
+	__asm __volatile("mrc p15, 0, %0, c13, c0, 4" : "=r" (ret));
+	return (ret);
 }
 
 static inline void
-set_pcpu(void *pcpu)
+set_curthread(struct thread *td)
 {
 
-	__asm __volatile("mcr p15, 0, %0, c13, c0, 4" : : "r" (pcpu));
+	__asm __volatile("mcr p15, 0, %0, c13, c0, 4" : : "r" (td));
 }
 
+
 static inline void *
 get_tls(void)
 {
@@ -93,6 +103,9 @@ set_tls(void *tls)
 
 	__asm __volatile("mcr p15, 0, %0, c13, c0, 3" : : "r" (tls));
 }
+
+#define curthread get_curthread()
+
 #else
 #define get_pcpu()	pcpup
 #endif