[mips] add support for using the MIPS user register for TLS data.

This work, originally from Stacey Son, uses the MIPS UserReg for
reading the TLS data, and will fall back to the normal syscall path
when it isn't supported.

This code dynamically patches cpu_switch() to bypass the UserReg
instruction so to avoid generating a machine exception.

Thanks to sson for the original work, and to Dan Nelson for
bringing it to date and testing it on MIPS32 with me.

Tested:

* mips64 (sson)
* mips74k (dnelson_1901@yahoo.com) - AR9344 SoC, UserReg support
* mips24k (adrian) - AR9331 SoC, no UserReg support

Obtained from:	sson, dnelson_1901@yahoo.com

10 files changed, 289 insertions, 12 deletions

diff --git a/lib/libthr/arch/mips/include/pthread_md.h b/lib/libthr/arch/mips/include/pthread_md.h
index 32af964c50a5..47c178b02a40 100644
--- a/lib/libthr/arch/mips/include/pthread_md.h
+++ b/lib/libthr/arch/mips/include/pthread_md.h
@@ -61,6 +61,7 @@ _tcb_set(struct tcb *tcb)
 /*
  * Get the current tcb.
  */
+#ifdef TLS_USE_SYSARCH
 static __inline struct tcb *
 _tcb_get(void)
 {
@@ -70,6 +71,55 @@ _tcb_get(void)
 	return tcb;
 }
 
+#else /* ! TLS_USE_SYSARCH */
+
+#  if defined(__mips_n64)
+static __inline struct tcb *
+_tcb_get(void)
+{
+	uint64_t _rv;
+
+	__asm__ __volatile__ (
+	    ".set\tpush\n\t"
+	    ".set\tmips64r2\n\t"
+	    "rdhwr\t%0, $29\n\t"
+	    ".set\tpop"
+	    : "=v" (_rv));
+
+	/*
+	 * XXXSS See 'git show c6be4f4d2d1b71c04de5d3bbb6933ce2dbcdb317'
+	 *
+	 * Remove the offset since this really a request to get the TLS
+	 * pointer via sysarch() (in theory).  Of course, this may go away
+	 * once the TLS code is rewritten.
+	 */
+	return (struct tcb *)(_rv - TLS_TP_OFFSET - TLS_TCB_SIZE);
+}
+#  else /* mips 32 */
+static __inline struct tcb *
+_tcb_get(void)
+{
+	uint32_t _rv;
+
+	__asm__ __volatile__ (
+	    ".set\tpush\n\t"
+	    ".set\tmips32r2\n\t"
+	    "rdhwr\t%0, $29\n\t"
+	    ".set\tpop"
+	    : "=v" (_rv));
+
+	/*
+	 * XXXSS See 'git show c6be4f4d2d1b71c04de5d3bbb6933ce2dbcdb317'
+	 *
+	 * Remove the offset since this really a request to get the TLS
+	 * pointer via sysarch() (in theory).  Of course, this may go away
+	 * once the TLS code is rewritten.
+	 */
+	return (struct tcb *)(_rv - TLS_TP_OFFSET - TLS_TCB_SIZE);
+}
+#  endif /* ! __mips_n64 */
+#endif /* ! TLS_USE_SYSARCH */
+
 extern struct pthread *_thr_initial;
 
 static __inline struct pthread *
diff --git a/libexec/rtld-elf/mips/reloc.c b/libexec/rtld-elf/mips/reloc.c
index d611c73f1fb3..bd75716b270c 100644
--- a/libexec/rtld-elf/mips/reloc.c
+++ b/libexec/rtld-elf/mips/reloc.c
@@ -634,13 +634,67 @@ allocate_initial_tls(Obj_Entry *objs)
 	sysarch(MIPS_SET_TLS, tls);
 }
 
