Instead of using sysarch() to store-retrieve the tp, add a magic address,

ARM_TP_ADDRESS, where the tp will be stored. On CPUs that support it, a cache
line will be allocated and locked for this address, so that it will never go
to RAM. On CPUs that does not, a page is allocated for it (it will be a bit
slower, and is wrong for SMP, but should be fine for UP).
The tp is still stored in the mdthread struct, and at each context switch,
ARM_TP_ADDRESS gets updated.

Suggested by:   davidxu

10 files changed, 70 insertions, 4 deletions

diff --git a/sys/arm/arm/cpufunc_asm.S b/sys/arm/arm/cpufunc_asm.S
index af252a9..d9cc70d 100644
--- a/sys/arm/arm/cpufunc_asm.S
+++ b/sys/arm/arm/cpufunc_asm.S
@@ -153,3 +153,26 @@ ENTRY(get_pc_str_offset)
 	ldr	r0, [sp]
 	sub	r0, r0, r1
 	ldmdb	fp, {fp, sp, pc}
+
+/* Allocate and lock a cacheline for the specified address. */
+
+#define CPWAIT_BRANCH			\
+	sub	pc, pc, #4
+#define CPWAIT() \
+	mrc	p15, 0, r2, c2, c0, 0;	\
+	mov	r2, r2;			\
+	CPWAIT_BRANCH
+
+ENTRY(arm_lock_cache_line)
+	mcr	p15, 0, r0, c7, c10, 4 /* Drain write buffer */
+	mov	r1, #1
+	mcr	p15, 0, r1, c9, c2, 0 /* Enable data cache lock mode */
+	CPWAIT()
+	mcr	p15, 0, r0, c7, c2, 5 /* Allocate the cache line */
+	mcr	p15, 0, r0, c7, c10, 4 /* Drain write buffer */
+	mov	r1, #0
+	str	r1, [r0]
+	mcr	p15, 0, r0, c7, c10, 4 /* Drain write buffer */
+	mcr	p15, 0, r1, c9, c2, 0 /* Disable data cache lock mode */
+	CPWAIT()
+	RET
diff --git a/sys/arm/arm/genassym.c b/sys/arm/arm/genassym.c
index edd3d96..d0e8ced 100644
--- a/sys/arm/arm/genassym.c
+++ b/sys/arm/arm/genassym.c
@@ -44,6 +44,8 @@ __FBSDID("$FreeBSD$");
 #include <machine/pcb.h>
 #include <machine/pte.h>
 #include <machine/intr.h>
+#include <machine/sysarch.h>
+
 #include <netinet/in.h>
 #include <netinet/in_systm.h>
 #include <netinet/ip.h>
@@ -90,6 +92,8 @@ ASSYM(TD_PCB, offsetof(struct thread, td_pcb));
 ASSYM(TD_FLAGS, offsetof(struct thread, td_flags));
 ASSYM(TD_PROC, offsetof(struct thread, td_proc));
 ASSYM(TD_FRAME, offsetof(struct thread, td_frame));
+ASSYM(TD_MD, offsetof(struct thread, td_md));
+ASSYM(MD_TP, offsetof(struct mdthread, md_tp));
 
 ASSYM(TF_R0, offsetof(struct trapframe, tf_r0));
 ASSYM(TF_R1, offsetof(struct trapframe, tf_r1));
@@ -97,6 +101,7 @@ ASSYM(TF_PC, offsetof(struct trapframe, tf_pc));
 ASSYM(P_PID, offsetof(struct proc, p_pid));
 ASSYM(P_FLAG, offsetof(struct proc, p_flag));
 
+ASSYM(ARM_TP_ADDRESS, ARM_TP_ADDRESS);
 ASSYM(PDESIZE, PDESIZE);
 ASSYM(PMAP_DOMAIN_KERNEL, PMAP_DOMAIN_KERNEL);
 #ifdef PMAP_INCLUDE_PTE_SYNC
diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 782aef8..67b34e3 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -87,8 +87,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/trap.h>
 #include <machine/undefined.h>
 #include <machine/vmparam.h>
-
-#define MDROOT_ADDR 0xd0400000
+#include <machine/sysarch.h>
 
 uint32_t cpu_reset_address = 0;
 int cold = 1;
@@ -220,6 +219,9 @@ static void
 cpu_startup(void *dummy)
 {
 	struct pcb *pcb = thread0.td_pcb;
+#ifndef ARM_CACHE_LOCK_ENABLE
+	vm_page_t m;
+#endif
 
 	vm_ksubmap_init(&kmi);
 	bufinit();
@@ -233,6 +235,13 @@ cpu_startup(void *dummy)
 	cpu_setup("");
 	identify_arm_cpu();
 	thread0.td_frame = (struct trapframe *)pcb->un_32.pcb32_sp - 1;
+#ifdef ARM_CACHE_LOCK_ENABLE
+	pmap_kenter_user(ARM_TP_ADDRESS, ARM_TP_ADDRESS);
+	arm_lock_cache_line(ARM_TP_ADDRESS);
+#else
+	m = vm_page_alloc(NULL, 0, VM_ALLOC_NOOBJ | VM_ALLOC_ZERO);
+	pmap_kenter_user(ARM_TP_ADDRESS, VM_PAGE_TO_PHYS(m));
+#endif
 	
 }
 
