 @@ -1,14 +1,14 @@
-/*	$NetBSD: mipsX_subr.S,v 1.26.36.1.2.51 2011/12/13 07:14:51 matt Exp $	*/
+/*	$NetBSD: mipsX_subr.S,v 1.26.36.1.2.52 2011/12/23 23:40:00 matt Exp $	*/
 /*
  * Copyright 2002 Wasabi Systems, Inc.
  * All rights reserved.
+ *
  * Written by Simon Burge for Wasabi Systems, Inc.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
 @@ -1528,57 +1528,64 @@ END(MIPSX(systemcall))
  * from some types of errors but it is tricky.
  */
 	.p2align 5
 NESTED_NOPROFILE(MIPSX(cache_exception), KERNFRAME_SIZ, ra)
 	.set	noat
 	.mask	0x80000000, -4
 #ifdef sbmips	/* XXX!  SB-1 needs a real cache error handler */
 	eret
 	nop
 #endif
 	PTR_LA	k0, panic			# return to panic
 	PTR_LA	a0, 9f				# panicstr
 	_MFC0	a1, MIPS_COP_0_ERROR_PC
-#if (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
+#if (MIPS64_RMIXL) > 0
 	.set	push
 	.set	arch=xlr
 	li	k1, 0x309	/* L1D_CACHE_ERROR_LOG */
 	mfcr	a2, k1
 	li	k1, 0x30b	/* L1D_CACHE_INTERRUPT */
 	mfcr	a3, k1
-	.set	pop
+#if defined(__mips_o32)
 #error O32 not supported.
 #endif
 	mfc0	a4, MIPS_COP_0_STATUS
 	mfc0	a5, MIPS_COP_0_CAUSE
 #elif (MIPS64R2_RMIXL) > 0
 	li	k1, 0x308	/* LSU_CERR_LOG0 */
 	mfcr	a3, k1
 	li	k1, 0x309	/* LSU_CERR_LOG1 */
 	mfcr	a2, k1
 #if defined(__mips_o32)
 #error O32 not supported.
 #endif
 	mfc0	a4, MIPS_COP_0_STATUS
 	mfc0	a5, MIPS_COP_0_CAUSE
 #else
 	mfc0	a2, MIPS_COP_0_ECC
 	mfc0	a3, MIPS_COP_0_CACHE_ERR
 #endif
 	_MTC0	k0, MIPS_COP_0_ERROR_PC		# set return address
 	COP0_SYNC
 	mfc0	k0, MIPS_COP_0_STATUS		# restore status
 	li	k1, MIPS3_SR_DIAG_PE		# ignore further errors
 	or	k0, k1
 	mtc0	k0, MIPS_COP_0_STATUS		# restore status
 	COP0_SYNC
 	eret
-#if defined(MIPS64_XLS)
+#if (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
-	MSG("cache error @ EPC %#lx\nL1D_CACHE_ERROR_LOG %#lx\nL1D_CACHE_INTERRUPT %#lx\nstatus %#x, cause %#x");
+	MSG("cache error @ EPC %#llx\nL1D_CACHE_ERROR_LOG %#llx\nL1D_CACHE_INTERRUPT %#lx\nstatus %#x, cause %#x");
 #else
 	MSG("cache error @ EPC 0x%x ErrCtl 0x%x CacheErr 0x%x");
 #endif
 	.set	at
 END(MIPSX(cache_exception))
 /*----------------------------------------------------------------------------
+ *
  *	R4000 TLB exception handlers
+ *
  *----------------------------------------------------------------------------
  */
 @@ -1905,27 +1912,27 @@ LEAF(MIPSX(tlb_update_addr))
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 #if (PGSHIFT & 1) == 0
 	and	t1, a0, MIPS3_PG_ODDPG	# t1 = Even/Odd flag
 #endif
 	li	v0, (MIPS3_PG_HVPN | MIPS3_PG_ASID)
 	and	a0, a0, v0