+#ifdef __mips_n64
+void *
+_mips_get_tls(void)
+{
+	uint64_t _rv;
+
+	__asm__ __volatile__ (
+	    ".set\tpush\n\t"
+	    ".set\tmips64r2\n\t"
+	    "rdhwr\t%0, $29\n\t"
+	    ".set\tpop"
+	    : "=v" (_rv));
+	/*
+	 * XXXSS See 'git show c6be4f4d2d1b71c04de5d3bbb6933ce2dbcdb317'
+	 *
+	 * Remove the offset since this really a request to get the TLS
+	 * pointer via sysarch() (in theory).  Of course, this may go away
+	 * once the TLS code is rewritten.
+	 */
+	_rv = _rv - TLS_TP_OFFSET - TLS_TCB_SIZE;
+
+	return (void *)_rv;
+}
+
+#else /* mips 32 */
+
+void *
+_mips_get_tls(void)
+{
+	uint32_t _rv;
+
+	__asm__ __volatile__ (
+	    ".set\tpush\n\t"
+	    ".set\tmips32r2\n\t"
+	    "rdhwr\t%0, $29\n\t"
+	    ".set\tpop"
+	    : "=v" (_rv));
+	/*
+	 * XXXSS See 'git show c6be4f4d2d1b71c04de5d3bbb6933ce2dbcdb317'
+	 *
+	 * Remove the offset since this really a request to get the TLS
+	 * pointer via sysarch() (in theory).  Of course, this may go away
+	 * once the TLS code is rewritten.
+	 */
+	_rv = _rv - TLS_TP_OFFSET - TLS_TCB_SIZE;
+
+	return (void *)_rv;
+}
+#endif /* ! __mips_n64 */
+
 void *
 __tls_get_addr(tls_index* ti)
 {
 	Elf_Addr** tls;
 	char *p;
 
+#ifdef TLS_USE_SYSARCH
 	sysarch(MIPS_GET_TLS, &tls);
+#else
+	tls = _mips_get_tls();
+#endif
 
 	p = tls_get_addr_common(tls, ti->ti_module, ti->ti_offset + TLS_DTP_OFFSET);
 
diff --git a/sys/mips/include/cpufunc.h b/sys/mips/include/cpufunc.h
index 3ebb8c14b448..427aba74cd3a 100644
--- a/sys/mips/include/cpufunc.h
+++ b/sys/mips/include/cpufunc.h
@@ -159,6 +159,7 @@ mips_wr_ ## n(uint64_t a0)					\
 MIPS_RW64_COP0(excpc, MIPS_COP_0_EXC_PC);
 MIPS_RW64_COP0(entryhi, MIPS_COP_0_TLB_HI);
 MIPS_RW64_COP0(pagemask, MIPS_COP_0_TLB_PG_MASK);
+MIPS_RW64_COP0_SEL(userlocal, MIPS_COP_0_USERLOCAL, 2);
 #ifdef CPU_CNMIPS
 MIPS_RW64_COP0_SEL(cvmcount, MIPS_COP_0_COUNT, 6);
 MIPS_RW64_COP0_SEL(cvmctl, MIPS_COP_0_COUNT, 7);
@@ -265,6 +266,7 @@ MIPS_RW32_COP0_SEL(cmgcrbase, 15, 3);
 #if !defined(__mips_n64)
 MIPS_RW32_COP0(entryhi, MIPS_COP_0_TLB_HI);
 MIPS_RW32_COP0(pagemask, MIPS_COP_0_TLB_PG_MASK);
+MIPS_RW32_COP0_SEL(userlocal, MIPS_COP_0_USERLOCAL, 2);
 #endif
 #ifdef CPU_NLM
 MIPS_RW32_COP0_SEL(pagegrain, MIPS_COP_0_TLB_PG_MASK, 1);
@@ -289,6 +291,7 @@ MIPS_RW32_COP0_SEL(perfcnt0, MIPS_COP_0_PERFCNT, 0);
 MIPS_RW32_COP0_SEL(perfcnt1, MIPS_COP_0_PERFCNT, 1);
 MIPS_RW32_COP0_SEL(perfcnt2, MIPS_COP_0_PERFCNT, 2);
 MIPS_RW32_COP0_SEL(perfcnt3, MIPS_COP_0_PERFCNT, 3);
+MIPS_RW32_COP0(hwrena, MIPS_COP_0_HWRENA);
 
 #undef	MIPS_RW32_COP0
 #undef	MIPS_RW32_COP0_SEL
diff --git a/sys/mips/include/cpuinfo.h b/sys/mips/include/cpuinfo.h
index deeb93bd6d28..ec746140b2d0 100644
--- a/sys/mips/include/cpuinfo.h
+++ b/sys/mips/include/cpuinfo.h
@@ -58,6 +58,7 @@ struct mips_cpuinfo {
 	u_int16_t	tlb_nentries;
 	u_int8_t	icache_virtual;
 	boolean_t	cache_coherent_dma;
+	boolean_t	userlocal_reg;
 	struct {
 		u_int32_t	ic_size;
 		u_int8_t	ic_linesize;
diff --git a/sys/mips/include/cpuregs.h b/sys/mips/include/cpuregs.h
index a1d9bc0195da..4ce7e1b4fb2a 100644
--- a/sys/mips/include/cpuregs.h
+++ b/sys/mips/include/cpuregs.h
@@ -454,9 +454,10 @@
  *  2	MIPS_COP_0_TLB_LO0	.636 r4k TLB entry low.
  *  3	MIPS_COP_0_TLB_LO1	.636 r4k TLB entry low, extended.
  *  4	MIPS_COP_0_TLB_CONTEXT	3636 TLB Context.
+ *  4/2	MIPS_COP_0_USERLOCAL	..36 UserLocal.
  *  5	MIPS_COP_0_TLB_PG_MASK	.333 TLB Page Mask register.
  *  6	MIPS_COP_0_TLB_WIRED	.333 Wired TLB number.
- *  7	MIPS_COP_0_INFO		..33 Info registers
+ *  7	MIPS_COP_0_HWRENA	..33 rdHWR Enable.
  *  8	MIPS_COP_0_BAD_VADDR	3636 Bad virtual address.
  *  9	MIPS_COP_0_COUNT	.333 Count register.
  * 10	MIPS_COP_0_TLB_HI	3636 TLB entry high.
@@ -534,7 +535,8 @@
 #define	MIPS_COP_0_ERROR_PC	_(30)
 
 /* MIPS32/64 */
-#define	MIPS_COP_0_INFO		_(7)
+#define	MIPS_COP_0_USERLOCAL	_(4)	/* sel 2 is userlevel register */
+#define	MIPS_COP_0_HWRENA	_(7)
 #define	MIPS_COP_0_DEBUG	_(23)
 #define	MIPS_COP_0_DEPC		_(24)
 #define	MIPS_COP_0_PERFCNT	_(25)
@@ -548,11 +550,21 @@
 #define MIPS_MMU_BAT			0x02		/* Standard BAT */
 #define MIPS_MMU_FIXED			0x03		/* Standard fixed mapping */
 
-#define MIPS_CONFIG0_MT_MASK		0x00000380	/* bits 9..7 MMU Type */
-#define MIPS_CONFIG0_MT_SHIFT		7
-#define MIPS_CONFIG0_BE			0x00008000	/* data is big-endian */
-#define MIPS_CONFIG0_VI			0x00000008	/* instruction cache is virtual */
-
+/*
+ * Config Register Fields
+ * (See "MIPS Architecture for Programmers Volume III", MD00091, Table 9.39)
+ */
+#define	MIPS_CONFIG0_M		0x80000000 	/* Flag: Config1 is present. */
+#define	MIPS_CONFIG0_MT_MASK	0x00000380	/* bits 9..7 MMU Type */
+#define	MIPS_CONFIG0_MT_SHIFT	7
+#define	MIPS_CONFIG0_BE		0x00008000	/* data is big-endian */
+#define	MIPS_CONFIG0_VI		0x00000008	/* inst cache is virtual */
+ 
+/*
+ * Config1 Register Fields
+ * (See "MIPS Architecture for Programmers Volume III", MD00091, Table 9-1)
+ */
+#define	MIPS_CONFIG1_M		0x80000000	/* Flag: Config2 is present. */
 #define MIPS_CONFIG1_TLBSZ_MASK		0x7E000000	/* bits 30..25 # tlb entries minus one */
 #define MIPS_CONFIG1_TLBSZ_SHIFT	25
 
@@ -586,6 +598,19 @@
 
 #define MIPS_CONFIG3_CMGCR_MASK		(1 << 29)	/* Coherence manager present */
 
+/*
+ * Config2 Register Fields
+ * (See "MIPS Architecture for Programmers Volume III", MD00091, Table 9.40)
+ */
+#define	MIPS_CONFIG2_M		0x80000000	/* Flag: Config3 is present. */
+
+/*
+ * Config3 Register Fields
+ * (See "MIPS Architecture for Programmers Volume III", MD00091, Table 9.41)
+ */
+#define	MIPS_CONFIG3_M		0x80000000	/* Flag: Config4 is present */
+#define	MIPS_CONFIG3_ULR	0x00002000	/* UserLocal reg implemented */
+
 #define MIPS_CONFIG4_MMUSIZEEXT		0x000000FF	/* bits 7.. 0 MMU Size Extension */
 #define MIPS_CONFIG4_MMUEXTDEF		0x0000C000	/* bits 15.14 MMU Extension Definition */
 #define MIPS_CONFIG4_MMUEXTDEF_MMUSIZEEXT	0x00004000 /* This values denotes CONFIG4 bits  */
@@ -667,4 +692,15 @@
 #define	MIPS_CMGCRB_BASE	11
 #define	MIPS_CMGCRF_BASE	(~((1 << MIPS_CMGCRB_BASE) - 1))
 
+/*
+ * Bits defined for for the HWREna (CP0 register 7, select 0).
+ */
+#define	MIPS_HWRENA_CPUNUM	(1<<0)	/* CPU number program is running on */
+#define	MIPS_HWRENA_SYNCI_STEP 	(1<<1)	/* Address step sized used with SYNCI */
+#define	MIPS_HWRENA_CC		(1<<2)	/* Hi Res cycle counter */
+#define	MIPS_HWRENA_CCRES	(1<<3)	/* Cycle counter resolution */
+#define	MIPS_HWRENA_UL		(1<<29)	/* UserLocal Register */
+#define	MIPS_HWRENA_IMPL30	(1<<30)	/* Implementation-dependent 30 */
+#define	MIPS_HWRENA_IMPL31	(1<<31)	/* Implementation-dependent 31 */
+
 #endif /* _MIPS_CPUREGS_H_ */
diff --git a/sys/mips/mips/cpu.c b/sys/mips/mips/cpu.c
index 60fe134d9ccf..c43ba08c61b8 100644
--- a/sys/mips/mips/cpu.c
+++ b/sys/mips/mips/cpu.c
@@ -30,6 +30,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/module.h>
+#include <sys/proc.h>
 #include <sys/stdint.h>
 
 #include <sys/bus.h>
@@ -49,6 +50,9 @@ __FBSDID("$FreeBSD$");
 #include <machine/pte.h>
 #include <machine/tlb.h>
 #include <machine/hwfunc.h>
+#include <machine/mips_opcode.h>
+#include <machine/regnum.h>
+#include <machine/tls.h>
 
 #if defined(CPU_CNMIPS)
 #include <contrib/octeon-sdk/cvmx.h>
@@ -59,6 +63,63 @@ static void cpu_identify(void);
 
 struct mips_cpuinfo cpuinfo;
 
+#define _ENCODE_INSN(a,b,c,d,e) \
+    ((uint32_t)(((a) << 26)|((b) << 21)|((c) << 16)|((d) << 11)|(e)))
+
+#if defined(__mips_n64)
+
+#   define	_LOAD_T0_MDTLS_A1 \
+    _ENCODE_INSN(OP_LD, A1, T0, 0, offsetof(struct thread, td_md.md_tls))
+
+#   if defined(COMPAT_FREEBSD32)
+#   define	_ADDIU_V0_T0_TLS_OFFSET \
+    _ENCODE_INSN(OP_DADDIU, T0, V0, 0, (TLS_TP_OFFSET + TLS_TCB_SIZE32))
+#   else
+#   define	_ADDIU_V0_T0_TLS_OFFSET \
+    _ENCODE_INSN(OP_DADDIU, T0, V0, 0, (TLS_TP_OFFSET + TLS_TCB_SIZE))
+#   endif /* ! COMPAT_FREEBSD32 */
+
+#   define _MTC0_V0_USERLOCAL \
+    _ENCODE_INSN(OP_COP0, OP_DMT, V0, 4, 2)
+
+#else /* mips 32 */
+
+#   define	_LOAD_T0_MDTLS_A1 \
+    _ENCODE_INSN(OP_LW, A1, T0, 0, offsetof(struct thread, td_md.md_tls))
+#   define	_ADDIU_V0_T0_TLS_OFFSET \
+    _ENCODE_INSN(OP_ADDIU, T0, V0, 0, (TLS_TP_OFFSET + TLS_TCB_SIZE))
+#   define _MTC0_V0_USERLOCAL \
+    _ENCODE_INSN(OP_COP0, OP_MT, V0, 4, 2)
+
+#endif /* ! __mips_n64 */
+
+#define	_JR_RA	_ENCODE_INSN(OP_SPECIAL, RA, 0, 0, OP_JR)
+#define	_NOP	0
+
+/*
+ * Patch cpu_switch() by removing the UserLocal register code at the end.
+ * For MIPS hardware that don't support UserLocal Register Implementation
+ * we remove the instructions that update this register which may cause a
+ * reserved instruction exception in the kernel.
+ */
+static void
+remove_userlocal_code(uint32_t *cpu_switch_code)
+{
+	uint32_t *instructp;
+
+	for (instructp = cpu_switch_code;; instructp++) {
+		if (instructp[0] == _JR_RA)
+			panic("%s: Unable to patch cpu_switch().", __func__);
+		if (instructp[0] == _LOAD_T0_MDTLS_A1 &&
+		    instructp[1] == _ADDIU_V0_T0_TLS_OFFSET &&
+		    instructp[2] == _MTC0_V0_USERLOCAL) {
+			instructp[0] = _JR_RA;
+			instructp[1] = _NOP;
+			break;
+		}
+	}
+}
+
 /*
  * Attempt to identify the MIPS CPU as much as possible.
  *
@@ -73,9 +134,8 @@ mips_get_identity(struct mips_cpuinfo *cpuinfo)
 	u_int32_t prid;
 	u_int32_t cfg0;
 	u_int32_t cfg1;
-#ifndef CPU_CNMIPS
 	u_int32_t cfg2;
-#endif
+	u_int32_t cfg3;
 #if defined(CPU_CNMIPS)
 	u_int32_t cfg4;
 #endif
@@ -96,13 +156,36 @@ mips_get_identity(struct mips_cpuinfo *cpuinfo)
 	    ((cfg0 & MIPS_CONFIG0_MT_MASK) >> MIPS_CONFIG0_MT_SHIFT);
 	cpuinfo->icache_virtual = cfg0 & MIPS_CONFIG0_VI;
 
-	/* If config register selection 1 does not exist, exit. */
-	if (!(cfg0 & MIPS_CONFIG_CM))
+	/* If config register selection 1 does not exist, return. */
+	if (!(cfg0 & MIPS_CONFIG0_M))
 		return;
 
 	/* Learn TLB size and L1 cache geometry. */
 	cfg1 = mips_rd_config1();
 
+	/* Get the Config2 and Config3 registers as well. */
+	if (cfg1 & MIPS_CONFIG1_M) {
+		cfg2 = mips_rd_config2();
+		if (cfg2 & MIPS_CONFIG2_M)
+			cfg3 = mips_rd_config3();
+	}
+
+	/* Check to see if UserLocal register is implemented. */
+	if (cfg3 & MIPS_CONFIG3_ULR) {
+		/* UserLocal register is implemented, enable it. */
+		cpuinfo->userlocal_reg = true;
+		tmp = mips_rd_hwrena();
+		mips_wr_hwrena(tmp | MIPS_HWRENA_UL);
+	} else {
+		/*
+		 * UserLocal register is not implemented. Patch
+		 * cpu_switch() and remove unsupported code.
+		 */
+		cpuinfo->userlocal_reg = false;
+		remove_userlocal_code((uint32_t *)cpu_switch);
+	}
+
+
 #if defined(CPU_NLM)
 	/* Account for Extended TLB entries in XLP */
 	tmp = mips_rd_config6();
@@ -387,7 +470,7 @@ cpu_identify(void)
 
 	/* Print Config3 if it contains any useful info */
 	if (cfg3 & ~(0x80000000))
-		printf("  Config3=0x%b\n", cfg3, "\20\2SmartMIPS\1TraceLogic");
+		printf("  Config3=0x%b\n", cfg3, "\20\14ULRI\2SmartMIPS\1TraceLogic");
 }
 
 static struct rman cpu_hardirq_rman;
diff --git a/sys/mips/mips/genassym.c b/sys/mips/mips/genassym.c
index 3c0c1cc8f7b4..0396a591b559 100644
--- a/sys/mips/mips/genassym.c
+++ b/sys/mips/mips/genassym.c
@@ -57,6 +57,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/pcb.h>
 #include <machine/sigframe.h>
 #include <machine/proc.h>
+#include <machine/tls.h>
 
 #ifdef	CPU_CNMIPS
 #include <machine/octeon_cop2.h>
@@ -72,6 +73,13 @@ ASSYM(TD_KSTACK, offsetof(struct thread, td_kstack));
 ASSYM(TD_FLAGS, offsetof(struct thread, td_flags));
 ASSYM(TD_LOCK, offsetof(struct thread, td_lock));
 ASSYM(TD_MDFLAGS, offsetof(struct thread, td_md.md_flags));
+ASSYM(TD_MDTLS, offsetof(struct thread, td_md.md_tls));
+
+#if defined(__mips_n64) && defined(COMPAT_FREEBSD32)
+ASSYM(TLS_TCB_OFFSET, (TLS_TP_OFFSET + TLS_TCB_SIZE32));
+#else
+ASSYM(TLS_TCB_OFFSET, (TLS_TP_OFFSET + TLS_TCB_SIZE));
+#endif
 
 ASSYM(U_PCB_REGS, offsetof(struct pcb, pcb_regs.zero));
 ASSYM(U_PCB_CONTEXT, offsetof(struct pcb, pcb_context));
diff --git a/sys/mips/mips/swtch.S b/sys/mips/mips/swtch.S
index 1cd7df470c65..b2e6d9a6bac1 100644
--- a/sys/mips/mips/swtch.S
+++ b/sys/mips/mips/swtch.S
@@ -358,6 +358,7 @@ sw2:
  * Restore registers and return.
  */
 	move	a0, s0
+	move	a1, s7
 	RESTORE_U_PCB_CONTEXT(gp, PCB_REG_GP, a0)
 	RESTORE_U_PCB_CONTEXT(v0, PCB_REG_SR, a0)	# restore kernel context
 	RESTORE_U_PCB_CONTEXT(ra, PCB_REG_RA, a0)
@@ -377,6 +378,15 @@ sw2:
 	or	v0, v0, t0
 	mtc0	v0, MIPS_COP_0_STATUS
 	ITLBNOPFIX
+/*
+ * Set the new thread's TLS pointer.
+ *
+ * Note that this code is removed if the CPU doesn't support ULRI by
+ * remove_userlocal_code() in cpu.c.
+ */
+	PTR_L	t0, TD_MDTLS(a1)		# Get TLS pointer
+	PTR_ADDIU v0, t0, TLS_TCB_OFFSET	# Add TLS/TCB offset
+	MTC0	v0, MIPS_COP_0_USERLOCAL, 2	# write it to ULR for rdhwr
 
 	j	ra
 	nop
diff --git a/sys/mips/mips/sys_machdep.c b/sys/mips/mips/sys_machdep.c
index 5cd5366e3ffa..976a7f198abf 100644
--- a/sys/mips/mips/sys_machdep.c
+++ b/sys/mips/mips/sys_machdep.c
@@ -39,7 +39,11 @@ __FBSDID("$FreeBSD$");
 #include <sys/syscall.h>
 #include <sys/sysent.h>
 
+#include <machine/cpufunc.h>
+#include <machine/cpuinfo.h>
 #include <machine/sysarch.h>
+#include <machine/cpuregs.h>
+#include <machine/tls.h>
 
 #ifndef _SYS_SYSPROTO_H_
 struct sysarch_args {
@@ -57,6 +61,22 @@ sysarch(struct thread *td, struct sysarch_args *uap)
 	switch (uap->op) {
 	case MIPS_SET_TLS:
 		td->td_md.md_tls = uap->parms;
+
+		/*
+		 * If there is an user local register implementation (ULRI)
+		 * update it as well.  Add the TLS and TCB offsets so the
+		 * value in this register is adjusted like in the case of the
+		 * rdhwr trap() instruction handler.
+		 */
+		if (cpuinfo.userlocal_reg == true) {
+#if defined(__mips_n64) && defined(COMPAT_FREEBSD32)
+			mips_wr_userlocal((unsigned long)(uap->parms +
+			    TLS_TP_OFFSET + TLS_TCB_SIZE32));
+#else
+			mips_wr_userlocal((unsigned long)(uap->parms +
+			    TLS_TP_OFFSET + TLS_TCB_SIZE));
+#endif
+		}
 		return (0);
 	case MIPS_GET_TLS: 
 		tlsbase = td->td_md.md_tls;
diff --git a/sys/mips/mips/vm_machdep.c b/sys/mips/mips/vm_machdep.c
index 177f634fc4aa..d71ac2e3ec96 100644
--- a/sys/mips/mips/vm_machdep.c
+++ b/sys/mips/mips/vm_machdep.c
@@ -60,8 +60,11 @@ __FBSDID("$FreeBSD$");
 #include <machine/cache.h>
 #include <machine/clock.h>
 #include <machine/cpu.h>
+#include <machine/cpufunc.h>
+#include <machine/cpuinfo.h>
 #include <machine/md_var.h>
 #include <machine/pcb.h>
+#include <machine/tls.h>
 
 #include <vm/vm.h>
 #include <vm/vm_extern.h>
@@ -492,6 +495,15 @@ cpu_set_user_tls(struct thread *td, void *tls_base)
 {
 
 	td->td_md.md_tls = (char*)tls_base;
+	if (td == curthread && cpuinfo.userlocal_reg == true) {
+#if defined(__mips_n64) && defined(COMPAT_FREEBSD32)
+		mips_wr_userlocal((unsigned long)tls_base + TLS_TP_OFFSET +
+		    TLS_TCB_SIZE32);
+#else
+		mips_wr_userlocal((unsigned long)tls_base + TLS_TP_OFFSET +
+		    TLS_TCB_SIZE);
+#endif
+	}
 
 	return (0);
 }