diff --git a/sys/arm/arm/pmap.c b/sys/arm/arm/pmap.c
index 8c65847..76489a1 100644
--- a/sys/arm/arm/pmap.c
+++ b/sys/arm/arm/pmap.c
@@ -353,6 +353,7 @@ struct l2_dtable {
 
 /* pmap_kenter_internal flags */
 #define KENTER_CACHE	0x1
+#define KENTER_USER	0x2
 
 /*
  * Given an L1 table index, calculate the corresponding l2_dtable index
@@ -2818,6 +2819,8 @@ pmap_kenter_internal(vm_offset_t va, vm_offset_t pa, int flags)
 	    VM_PROT_READ | VM_PROT_WRITE);
 	if (flags & KENTER_CACHE)
 		*pte |= pte_l2_s_cache_mode;
+	if (flags & KENTER_USER)
+		*pte |= L2_S_PROT_U;
 	PTE_SYNC(pte);
 }
 
@@ -2827,7 +2830,11 @@ pmap_kenter(vm_offset_t va, vm_paddr_t pa)
 	pmap_kenter_internal(va, pa, KENTER_CACHE);
 }
 
-
+void
+pmap_kenter_user(vm_offset_t va, vm_paddr_t pa)
+{
+	pmap_kenter_internal(va, pa, KENTER_CACHE|KENTER_USER);
+}
 
 /*
  * remove a page rom the kernel pagetables
diff --git a/sys/arm/arm/swtch.S b/sys/arm/arm/swtch.S
index 2bf7889..a52298b 100644
--- a/sys/arm/arm/swtch.S
+++ b/sys/arm/arm/swtch.S
@@ -201,6 +201,12 @@ ENTRY(cpu_throw)
 	ldr	r6, .Lcurthread
 	str	r5, [r6]
 
+	/* Set the new tp */
+	ldr	r6, [r5, #(TD_MD)]
+	add	r6, r6, #(MD_TP)
+	mov	r5, #ARM_TP_ADDRESS
+	str	r6, [r5]
+
 	/* Hook in a new pcb */
 	ldr	r6, .Lcurpcb
 	str	r7, [r6]
@@ -253,7 +259,15 @@ ENTRY(cpu_switch)
 	 * NOTE: We can now use r8-r13 until it is time to restore
 	 * them for the new process.
 	 */
-
+	/* Store the old tp */
+	mov	r3, #ARM_TP_ADDRESS
+	ldr	r9, [r3]
+	str	r9, [r0, #(TD_MD + MD_TP)]
+
+	/* Set the new tp */
+	ldr	r9, [r1, #(TD_MD + MD_TP)]
+	mov	r3, #ARM_TP_ADDRESS
+	str	r9, [r3]
 
 	/* Get the user structure for the new process in r9 */
 	ldr	r9, [r1, #(TD_PCB)]
diff --git a/sys/arm/include/machdep.h b/sys/arm/include/machdep.h
index 4d201cc..b26f386 100644
--- a/sys/arm/include/machdep.h
+++ b/sys/arm/include/machdep.h
@@ -10,4 +10,6 @@ void data_abort_handler (trapframe_t *);
 void prefetch_abort_handler (trapframe_t *);
 void undefinedinstruction_bounce (trapframe_t *);
 
+void arm_lock_cache_line(vm_offset_t);
+
 #endif /* !_MACHINE_MACHDEP_H_ */
diff --git a/sys/arm/include/pmap.h b/sys/arm/include/pmap.h
index cc4eeda..aa87c08 100644
--- a/sys/arm/include/pmap.h
+++ b/sys/arm/include/pmap.h
@@ -208,6 +208,7 @@ extern vm_offset_t virtual_end;
 
 void	pmap_bootstrap(vm_offset_t, vm_offset_t, struct pv_addr *);
 void	pmap_kenter(vm_offset_t va, vm_paddr_t pa);
+void 	pmap_kenter_user(vm_offset_t va, vm_paddr_t pa);
 void	pmap_kremove(vm_offset_t);
 void	*pmap_mapdev(vm_offset_t, vm_size_t);
 void	pmap_unmapdev(vm_offset_t, vm_size_t);
diff --git a/sys/arm/include/sysarch.h b/sys/arm/include/sysarch.h
index d0c0c47..4d6234b 100644
--- a/sys/arm/include/sysarch.h
+++ b/sys/arm/include/sysarch.h
@@ -53,6 +53,8 @@
 #define ARM_SET_TP		2
 #define ARM_GET_TP		3
 
+#define ARM_TP_ADDRESS		0xe0000000 /* Magic */
+
 struct arm_sync_icache_args {
 	uintptr_t	addr;		/* Virtual start address */
 	size_t		len;		/* Region size */
diff --git a/sys/arm/xscale/i80321/std.i80321 b/sys/arm/xscale/i80321/std.i80321
index 5a6e34b..3d8b8f9 100644
--- a/sys/arm/xscale/i80321/std.i80321
+++ b/sys/arm/xscale/i80321/std.i80321
@@ -1,4 +1,5 @@
 #XScale i80321 generic configuration
 #$FreeBSD$
 files		"../xscale/i80321/files.i80321"
+include		"../xscale/std.xscale"
 cpu 		CPU_XSCALE_80321
diff --git a/sys/arm/xscale/std.xscale b/sys/arm/xscale/std.xscale
new file mode 100644
index 0000000..48c588f
--- /dev/null
+++ b/sys/arm/xscale/std.xscale
@@ -0,0 +1,2 @@
+# $FreeBSD$
+options		ARM_CACHE_LOCK_ENABLE