-	_MFC0	t0, MIPS_COP_0_TLB_HI		# Save current PID
+	_MFC0	t0, MIPS_COP_0_TLB_HI		# Save current ASID
 	_MTC0	a0, MIPS_COP_0_TLB_HI		# Init high reg
 	COP0_SYNC
 	and	a2, a1, MIPS3_PG_G		# Copy global bit
 	tlbp					# Probe for the entry.
 	COP0_SYNC
 #if (MIPS32R2 + MIPS64R2 + MIPS64R2_RMIXL) > 0
 	_EXT	a1, a1, 0, WIRED_POS
 #else
 	_SLL	a1, a1, WIRED_SHIFT		# Clear top 34 bits of EntryLo
 	_SRL	a1, a1, WIRED_SHIFT
 #endif
 	mfc0	v0, MIPS_COP_0_TLB_INDEX	# See what we got
 #if (PGSHIFT & 1) == 0
 @@ -1962,27 +1969,27 @@ LEAF(MIPSX(tlb_update_addr))
 	COP0_SYNC
 	_MTC0	a1, MIPS_COP_0_TLB_LO1		# init low reg1.
 	COP0_SYNC
 	tlbwi					# update slot found
 	COP0_SYNC
 #endif /* (PGSHIFT & 1) == 0 */
 :
 #ifdef MIPS3
 	nop					# Make sure pipeline
 	nop					# advances before we
 	nop					# use the TLB.
 	nop
 #endif
-	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore PID
+	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore ASID
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# Restore the status register
 	JR_HB_RA
 END(MIPSX(tlb_update_addr))
 /*--------------------------------------------------------------------------
+ *
  * mipsN_tlb_read_indexed --
+ *
  *	Read the TLB entry.
 @@ -1998,100 +2005,109 @@ END(MIPSX(tlb_update_addr))
  *--------------------------------------------------------------------------
  */
 LEAF(MIPSX(tlb_read_indexed))
 	mfc0	v1, MIPS_COP_0_STATUS		# Save the status register.
 	mtc0	zero, MIPS_COP_0_STATUS		# Disable interrupts
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
-	mfc0	ta2, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
+	_MFC0	ta0, MIPS_COP_0_TLB_HI		# Get current ASID
 	mfc0	ta1, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
 	mfc0	ta2, MIPS_COP_0_TLB_INDEX	# save the index register
 #ifdef MIPS3
 	nop
 #endif
 	_MFC0	t0, MIPS_COP_0_TLB_HI		# Get current PID
 	mtc0	a0, MIPS_COP_0_TLB_INDEX	# Set the index register
 	COP0_SYNC
 	tlbr					# Read from the TLB
 	COP0_SYNC
-	mfc0	t2, MIPS_COP_0_TLB_PG_MASK	# fetch the pgMask
+	mfc0	t3, MIPS_COP_0_TLB_PG_MASK	# fetch the pgMask
-	_MFC0	t3, MIPS_COP_0_TLB_HI		# fetch the hi entry
+	_MFC0	t2, MIPS_COP_0_TLB_HI		# fetch the hi entry
-	_MFC0	ta0, MIPS_COP_0_TLB_LO0		# See what we got
+	_MFC0	t1, MIPS_COP_0_TLB_LO1		# See what we got
-	_MFC0	ta1, MIPS_COP_0_TLB_LO1		# See what we got
+	_MFC0	t0, MIPS_COP_0_TLB_LO0		# See what we got
-	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore PID
+	_MTC0	ta0, MIPS_COP_0_TLB_HI		# restore ASID
-	mtc0	ta2, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
+	mtc0	ta1, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
 	mtc0	ta2, MIPS_COP_0_TLB_INDEX	# make sure index is invalid
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# Restore the status register
 	COP0_SYNC
-	PTR_S	t3, TLBMASK_HI(a1)
+	PTR_S	t2, TLBMASK_HI(a1)
-	INT_S	ta0, TLBMASK_LO0(a1)
+	INT_S	t1, TLBMASK_LO1(a1)
-	INT_S	ta1, TLBMASK_LO1(a1)
+	INT_S	t0, TLBMASK_LO0(a1)
 	j	ra
-	 INT_S	t2, TLBMASK_MASK(a1)
+	 INT_S	t3, TLBMASK_MASK(a1)
 END(MIPSX(tlb_read_indexed))
 /*--------------------------------------------------------------------------
+ *
  * void mipsN_tlb_invalidate_addr(vaddr_t va)
+ *
  * Invalidate a TLB entry which has the given vaddr and ASID if found.
  *--------------------------------------------------------------------------
  */
 LEAF_NOPROFILE(MIPSX(tlb_invalidate_addr))
 	mfc0	v1, MIPS_COP_0_STATUS		# save status register
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interrupts
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 	li	v0, (MIPS3_PG_HVPN | MIPS3_PG_ASID)
-	_MFC0	t0, MIPS_COP_0_TLB_HI		# save current ASID
+	_MFC0	ta0, MIPS_COP_0_TLB_HI		# save current ASID
-	mfc0	t3, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
+	mfc0	ta1, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
 	mfc0	ta2, MIPS_COP_0_TLB_INDEX	# see what we got
 	and	a0, v0				# make sure valid entryHi
 	_MTC0	a0, MIPS_COP_0_TLB_HI		# look for the vaddr & ASID
 	COP0_SYNC
 	tlbp					# probe the entry in question
 	COP0_SYNC
-	mfc0	v0, MIPS_COP_0_TLB_INDEX	# see what we got
+	mfc0	t2, MIPS_COP_0_TLB_INDEX	# see what we got
-	bltz	v0, 1f				# index < 0 then skip
+	bltz	t2, 1f				# index < 0 then skip
-	 li	t1, MIPS_KSEG0_START		# invalid address
+	 li	v0, MIPS_KSEG0_START		# invalid address
-	PTR_SLL	v0, (PGSHIFT | 1)		# PAGE_SHIFT | 1
+#if (MIPS32R2 + MIPS64R2 + RMIXL_MIPS64R2) > 0
-	PTR_ADDU t1, v0
+	_INS	v0, t2, PGSHIFT | 1, 10
-	_MTC0	t1, MIPS_COP_0_TLB_HI		# make entryHi invalid
+#else
 	PTR_SLL	t2, (PGSHIFT | 1)		# PAGE_SHIFT | 1
 	PTR_ADDU v0, t2
 #endif
 	_MTC0	v0, MIPS_COP_0_TLB_HI		# make entryHi invalid
 	_MTC0	zero, MIPS_COP_0_TLB_LO0	# zero out entryLo0
 	_MTC0	zero, MIPS_COP_0_TLB_LO1	# zero out entryLo1
 #if 0
 	mtc0	zero, MIPS_COP_0_TLB_PG_MASK	# zero out pageMask
 #endif
 	COP0_SYNC
 	tlbwi
 	COP0_SYNC
 :
-	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore current ASID
+	_MTC0	ta0, MIPS_COP_0_TLB_HI		# restore current ASID
-	mtc0	t3, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
+	mtc0	ta1, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
 	mtc0	ta2, MIPS_COP_0_TLB_INDEX	# invalidate TLB index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# restore status register
 	JR_HB_RA
 END(MIPSX(tlb_invalidate_addr))
 /*
  * void mipsN_tlb_invalidate_asids(uint32_t base, uint32_t limit);
+ *
  * Invalidate TLB entries belong to per process user spaces with
  * base <= ASIDs < limit while leaving entries for kernel space
 @@ -2099,67 +2115,74 @@ END(MIPSX(tlb_invalidate_addr))
  */
 LEAF_NOPROFILE(MIPSX(tlb_invalidate_asids))
 	mfc0	v1, MIPS_COP_0_STATUS		# save status register
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interrupts
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
-	_MFC0	t0, MIPS_COP_0_TLB_HI		# Save the current PID.
+	_MFC0	t0, MIPS_COP_0_TLB_HI		# Save the current ASID.
-	mfc0	t1, MIPS_COP_0_TLB_WIRED
+	mfc0	t1, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
 	mfc0	t2, MIPS_COP_0_TLB_WIRED
 	INT_L	t3, _C_LABEL(mips_options) + MO_NUM_TLB_ENTRIES
 	li	v0, MIPS_KSEG0_START		# invalid address
-	INT_L	t2, _C_LABEL(mips_options) + MO_NUM_TLB_ENTRIES
+#if (MIPS32R2 + MIPS64R2 + RMIXL_MIPS64R2) > 0
-	mfc0	t3, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
+	_INS	v0, t2, PGSHIFT | 1, 10		# insert TLB index as page #
 #else
 	sll	ta0, t2, PGSHIFT | 1		# PAGE_SHIFT | 1
 	PTR_ADDU v0, ta0			# start at first unwired entry
 #endif
 	li	ta0, 1 << (PGSHIFT | 1)		# address increment.
-	# do {} while (t1 < t2)
+	# do {} while (t2 < t3)
 :
-	mtc0	t1, MIPS_COP_0_TLB_INDEX	# set index
+	mtc0	t2, MIPS_COP_0_TLB_INDEX	# set index
 	COP0_SYNC
 	sll	ta0, t1, PGSHIFT | 1		# PAGE_SHIFT | 1
 	tlbr					# obtain an entry
 	COP0_SYNC
-	_MFC0	a0, MIPS_COP_0_TLB_LO1
+	_MFC0	a2, MIPS_COP_0_TLB_LO1
-	and	a0, MIPS3_PG_G			# check to see it has G bit
+	and	a2, MIPS3_PG_G			# check to see it has G bit
-	bnez	a0, 2f				# yep, skip this one.
+	bnez	a2, 2f				# yep, skip this one.
 	 nop
-	_MFC0	a0, MIPS_COP_0_TLB_HI		# get VA and ASID
+	_MFC0	a2, MIPS_COP_0_TLB_HI		# get VA and ASID
-	and	a0, MIPS3_PG_ASID		# focus on ASID
+	and	a2, MIPS3_PG_ASID		# focus on ASID
-	sltu	a3, a0, a1			# asid < base?
+	sltu	a3, a2, a0			# asid < base?
 	bnez	a3, 2f				# yes, skip this entry.
 	 nop
-	sltu	a3, a0, a2			# asid < limit
+	sltu	a3, a2, a1			# asid < limit
 	beqz	a3, 2f				# nope, skip this entry.
 	 nop
 	PTR_ADDU ta0, v0
-	_MTC0	ta0, MIPS_COP_0_TLB_HI		# make entryHi invalid
+	_MTC0	v0, MIPS_COP_0_TLB_HI		# make entryHi invalid
 	_MTC0	zero, MIPS_COP_0_TLB_LO0	# zero out entryLo0
 	_MTC0	zero, MIPS_COP_0_TLB_LO1	# zero out entryLo1
 	mtc0	zero, MIPS_COP_0_TLB_PG_MASK	# zero out mask entry
 	COP0_SYNC
 	tlbwi					# invalidate the TLB entry
 	COP0_SYNC
 :
-	addu	t1, 1
+	addu	t2, 1
-	bne	t1, t2, 1b
+	bne	t2, t3, 1b
-	 nop
+	 PTR_ADDU v0, ta0
-	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore PID.
+	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore ASID.
-	mtc0	t3, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
+	mtc0	t1, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
 	# since v0 has its MSB set, it is an invalid TLB index
 	mtc0	v0, MIPS_COP_0_TLB_INDEX	# invalidate index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# restore status register
 	JR_HB_RA				# new ASID will be set soon
 END(MIPSX(tlb_invalidate_asids))
 /*
  * void mipsN_tlb_invalidate_globals(void);
+ *
  * Invalidate the non-wired TLB entries belonging to kernel space while
  * leaving entries for user space (not marked global) intact.
 @@ -2169,56 +2192,62 @@ LEAF_NOPROFILE(MIPSX(tlb_invalidate_glob
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interrupts
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 	_MFC0	t0, MIPS_COP_0_TLB_HI		# save current ASID
 	mfc0	t1, MIPS_COP_0_TLB_WIRED
 	li	v0, MIPS_KSEG0_START		# invalid address
 #if (MIPS32R2 + MIPS64R2 + RMIXL_MIPS64R2) > 0
 	_INS	v0, t1, PGSHIFT | 1, 10		# insert TLB index as page #
 #else
 	sll	ta0, t1, PGSHIFT | 1		# PAGE_SHIFT | 1
 	PTR_ADDU v0, ta0			# offset for TLB index
 #endif
 	li	ta0, 1 << (PGSHIFT | 1)
 	INT_L	t2, _C_LABEL(mips_options) + MO_NUM_TLB_ENTRIES
 	mfc0	t3, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
 	# do {} while (t1 < t2)
 :
 	mtc0	t1, MIPS_COP_0_TLB_INDEX	# set index
 	COP0_SYNC
 	sll	ta0, t1, PGSHIFT | 1		# PAGE_SHIFT | 1
 	tlbr					# obtain an entry
 	COP0_SYNC
 	_MFC0	a0, MIPS_COP_0_TLB_LO1
 	and	a0, MIPS3_PG_G			# check to see it has G bit
 	beqz	a0, 2f				# no, skip this entry
 	 nop
 	PTR_ADDU ta0, v0
-	_MTC0	ta0, MIPS_COP_0_TLB_HI		# make entryHi invalid
+	_MTC0	v0, MIPS_COP_0_TLB_HI		# make entryHi invalid
 	_MTC0	zero, MIPS_COP_0_TLB_LO0	# zero out entryLo0
 	_MTC0	zero, MIPS_COP_0_TLB_LO1	# zero out entryLo1
 	mtc0	zero, MIPS_COP_0_TLB_PG_MASK	# zero out mask entry
 	COP0_SYNC
 	tlbwi					# invalidate the TLB entry
 	COP0_SYNC
 :
 	addu	t1, 1
 	bne	t1, t2, 1b
-	 nop
+	 PTR_ADDU v0, ta0
 	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore current ASID
 	mtc0	t3, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
 	mtc0	v0, MIPS_COP_0_TLB_INDEX	# invalidate index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# restore status register
 	JR_HB_RA
 END(MIPSX(tlb_invalidate_globals))
 /*
  * void mipsN_tlb_invalidate_all(void);
+ *
  * Invalidate all of non-wired TLB entries.
  */
 @@ -2230,64 +2259,70 @@ LEAF_NOPROFILE(MIPSX(tlb_invalidate_all)
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 	INT_L	a0, _C_LABEL(mips_options) + MO_NUM_TLB_ENTRIES
 	li	v0, MIPS_KSEG0_START		# invalid address
 	_MFC0	t0, MIPS_COP_0_TLB_HI		# save current ASID
 	mfc0	t1, MIPS_COP_0_TLB_WIRED
 	mfc0	t2, MIPS_COP_0_TLB_PG_MASK	# save current pgMask
 #if (MIPS32R2 + MIPS64R2 + RMIXL_MIPS64R2) > 0
 	_INS	v0, t1, PGSHIFT | 1, 10		# insert TLB index as page #
 #else
 	sll	ta0, t1, PGSHIFT | 1		# addr for TLB index
 	PTR_ADDU v0, ta0
 #endif
 	li	ta0, 1 << (PGSHIFT | 1)
 	_MTC0	zero, MIPS_COP_0_TLB_LO0	# zero out entryLo0
 	_MTC0	zero, MIPS_COP_0_TLB_LO1	# zero out entryLo1
 	mtc0	zero, MIPS_COP_0_TLB_PG_MASK	# zero out pageMask
 	# do {} while (t1 < a0)
 :
 	mtc0	t1, MIPS_COP_0_TLB_INDEX	# set TLBindex
 	COP0_SYNC
-	sll	ta0, t1, PGSHIFT | 1		# PAGE_SHIFT | 1
+	_MTC0	v0, MIPS_COP_0_TLB_HI		# make entryHi invalid
 	PTR_ADDU ta0, v0
 	_MTC0	ta0, MIPS_COP_0_TLB_HI		# make entryHi invalid
 	COP0_SYNC
 	tlbwi					# clear the entry
 	COP0_SYNC
 	addu	t1, 1				# increment index
 	bne	t1, a0, 1b
-	 nop
+	 PTR_ADDU v0, ta0
 	_MTC0	t0, MIPS_COP_0_TLB_HI		# restore ASID
 	mtc0	t2, MIPS_COP_0_TLB_PG_MASK	# restore pgMask
 	mtc0	v0, MIPS_COP_0_TLB_INDEX	# invalidate index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# restore status register
 	JR_HB_RA
 END(MIPSX(tlb_invalidate_all))
 /*
  * u_int mipsN_tlb_record_asids(u_long *bitmap, uint32_t asid_mask);
+ *
  * Record all the ASIDs in use in the TLB and return the number of different
  * ASIDs present.
  */
 LEAF_NOPROFILE(MIPSX(tlb_record_asids))
-	_MFC0	a3, MIPS_COP_0_TLB_HI		# Save the current PID.
+	_MFC0	a3, MIPS_COP_0_TLB_HI		# Save the current ASID.
 	mfc0	ta0, MIPS_COP_0_TLB_WIRED
 	INT_L	ta1, _C_LABEL(mips_options) + MO_NUM_TLB_ENTRIES
 	move	ta2, zero
 	li	t3, 1
 	mfc0	v1, MIPS_COP_0_STATUS		# save status register
 #ifdef _LP64
 	and	t0, v1, MIPS_SR_INT_IE
 	xor	t0, v1
 	mtc0	t0, MIPS_COP_0_STATUS		# disable interrupts
 #else
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interrupts
 #endif
 @@ -2358,111 +2393,119 @@ END(MIPSX(tlb_record_asids))
  * mipsN_tlb_enter(size_t tlb_index, vaddr_t va, uint32_t pte);
  */
 LEAF(MIPSX(tlb_enter))
 	.set	noat
 	mfc0	v1, MIPS_COP_0_STATUS		# save status
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interupts
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 	# a0, a1, a3, v0, ta3 in use
 	_MFC0	ta0, MIPS_COP_0_TLB_HI		# save EntryHi
 	mfc0	ta1, MIPS_COP_0_TLB_INDEX	# save TLB index
 #if (PGSHIFT & 1) == 0
 	and	a3, a1, MIPS3_PG_ODDPG		# select odd page bit
-	xor	a3, a1				# clear it.
+	xor	a1, a3				# clear it.
 	/* a3 contain ODDPG bit, a1 is now even */
 #endif
-	_MTC0	a3, MIPS_COP_0_TLB_HI		# set the VA for tlbp
+	_MTC0	a1, MIPS_COP_0_TLB_HI		# set the VA for tlbp
 	COP0_SYNC
 #if (PGSHIFT & 1) == 0
 	and	t2, a2, MIPS3_PG_G		# make prototype tlb_lo0
 	and	t3, a2, MIPS3_PG_G		# make prototype tlb_lo1
 #endif
 	tlbp					# is va in TLB?
 	COP0_SYNC
-	mfc0	v0, MIPS_COP_0_TLB_INDEX	# was it in the TLB?
+	mfc0	t0, MIPS_COP_0_TLB_INDEX	# was it in the TLB?
-	bltz	v0, 1f				# nope
+	bltz	t0, 1f				# nope
 	 nop
 #if (PGSHIFT & 1) == 0
 	/*
 	 * Get the existing tlb_lo's because we need to replace one of them.
 	 */
 	mfc0	t2, MIPS_COP_0_TLB_LO0		# save for update
 	mfc0	t3, MIPS_COP_0_TLB_LO1		# save for update
 #endif
 	/*
 	 * If it's already where we want, no reason to invalidate it.
 	 */
-	beq	v0, a0, 2f			# already where we want it?
+	beq	t0, a0, 2f			# already where we want it?
 	 nop
 	/*
 	 * Clear the existing TLB entry for it.
 	 */
-	sll	t1, v0, (1 | PGSHIFT)		# make a fake addr for the entry
+	li	v0, MIPS_KSEG0_START
-	lui	t3, %hi(MIPS_KSEG0_START)
+#if (MIPS32R2 + MIPS64R2 + RMIXL_MIPS64R2) > 0
-	or	t1, t3
+	_INS	v0, t0, PGSHIFT | 1, 10		# insert TLB index into addr
-	_MTC0	t1, MIPS_COP_0_TLB_HI
+#else
 	sll	t1, t0, PGSHIFT | 1		# make a fake addr for the entry
 	or	v0, t1
 #endif
 	_MTC0	v0, MIPS_COP_0_TLB_HI		# set to KSEG0 addr (invalid)
 	COP0_SYNC
 	and	t0, a2, MIPS3_PG_G		# make prototype tlb_lo
 	mtc0	t0, MIPS_COP_0_TLB_LO0		# use an invalid tlb_lo0
 	mtc0	t0, MIPS_COP_0_TLB_LO1		# use an invalid tlb_lo1
 	COP0_SYNC
 	tlbwi					# now write the invalid TLB
 	COP0_SYNC
-	_MTC0	a3, MIPS_COP_0_TLB_HI		# restore the addr for new TLB
+	_MTC0	a1, MIPS_COP_0_TLB_HI		# restore the addr for new TLB
-	COP0_SYNC
+	COP0_SYNC				# a1 is free for use.
 :
 	mtc0	a0, MIPS_COP_0_TLB_INDEX	# set the index
-	COP0_SYNC
+	COP0_SYNC				# a0 is free for use.
 :
 #if (PGSHIFT & 1) == 0
-	and	t3, a1, MIPS3_PG_ODDPG		# odd or even page
+	sll	a3, 31 - PGSHIFT		# move ODDPG to MSB
-	sll	t3, 31 - PGSHIFT		# move to MSB
+	sra	a3, 31				# a3 a mask (0/~0 = even/odd)
-	sra	t3, 31				# t3 a mask (0/~0 = even/odd)
+	not	v0, a3				# v0 a mask (~0/0 = even/odd)
 	not	v0, t3				# v0 a mask (~0/0 = even/odd)
 	/* a0 and a1 are now free for use */
-	and	ta1, t2, t3
+	and	a0, a3, t2
-	and	ta2, a2, v0
+	and	a1, v0, a2
-	or	t2, ta1, ta2			# t2 = (t3 & t2) | (~t3 & a2)
+	or	t2, a0, a1			# t2 = (a3 & t2) | (~a3 & a2)
-	and	ta1, t3, v0
+	and	a0, v0, t3
-	and	ta2, a2, t3
+	and	a1, a3, a2
-	or	t3, ta1, ta2			# t3 = (~t3 & t3) | (t3 & a2)
+	or	t3, a0, a1			# t3 = (~a3 & t3) | (a3 & a2)
 	mtc0	t2, MIPS_COP_0_TLB_LO0		# set tlb_lo0 (even)
 	mtc0	t3, MIPS_COP_0_TLB_LO1		# set tlb_lo1 (odd)
 #else
 	mtc0	a2, MIPS_COP_0_TLB_LO0		# set tlb_lo1 (lower half)
 	INT_ADDU a2, MIPS3_PG_NEXT
 	mtc0	a2, MIPS_COP_0_TLB_LO1		# set tlb_lo1 (upper half)
 #endif
 	COP0_SYNC
 	tlbwi					# enter it into the TLB
 	COP0_SYNC
 	_MTC0	ta0, MIPS_COP_0_TLB_HI		# restore EntryHi
 	mtc0	ta1, MIPS_COP_0_TLB_INDEX	# restore TLB index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# restore status register
 	JR_HB_RA
 	.set	at
 END(MIPSX(tlb_enter))
 /*--------------------------------------------------------------------------
+ *
  * mipsN_tlb_write_indexed --
 @@ -2488,41 +2531,43 @@ LEAF(MIPSX(tlb_write_indexed))
 	INT_L	t1, TLBMASK_LO1(a1)		# fetch tlb->tlb_lo1
 	INT_L	t2, TLBMASK_MASK(a1)		# fetch tlb->tlb_mask
 	PTR_L	t3, TLBMASK_HI(a1)		# fetch tlb->tlb_hi
 	mfc0	v1, MIPS_COP_0_STATUS		# save status
 	mtc0	zero, MIPS_COP_0_STATUS		# disable interrupts
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	_MFC0	ta3, MIPS_COP_0_OSSCRATCH, 2
 :	li	v0, __SIMPLELOCK_LOCKED
 	swapw	v0, ta3
 	bnez	v0, 1b
 	 nop
 #endif
 	_MFC0	ta0, MIPS_COP_0_TLB_HI		# Save the current ASID.
 	mfc0	ta1, MIPS_COP_0_TLB_PG_MASK	# Save current page mask.
-	_MFC0	ta0, MIPS_COP_0_TLB_HI		# Save the current PID.
+	mfc0	ta2, MIPS_COP_0_TLB_INDEX	# Save current tlb index
 	_MTC0	t0, MIPS_COP_0_TLB_LO0		# Set up entry lo0.
 	_MTC0	t1, MIPS_COP_0_TLB_LO1		# Set up entry lo1.
 	COP0_SYNC
 	mtc0	a0, MIPS_COP_0_TLB_INDEX	# Set the index.
 	mtc0	t2, MIPS_COP_0_TLB_PG_MASK	# Set up entry pagemask.
 	_MTC0	t3, MIPS_COP_0_TLB_HI		# Set up entry high.
 	COP0_SYNC
 	tlbwi					# Write the TLB
 	COP0_SYNC
-	_MTC0	ta0, MIPS_COP_0_TLB_HI		# Restore the PID.
+	_MTC0	ta0, MIPS_COP_0_TLB_HI		# Restore the ASID.
 	mtc0	ta1, MIPS_COP_0_TLB_PG_MASK	# Restore page mask.
 	mtc0	ta2, MIPS_COP_0_TLB_INDEX	# Restore TLB index
 	COP0_SYNC
 #if defined(MULTIPROCESSOR) && (MIPS64_RMIXL + MIPS64R2_RMIXL) > 0
 	INT_S	zero, 0(ta3)			# unlock the tlb
 #endif
 	mtc0	v1, MIPS_COP_0_STATUS		# Restore the status register
 	JR_HB_RA
 END(MIPSX(tlb_write_indexed))
 /*
  * mipsN_lwp_trampoline()
+ *
  * Arrange for a function to be invoked neatly, after a cpu_switch().
  * Call the service function with one argument, specified by the s0