R3000 changes which don't affect common code

5 files changed, 819 insertions, 540 deletions

diff --git a/arch/mips/kernel/r2300_fpu.S b/arch/mips/kernel/r2300_fpu.S
index 6c699c74d..de682be45 100644
--- a/arch/mips/kernel/r2300_fpu.S
+++ b/arch/mips/kernel/r2300_fpu.S
@@ -1,16 +1,17 @@
-/*
+/* $Id: $
  * r2300_fpu.S: Save/restore floating point context for signal handlers.
  *
  * This file is subject to the terms and conditions of the GNU General Public
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  *
- * Copyright (C) 1996 by Ralf Baechle
+ * Copyright (C) 1996, 1998 by Ralf Baechle
  *
  * Multi-arch abstraction and asm macros for easier reading:
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
  *
- * $Id: r2300_fpu.S,v 1.3 1997/12/01 16:54:20 ralf Exp $
+ * Further modifications to make this work:
+ * Copyright (c) 1998 Harald Koerfgen
  */
 #include <asm/asm.h>
 #include <asm/fpregdef.h>
@@ -18,61 +19,55 @@
 #include <asm/offset.h>
 #include <asm/regdef.h>
 
-	.set	mips3
+#define EX(a,b)							\
+9:	a,##b;							\
+	.section __ex_table,"a";				\
+	PTR	9b,bad_stack;					\
+	.previous
+
 	.set	noreorder
+	.set	mips1
 	/* Save floating point context */
-	.align	5
-	LEAF(r2300_save_fp_context)
-	mfc0	t0,CP0_STATUS
-	sll	t0,t0,2
+LEAF(r2300_save_fp_context)
 
-	bgez	t0,1f
-	 nop
-
-	cfc1	t0,fcr31
-	/* Store the 32 single precision registers */
-	swc1	$f0,(SC_FPREGS+0)(a0)
-	swc1	$f1,(SC_FPREGS+8)(a0)
-	swc1	$f2,(SC_FPREGS+16)(a0)
-	swc1	$f3,(SC_FPREGS+24)(a0)
-	swc1	$f4,(SC_FPREGS+32)(a0)
-	swc1	$f5,(SC_FPREGS+40)(a0)
-	swc1	$f6,(SC_FPREGS+48)(a0)
-	swc1	$f7,(SC_FPREGS+56)(a0)
-	swc1	$f8,(SC_FPREGS+64)(a0)
-	swc1	$f9,(SC_FPREGS+72)(a0)
-	swc1	$f10,(SC_FPREGS+80)(a0)
-	swc1	$f11,(SC_FPREGS+88)(a0)
-	swc1	$f12,(SC_FPREGS+96)(a0)
-	swc1	$f13,(SC_FPREGS+104)(a0)
-	swc1	$f14,(SC_FPREGS+112)(a0)
-	swc1	$f15,(SC_FPREGS+120)(a0)
-	swc1	$f16,(SC_FPREGS+128)(a0)
-	swc1	$f17,(SC_FPREGS+136)(a0)
-	swc1	$f18,(SC_FPREGS+144)(a0)
-	swc1	$f19,(SC_FPREGS+152)(a0)
-	swc1	$f20,(SC_FPREGS+160)(a0)
-	swc1	$f21,(SC_FPREGS+168)(a0)
-	swc1	$f22,(SC_FPREGS+176)(a0)
-	swc1	$f23,(SC_FPREGS+184)(a0)
-	swc1	$f24,(SC_FPREGS+192)(a0)
-	swc1	$f25,(SC_FPREGS+200)(a0)
-	swc1	$f26,(SC_FPREGS+208)(a0)
-	swc1	$f27,(SC_FPREGS+216)(a0)
-	swc1	$f28,(SC_FPREGS+224)(a0)
-	swc1	$f29,(SC_FPREGS+232)(a0)
-	swc1	$f30,(SC_FPREGS+240)(a0)
-	swc1	$f31,(SC_FPREGS+248)(a0)
-	sw	t0,SC_FPC_CSR(a0)
+	cfc1	t1,fcr31
+	EX(swc1	$f0,(SC_FPREGS+0)(a0))
+	EX(swc1	$f1,(SC_FPREGS+8)(a0))
+	EX(swc1	$f2,(SC_FPREGS+16)(a0))
+	EX(swc1	$f3,(SC_FPREGS+24)(a0))
+	EX(swc1	$f4,(SC_FPREGS+32)(a0))
+	EX(swc1	$f5,(SC_FPREGS+40)(a0))
+	EX(swc1	$f6,(SC_FPREGS+48)(a0))
+	EX(swc1	$f7,(SC_FPREGS+56)(a0))
+	EX(swc1	$f8,(SC_FPREGS+64)(a0))
+	EX(swc1	$f9,(SC_FPREGS+72)(a0))
+	EX(swc1	$f10,(SC_FPREGS+80)(a0))
+	EX(swc1	$f11,(SC_FPREGS+88)(a0))
+	EX(swc1	$f12,(SC_FPREGS+96)(a0))
+	EX(swc1	$f13,(SC_FPREGS+104)(a0))
+	EX(swc1	$f14,(SC_FPREGS+112)(a0))
+	EX(swc1	$f15,(SC_FPREGS+120)(a0))
+	EX(swc1	$f16,(SC_FPREGS+128)(a0))
+	EX(swc1	$f17,(SC_FPREGS+136)(a0))
+	EX(swc1	$f18,(SC_FPREGS+144)(a0))
+	EX(swc1	$f19,(SC_FPREGS+152)(a0))
+	EX(swc1	$f20,(SC_FPREGS+160)(a0))
+	EX(swc1	$f21,(SC_FPREGS+168)(a0))
+	EX(swc1	$f22,(SC_FPREGS+176)(a0))
+	EX(swc1	$f23,(SC_FPREGS+184)(a0))
+	EX(swc1	$f24,(SC_FPREGS+192)(a0))
+	EX(swc1	$f25,(SC_FPREGS+200)(a0))
+	EX(swc1	$f26,(SC_FPREGS+208)(a0))
+	EX(swc1	$f27,(SC_FPREGS+216)(a0))
+	EX(swc1	$f28,(SC_FPREGS+224)(a0))
+	EX(swc1	$f29,(SC_FPREGS+232)(a0))
+	EX(swc1	$f30,(SC_FPREGS+240)(a0))
+	EX(swc1	$f31,(SC_FPREGS+248)(a0))
+	EX(sw	t1,SC_FPC_CSR(a0))
 	cfc1	t0,$0				# implementation/version
 	jr	ra
 	.set	nomacro
-	 sw	t0,SC_FPC_EIR(a0)
-	.set	macro
-1:
-	jr	ra
-	.set	nomacro
-	 nop
+	 EX(sw	t0,SC_FPC_EIR(a0))
 	.set	macro
 	END(r2300_save_fp_context)
 
@@ -85,56 +80,40 @@
  * frame on the current content of c0_status, not on the content of the
  * stack frame which might have been changed by the user.
  */
-	LEAF(r2300_restore_fp_context)
-	mfc0	t0,CP0_STATUS
-	sll	t0,t0,2
-
-	bgez	t0,1f
-	 nop
-
-	 lw	t0,SC_FPC_CSR(a0)
-	/* Restore the 16 odd double precision registers only
-	 * when enabled in the cp0 status register.
-	 */
-	ldc1	$f0,(SC_FPREGS+0)(a0)
-	ldc1	$f1,(SC_FPREGS+8)(a0)
-	ldc1	$f2,(SC_FPREGS+16)(a0)
-	ldc1	$f3,(SC_FPREGS+24)(a0)
-	ldc1	$f4,(SC_FPREGS+32)(a0)
-	ldc1	$f5,(SC_FPREGS+40)(a0)
-	ldc1	$f6,(SC_FPREGS+48)(a0)
-	ldc1	$f7,(SC_FPREGS+56)(a0)
-	ldc1	$f8,(SC_FPREGS+64)(a0)
-	ldc1	$f9,(SC_FPREGS+72)(a0)
-	ldc1	$f10,(SC_FPREGS+80)(a0)
-	ldc1	$f11,(SC_FPREGS+88)(a0)
-	ldc1	$f12,(SC_FPREGS+96)(a0)
-	ldc1	$f13,(SC_FPREGS+104)(a0)
-	ldc1	$f14,(SC_FPREGS+112)(a0)
-	ldc1	$f15,(SC_FPREGS+120)(a0)
-	ldc1	$f16,(SC_FPREGS+128)(a0)
-	ldc1	$f17,(SC_FPREGS+136)(a0)
-	ldc1	$f18,(SC_FPREGS+144)(a0)
-	ldc1	$f19,(SC_FPREGS+152)(a0)
-	ldc1	$f20,(SC_FPREGS+160)(a0)
-	ldc1	$f21,(SC_FPREGS+168)(a0)
-	ldc1	$f22,(SC_FPREGS+176)(a0)
-	ldc1	$f23,(SC_FPREGS+184)(a0)
-	ldc1	$f24,(SC_FPREGS+192)(a0)
-	ldc1	$f25,(SC_FPREGS+200)(a0)
-	ldc1	$f26,(SC_FPREGS+208)(a0)
-	ldc1	$f27,(SC_FPREGS+216)(a0)
-	ldc1	$f28,(SC_FPREGS+224)(a0)
-	ldc1	$f29,(SC_FPREGS+232)(a0)
-	ldc1	$f30,(SC_FPREGS+240)(a0)
-	ldc1	$f31,(SC_FPREGS+248)(a0)
+LEAF(r2300_restore_fp_context)
+	EX(lw t0,SC_FPC_CSR(a0))
+	EX(lwc1	$f0,(SC_FPREGS+0)(a0))
+	EX(lwc1	$f1,(SC_FPREGS+8)(a0))
+	EX(lwc1	$f2,(SC_FPREGS+16)(a0))
+	EX(lwc1	$f3,(SC_FPREGS+24)(a0))
+	EX(lwc1	$f4,(SC_FPREGS+32)(a0))
+	EX(lwc1	$f5,(SC_FPREGS+40)(a0))
+	EX(lwc1	$f6,(SC_FPREGS+48)(a0))
+	EX(lwc1	$f7,(SC_FPREGS+56)(a0))
+	EX(lwc1	$f8,(SC_FPREGS+64)(a0))
+	EX(lwc1	$f9,(SC_FPREGS+72)(a0))
+	EX(lwc1	$f10,(SC_FPREGS+80)(a0))
+	EX(lwc1	$f11,(SC_FPREGS+88)(a0))
+	EX(lwc1	$f12,(SC_FPREGS+96)(a0))
+	EX(lwc1	$f13,(SC_FPREGS+104)(a0))
+	EX(lwc1	$f14,(SC_FPREGS+112)(a0))
+	EX(lwc1	$f15,(SC_FPREGS+120)(a0))
+	EX(lwc1	$f16,(SC_FPREGS+128)(a0))
+	EX(lwc1	$f17,(SC_FPREGS+136)(a0))
+	EX(lwc1	$f18,(SC_FPREGS+144)(a0))
+	EX(lwc1	$f19,(SC_FPREGS+152)(a0))
+	EX(lwc1	$f20,(SC_FPREGS+160)(a0))
+	EX(lwc1	$f21,(SC_FPREGS+168)(a0))
+	EX(lwc1	$f22,(SC_FPREGS+176)(a0))
+	EX(lwc1	$f23,(SC_FPREGS+184)(a0))
+	EX(lwc1	$f24,(SC_FPREGS+192)(a0))
+	EX(lwc1	$f25,(SC_FPREGS+200)(a0))
+	EX(lwc1	$f26,(SC_FPREGS+208)(a0))
+	EX(lwc1	$f27,(SC_FPREGS+216)(a0))
+	EX(lwc1	$f28,(SC_FPREGS+224)(a0))
+	EX(lwc1	$f29,(SC_FPREGS+232)(a0))
+	EX(lwc1	$f30,(SC_FPREGS+240)(a0))
+	EX(lwc1	$f31,(SC_FPREGS+248)(a0))
 	jr	ra
-	.set	nomacro
 	 ctc1	t0,fcr31
-	.set	macro
-1:
-	jr	ra
-	.set	nomacro
-	 nop
-	.set	macro
 	END(r2300_restore_fp_context)
diff --git a/arch/mips/kernel/r2300_misc.S b/arch/mips/kernel/r2300_misc.S
index 1a557d456..935e783d7 100644
--- a/arch/mips/kernel/r2300_misc.S
+++ b/arch/mips/kernel/r2300_misc.S
@@ -5,10 +5,15 @@
  *
  * Multi-cpu abstraction reworking:
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
+ *
+ * Further modifications to make this work:
+ * Copyright (c) 1998 Harald Koerfgen
+ * Copyright (c) 1998 Gleb Raiko & Vladimir Roganov
  */
 #include <linux/config.h>
 
 #include <asm/asm.h>
+#include <asm/current.h>
 #include <asm/bootinfo.h>
 #include <asm/cachectl.h>
 #include <asm/fpregdef.h>
@@ -18,380 +23,175 @@
 #include <asm/pgtable.h>
 #include <asm/processor.h>
 #include <asm/regdef.h>
+#include <asm/segment.h>
 #include <asm/stackframe.h>
 
 	.text
 	.set	mips1
 	.set	noreorder
 
-	.align	5
-	NESTED(r2300_handle_tlbl, PT_SIZE, sp)
-	.set	noat
-	/* Check whether this is a refill or an invalid exception */
-	mfc0	k0,CP0_BADVADDR
-	mfc0	k1,CP0_ENTRYHI
-	ori	k0,0xfff			# clear ASID...
-	xori	k0,0xfff			# in BadVAddr
-	andi	k1,0xfc0			# get current ASID
-	or	k0,k1				# make new entryhi
-	mfc0	k1,CP0_ENTRYHI
-	mtc0	k0,CP0_ENTRYHI
-	nop					# for pipeline
-	nop
-	nop
-	tlbp
-	nop					# for pipeline
-	nop
-	mfc0	k0,CP0_INDEX
-
-	bgez	k0,invalid_tlbl			# bad addr in c0_badvaddr
-	 mtc0	k1,CP0_ENTRYHI
-
-	/* Damn... The next nop is required on the R4400PC V5.0, but
-	 * I don't know why - at least there is no documented
-	 * reason as for the others :-(
-	 * And I haven't tested it as being necessary on R3000 - PMA.
-	 * (The R3000 pipeline has only 5 stages, so it's probably not
-	 * required -- Ralf)
-	 */
-	nop
-
-#ifdef CONF_DEBUG_TLB
-	/* OK, this is a double fault. Let's see whether this is
-	 * due to an invalid entry in the page_table.
-	 */
-	/* used to be dmfc0 */
-	mfc0	k0,CP0_BADVADDR
-	/* FIXME: This srl/sll sequence is as it is for the R4xx0,
-	 *        and I suspect that it should be different for
-	 *	  the R[23]000.  PMA
-	 *        (No, it's the assembler way to do
-	 *            k0 = k0 / PAGE_SIZE;
-	 *            k0 = k0 * sizeof(pte_t)
-	 *        Acutally the R4xx0 code will have to change when
-	 *        switching to 64 bit ... -- Ralf)
-	 */
-	srl	k0,12				# get PFN?
-	sll	k0,2
-	lui	k1,%HI(TLBMAP)
-	addu	k0,k1
-	lw	k1,(k0)
-	andi	k1,(_PAGE_PRESENT|_PAGE_ACCESSED)
-	bnez	k1,reload_pgd_entries
-	 nop
-
-	.set	noat
-	SAVE_ALL
-	.set	at
-	PRINT("Double fault caused by invalid entries in pgd:\n")
-	mfc0	a1,CP0_BADVADDR
-	PRINT("Double fault address     : %08lx\n")
-	mfc0	a1,CP0_EPC
-	PRINT("c0_epc                   : %08lx\n")
-
-	jal	show_regs
-	 move	a0,sp
-
-	jal	dump_tlb_nonwired
-	 nop
-
-	mfc0	a0,CP0_BADVADDR
+#undef NOTLB_OPTIMIZE /* If you are paranoid, define this. */
+
+	/* ABUSE of CPP macros 101. */
+
+	/* After this macro runs, the pte faulted on is
+	 * in register PTE, a ptr into the table in which
+	 * the pte belongs is in PTR.
+	 */
+#define LOAD_PTE(pte, ptr) \
+	mfc0	pte, CP0_BADVADDR; \
+	_GET_CURRENT(ptr); \
+	srl	pte, pte, 22; \
+	lw	ptr, THREAD_PGDIR(ptr); \
+	sll	pte, pte, 2; \
+	addu	ptr, pte, ptr; \
+	mfc0	pte, CP0_CONTEXT; \
+	lw	ptr, (ptr); \
+	andi	pte, pte, 0xffc; \
+	addu	ptr, ptr, pte; \
+	lw	pte, (ptr); \
+	nop;
+
+	/* This places the even/odd pte pair in the page
+	 * table at PTR into ENTRYLO0 and ENTRYLO1 using
+	 * TMP as a scratch register.
+	 */
+#define PTE_RELOAD(ptr) \
+	lw	ptr, (ptr)	; \
+	nop			; \
+	mtc0	ptr, CP0_ENTRYLO0; \
+	nop;
+
+#define DO_FAULT(write) \
+	.set	noat; \
+	.set	macro; \
+	SAVE_ALL; \
+	mfc0	a2, CP0_BADVADDR; \
+	STI; \
+	.set	at; \
+	move	a0, sp; \
+	jal	do_page_fault; \
+	 li	a1, write; \
+	j	ret_from_sys_call; \
+	 nop; \
+	.set	noat; \
+	.set	nomacro;
+
+	/* Check is PTE is present, if not then jump to LABEL.
+	 * PTR points to the page table where this PTE is located,
+	 * when the macro is done executing PTE will be restored
+	 * with it's original value.
+	 */
+#define PTE_PRESENT(pte, ptr, label) \
+	andi	pte, pte, (_PAGE_PRESENT | _PAGE_READ); \
+	xori	pte, pte, (_PAGE_PRESENT | _PAGE_READ); \
+	bnez	pte, label; \
+	.set	push;       \
+	.set	reorder;    \
+	 lw	pte, (ptr); \
+	.set	pop; 
+
+	/* Make PTE valid, store result in PTR. */
+#define PTE_MAKEVALID(pte, ptr) \
+	ori	pte, pte, (_PAGE_VALID | _PAGE_ACCESSED); \
+	sw	pte, (ptr);
+
+	/* Check if PTE can be written to, if not branch to LABEL.
+	 * Regardless restore PTE with value from PTR when done.
+	 */
+#define PTE_WRITABLE(pte, ptr, label) \
+	andi	pte, pte, (_PAGE_PRESENT | _PAGE_WRITE); \
+	xori	pte, pte, (_PAGE_PRESENT | _PAGE_WRITE); \
+	bnez	pte, label; \
+	.set    push;       \
+	.set    reorder;    \
+	lw      pte, (ptr); \
+	.set    pop;
+ 
+
+	/* Make PTE writable, update software status bits as well,
+	 * then store at PTR.
+	 */
+#define PTE_MAKEWRITE(pte, ptr) \
+	ori	pte, pte, (_PAGE_ACCESSED | _PAGE_MODIFIED | \
+			   _PAGE_VALID | _PAGE_DIRTY); \
+	sw	pte, (ptr);
 
-	jal	dump_list_current
-	 nop
+	.set	noreorder
 
-	.set	noat
-	STI
-	.set	at
-	PANIC("Corrupted pagedir")
+	.align	5
+NESTED(r2300_handle_tlbl, PT_SIZE, sp)
 	.set	noat
 
-reload_pgd_entries:
-#endif /* CONF_DEBUG_TLB */
-
-	/* Load missing pair of entries from the pgd and return. */
-	mfc0	k1,CP0_CONTEXT
-	lw	k0,(k1)			# Never causes nested exception
-	mfc0	k1,CP0_EPC		# get the return PC
-	srl	k0,12			# Convert to EntryLo format
-	mtc0	k0,CP0_ENTRYLO0
-	nop				# for pipeline
-	tlbwr
-	nop				# for pipeline
+#ifndef NOTLB_OPTIMIZE
+	/* Test present bit in entry. */
+	LOAD_PTE(k0, k1)
+        tlbp
 	nop
+        PTE_PRESENT(k0, k1, nopage_tlbl)
+        PTE_MAKEVALID(k0, k1)
+        PTE_RELOAD(k1)
+	tlbwi
 	nop
-	/* We don't know whether the original access was read or
-	 * write, so return and see what happens...
-	 */
-	jr	k1
-	 rfe
-
-	/* Handle invalid exception
-	 *
-	 * There are two possible causes for an invalid (tlbl)
-	 * exception:
-	 * 1) pages with present bit set but the valid bit clear
-	 * 2) nonexistant pages
-	 * Case one needs fast handling, therefore don't save
-	 * registers yet.
-	 *
-	 * k0 contains c0_index.
-	 */
-invalid_tlbl:	
-#ifdef CONFIG_TLB_SHUTDOWN
-	/* Remove entry so we don't need to care later
-	 * For sake of the pipeline the tlbwi insn has been moved down.
-	 * Moving it around is juggling with explosives...
-	 */
-	/* FIXME: Why is Ralf setting bit 3 of k1?  This may need to
-	 *	  be changed for R[236]000! PMA
-	 *        (The new ENTRYHI value will then point represent a
-	 *        inique virtual address outside the 32 bit address
-	 *        limit.  This is just paranoia to avoid a tlb
-	 *        shutdown.  This whole part of the routine is probably
-	 *        no longer required and can be removed -- Ralf)
-	 */
-	lui	k1,0x0008
-	or	k0,k1
-	sll	k0,12				# make it EntryHi format
-	mtc0	k0,CP0_ENTRYHI
-	mtc0	zero,CP0_ENTRYLO0
-#endif
-	/* Test present bit in entry */
-	mfc0	k0,CP0_BADVADDR
-	/* FIXME: This srl/sll sequence is as it is for the R4xx0,
-	 *        and I suspect that it should be different for
-	 *	  the R[23]000.  PMA
-	 *        (No, it's the assembler way to do
-	 *            k0 = k0 / PAGE_SIZE;
-	 *            k0 = k0 * sizeof(pte_t)
-	 *        Acutally the R4xx0 code will have to change when
-	 *        switching to 64 bit ... -- Ralf)
-	 */
-	srl	k0,12
-	sll	k0,2
-#ifdef CONFIG_TLB_SHUTDOWN
-	tlbwi						# do not move!
-#endif
-	lui	k1,%HI(TLBMAP)
-	addu	k0,k1
-	lw	k1,(k0)
-	andi	k1,(_PAGE_PRESENT|_PAGE_READ)
-	xori	k1,(_PAGE_PRESENT|_PAGE_READ)
-
-	bnez	k1,nopage_tlbl
-	 lw	k1,(k0)
-
-	/* Present and read bits are set -> set valid and accessed bits */
-	ori	k1,(_PAGE_VALID|_PAGE_ACCESSED)
-	sw	k1,(k0)
-	mfc0	k1,CP0_EPC
+	mfc0	k0, CP0_EPC
 	nop
-
-	jr	k1
+	jr	k0
 	 rfe
-
-	/* Page doesn't exist. Lots of work which is less important
-	 * for speed needs to be done, so hand it all over to the
-	 * kernel memory management routines.
-	 */
 nopage_tlbl:
-	SAVE_ALL
-	mfc0	a2,CP0_BADVADDR
-	STI
-	.set	at
-	/* a0 (struct pt_regs *) regs
-	 * a1 (unsigned long)    0 for read access
-	 * a2 (unsigned long)    faulting virtual address
-	 */
-	move	a0,sp
-	jal	do_page_fault
-	 li	a1,0
-
-	j	ret_from_sys_call
-	 nop
-	END(r2300_handle_tlbl)
+#endif
 
+	DO_FAULT(0)
+END(r2300_handle_tlbl)
 
-	.text
-	.align	5
-	NESTED(r2300_handle_tlbs, PT_SIZE, sp)
+NESTED(r2300_handle_tlbs, PT_SIZE, sp)
 	.set	noat
-	/* It is impossible that is a nested reload exception.
-	 * Therefore this must be a invalid exception.
-	 * Two possible cases:
-	 * 1) Page exists but not dirty.
-	 * 2) Page doesn't exist yet. Hand over to the kernel.
-	 *
-	 * Test whether present bit in entry is set
-	 */
-	/* used to be dmfc0 */
-	mfc0	k0,CP0_BADVADDR
-	/* FIXME: This srl/sll sequence is as it is for the R4xx0,
-	 *        and I suspect that it should be different for
-	 *	  the R[23]000.  PMA
-	 */
-	srl	k0,12
-	sll	k0,2
-	lui	k1,%HI(TLBMAP)
-	addu	k0,k1
-	lw	k1,(k0)
-	tlbp					# find faulting entry
-	andi	k1,(_PAGE_PRESENT|_PAGE_WRITE)
-	xori	k1,(_PAGE_PRESENT|_PAGE_WRITE)
 
-	bnez	k1,nopage_tlbs
-	 lw	k1,(k0)
-
-	/* Present and writable bits set: set accessed and dirty bits. */
-	ori	k1,k1,(_PAGE_ACCESSED|_PAGE_MODIFIED| \
-		       _PAGE_VALID|_PAGE_DIRTY)
-	sw	k1,(k0)
-	/* Now reload the entry into the TLB */
-	/* FIXME: Why has Ralf set bit 2?  Should it be different for
-	 *	  R[23]000?  PMA
-	 *        (The ori/xori combination actually _clears_ bit 2.
-	 *        This is required for the R4xx0 these CPUs always
-	 *        map page pairs; a page pair of 4k pages therfore
-	 *        has always an address with bit 2 set to zero. -- Ralf)
-	 */
-	ori	k0,0x0004
-	xori	k0,0x0004
-	lw	k0,(k0)
-	srl	k0,12
-	mtc0	k0,CP0_ENTRYLO0
-	mfc0	k1,CP0_EPC
-	nop				# for pipeline
+#ifndef NOTLB_OPTIMIZE
+	LOAD_PTE(k0, k1)
+	tlbp                            # find faulting entry
+	nop
+	PTE_WRITABLE(k0, k1, nopage_tlbs)
+	PTE_MAKEWRITE(k0, k1)
+	PTE_RELOAD(k1)
 	tlbwi
-	nop				# for pipeline
 	nop
+	mfc0	k0, CP0_EPC
 	nop
-
-	jr	k1
+	jr	k0
 	 rfe
-
-	/* Page doesn't exist. Lots of work which is less important
-	 * for speed needs to be done, so hand it all over to the
-	 * kernel memory management routines.
-	 */
 nopage_tlbs:
-nowrite_mod:
-#ifdef CONFIG_TLB_SHUTDOWN
-	/* Remove entry so we don't need to care later */
-	mfc0	k0,CP0_INDEX
-#ifdef CONF_DEBUG_TLB
-	bgez	k0,2f
-	 nop
-	/* We got a tlbs exception but found no matching entry in
-	 * the tlb.  This should never happen.  Paranoia makes us
-	 * check it, though.
-	 */
-	SAVE_ALL
-	jal	show_regs
-	 move	a0,sp
-	.set	at
-	mfc0	a1,CP0_BADVADDR
-	PRINT("c0_badvaddr == %08lx\n")
-	mfc0	a1,CP0_INDEX
-	PRINT("c0_index    == %08x\n")
-	mfc0	a1,CP0_ENTRYHI
-	PRINT("c0_entryhi  == %08x\n")
-	.set	noat
-	STI
-	.set	at
-	PANIC("Tlbs or tlbm exception with no matching entry in tlb")
-1:
-	j	1b
-	 nop
-2:
-#endif /* CONF_DEBUG_TLB */
-	/* FIXME: Why is Ralf setting bit 3 of k1?  This may need to
-	 *	  be changed for R[236]000! PMA
-	 *        (The new ENTRYHI value will then point represent a
-	 *        inique virtual address outside the 32 bit address
-	 *        limit.  This is just paranoia to avoid a tlb
-	 *        shutdown.  This whole part of the routine is probably
-	 *        no longer required and can be removed -- Ralf)
-	 */
-	lui	k1,0x0008
-	or	k0,k1
-	sll	k0,12
-	mtc0	k0,CP0_ENTRYHI
-	mtc0	zero,CP0_ENTRYLO0
-	nop				# for pipeline
-	nop				# R4000 V2.2 requires 4 NOPs
-	nop
-	nop
-	tlbwi
-#endif /* CONFIG_TLB_SHUTDOWN */
-	.set	noat
-	SAVE_ALL
-	mfc0	a2,CP0_BADVADDR
-	STI
-	.set	at
-	/* a0 (struct pt_regs *) regs
-	 * a1 (unsigned long)    1 for write access
-	 * a2 (unsigned long)    faulting virtual address
-	 */
-	move	a0,sp
-	jal	do_page_fault
-	 li	a1,1
-
-	j	ret_from_sys_call
-	 nop
-	END(r2300_handle_tlbs)
+#endif
 
+	DO_FAULT(1)
+END(r2300_handle_tlbs)
 
 	.align	5
-	NESTED(r2300_handle_mod, PT_SIZE, sp)
+NESTED(r2300_handle_mod, PT_SIZE, sp)
 	.set	noat
-	/* Two possible cases:
-	 * 1) Page is writable but not dirty -> set dirty and return
-	 * 2) Page is not writable -> call C handler
-	 */
-	/* used to be dmfc0 */
-	mfc0	k0,CP0_BADVADDR
-	/* FIXME: This srl/sll sequence is as it is for the R4xx0,
-	 *        and I suspect that it should be different for
-	 *	  the R[23]000.  PMA
-	 */
-	srl	k0,12
-	sll	k0,2
-	lui	k1,%HI(TLBMAP)
-	addu	k0,k1
-	lw	k1,(k0)
+#ifndef NOTLB_OPTIMIZE
+	LOAD_PTE(k0, k1)
 	tlbp					# find faulting entry
-	andi	k1,_PAGE_WRITE
-
-	beqz	k1,nowrite_mod
-	 lw	k1,(k0)
-
-	/* Present and writable bits set: set accessed and dirty bits. */
-	ori	k1,(_PAGE_ACCESSED|_PAGE_DIRTY)
-	sw	k1,(k0)
-	/* Now reload the entry into the tlb */
-	/* FIXME: Why has Ralf set bit 2?  Should it be different for
-	 *	  R[23]000?  PMA
-	 *        (The ori/xori combination actually _clears_ bit 2.
-	 *        This is required for the R4xx0 these CPUs always
-	 *        map page pairs; a page pair of 4k pages therfore
-	 *        has always an address with bit 2 set to zero. -- Ralf)
-	 */
-	ori	k0,0x0004
-	xori	k0,0x0004
-	lw	k0,(k0)
-	srl	k0,12
-	mtc0	k0,CP0_ENTRYLO0
-	mfc0	k1,CP0_EPC
-	nop				# for pipeline
-	nop
+	andi	k0, k0, _PAGE_WRITE
+	beqz	k0, nowrite_mod
+	.set	push
+	.set    reorder
+	lw	k0, (k1)
+	.set    pop
+
+	/* Present and writable bits set, set accessed and dirty bits. */
+	PTE_MAKEWRITE(k0, k1)
+
+	/* Now reload the entry into the tlb. */
+	PTE_RELOAD(k1)
 	nop
 	tlbwi
-	nop				# for pipeline
 	nop
+	mfc0	k0, CP0_EPC
 	nop
-
-	jr	k1
+	jr	k0
 	 rfe
-	END(r2300_handle_mod)
-	.set	at
+#endif
+
+nowrite_mod:
+	DO_FAULT(1)
+END(r2300_handle_mod)
diff --git a/arch/mips/kernel/r2300_switch.S b/arch/mips/kernel/r2300_switch.S
index b2b55bf0b..4b8698d3b 100644
--- a/arch/mips/kernel/r2300_switch.S
+++ b/arch/mips/kernel/r2300_switch.S
@@ -1,12 +1,13 @@
-/*
- * r2300_switch.S: R3000/R2000 specific task switching code.
+/* $Id: $
+ * r2300_switch.S: R2300 specific task switching code.
  *
  * Copyright (C) 1994, 1995, 1996 by Ralf Baechle and Andreas Busse
  *
  * Multi-cpu abstraction and macros for easier reading:
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
  *
- * $Id: r2300_switch.S,v 1.3 1998/03/27 04:47:55 ralf Exp $
+ * Further modifications to make this work:
+ * Copyright (c) 1998 Harald Koerfgen
  */
 #include <asm/asm.h>
 #include <asm/bootinfo.h>
@@ -15,6 +16,7 @@
 #include <asm/fpregdef.h>
 #include <asm/mipsconfig.h>
 #include <asm/mipsregs.h>
+#include <asm/offset.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include <asm/processor.h>
@@ -23,68 +25,132 @@
 
 #include <asm/asmmacro.h>
 
-/* XXX The following is fucking losing... find a better way dave. */
-MODE_ALIAS	=	0x00e0			# uncachable, dirty, valid
-
-	.text
 	.set	mips1
-	.set	noreorder
-/*
- * Code necessary to switch tasks on an Linux/MIPS machine.
- * FIXME: We don't need to disable interrupts anymore.
- */
 	.align	5
-	LEAF(r2300_resume)
-	mfc0	t1,CP0_STATUS		# Save status register
-	sw	t1,THREAD_STATUS($28)
-	ori	t2,t1,0x1f		# Disable interrupts
-	xori	t2,0x1e
-	mtc0	t2,CP0_STATUS
+
+LEAF(r2300_resume)
+	.set	reorder
+	mfc0	t1, CP0_STATUS
+	.set	noreorder
+	sw	t1, THREAD_STATUS($28)
 	CPU_SAVE_NONSCRATCH($28)
-	sll	t2,t1,2			# Save floating point state
-	bgez	t2,1f
-	 sw	ra,THREAD_REG31($28)
-	FPU_SAVE($28, t0)
-1:
+	sw	ra, THREAD_REG31($28)
+
+	/*
+	 * The order of restoring the registers takes care of the race
+	 * updating $28, $29 and kernelsp without disabling ints.
+	 */
 	move	$28, a0
-	lw	t0,THREAD_PGDIR($28)	# Switch the root pointer
-	li	t1,TLB_ROOT		# get PFN
-	mtc0	t1,CP0_ENTRYHI
-	mtc0	zero,CP0_INDEX
-	srl	t0,12			# PFN is 12 bits west
-	ori	t0,MODE_ALIAS		# want uncachable, dirty, valid
-	mtc0	t0,CP0_ENTRYLO0
-	lw	a2,THREAD_STATUS($28)
-	tlbwi
-
-	/* Flush TLB. */
-	mfc0	t3,CP0_STATUS		# disable interrupts...
-	ori	t4,t3,1
-	xori	t4,1
-	mtc0	t4,CP0_STATUS
-	lw      t0,mips_tlb_entries
-	mtc0	zero,CP0_ENTRYLO0
-1:
-	subu	t0,1
-	mtc0	t0,CP0_INDEX
-	lui	t1,0x0008
-	or	t1,t0,t1
-	sll	t1,12
-	mtc0	t1,CP0_ENTRYHI
-	bne	t2,t0,1b
-	 tlbwi
-
-	ori	t1,a2,1			# Restore FPU, pipeline magic
-	xori	t1,1
-	mtc0	t1,CP0_STATUS
-	sll	t0,a2,2
-	bgez	t0,1f
-	 lw	ra,THREAD_REG31($28)
-	FPU_RESTORE($28, t0)
-1:
 	CPU_RESTORE_NONSCRATCH($28)
 	addiu	t0, $28, KERNEL_STACK_SIZE-32
-	sw	t0,kernelsp
+	sw	t0, kernelsp
+	mfc0	t1, CP0_STATUS		/* Do we really need this? */
+	li	a3, 0xff00
+	and	t1, a3
+	lw	a2, THREAD_STATUS($28)
+	nor	a3, $0, a3
+	and	a2, a3
+	lw	a3, TASK_MM($28)
+	or	a2, t1
+	lw	a3, MM_CONTEXT(a3)
+	mtc0	a2, CP0_STATUS
+	andi	a3, 0xfc0
 	jr	ra
-	 mtc0	a2,CP0_STATUS			# Restore status register
+	 mtc0	a3, CP0_ENTRYHI
 	END(r2300_resume)
+
+/*
+ * Do lazy fpu context switch.  Saves FPU context to the process in a0
+ * and loads the new context of the current process.
+ */
+
+#define ST_OFF (KERNEL_STACK_SIZE - 32 - PT_SIZE + PT_STATUS)
+
+LEAF(r2300_lazy_fpu_switch)
+	mfc0	t0, CP0_STATUS			# enable cp1
+	li	t3, 0x20000000
+	or	t0, t3
+	mtc0	t0, CP0_STATUS
+
+	beqz	a0, 2f				# Save floating point state
+	 nor	t3, zero, t3
+	.set	reorder
+	lw	t1, ST_OFF(a0)			# last thread looses fpu
+	.set	noreorder
+	and	t1, t3
+	sw	t1, ST_OFF(a0)
+	swc1	$f0, (THREAD_FPU + 0x00)(a0)
+	FPU_SAVE(a0, t1)			# clobbers t1
+
+2:
+	lwc1	$f0, (THREAD_FPU + 0x00)($28)
+	.set	reorder
+	FPU_RESTORE($28, t0)		# clobbers t0
+	jr	ra
+	END(r2300_lazy_fpu_switch)
+
+/*
+ * Save a thread's fp context.
+ */
+	.set	noreorder
+LEAF(r2300_save_fp)
+	FPU_SAVE(a0, t1)			# clobbers t1
+	jr	ra
+	 swc1	$f0, (THREAD_FPU + 0x00)(a0)
+	END(r2300_save_fp)
+
+/*
+ * Load the FPU with signalling NANS.  This bit pattern we're using has
+ * the property that no matter wether considered as single or as double
+ * precission represents signaling NANS.
+ *
+ * We initialize fcr31 to rounding to nearest, no exceptions.
+ */
+
+#define FPU_DEFAULT  0x00000000
+
+LEAF(r2300_init_fpu)
+	mfc0	t0, CP0_STATUS
+	li	t1, 0x20000000
+	or	t0, t1
+	mtc0	t0, CP0_STATUS
+
+	li	t1, FPU_DEFAULT
+	ctc1	t1, fcr31
+
+	li	t0, -1
+
+	mtc1	t0, $f0
+	mtc1	t0, $f1
+	mtc1	t0, $f2
+	mtc1	t0, $f3
+	mtc1	t0, $f4
+	mtc1	t0, $f5
+	mtc1	t0, $f6
+	mtc1	t0, $f7
+	mtc1	t0, $f8
+	mtc1	t0, $f9
+	mtc1	t0, $f10
+	mtc1	t0, $f11
+	mtc1	t0, $f12
+	mtc1	t0, $f13
+	mtc1	t0, $f14
+	mtc1	t0, $f15
+	mtc1	t0, $f16
+	mtc1	t0, $f17
+	mtc1	t0, $f18
+	mtc1	t0, $f19
+	mtc1	t0, $f20
+	mtc1	t0, $f21
+	mtc1	t0, $f22
+	mtc1	t0, $f23
+	mtc1	t0, $f24
+	mtc1	t0, $f25
+	mtc1	t0, $f26
+	mtc1	t0, $f27
+	mtc1	t0, $f28
+	mtc1	t0, $f29
+	mtc1	t0, $f30
+	jr	ra
+	 mtc1	t0, $f31
+	END(r2300_init_fpu)
diff --git a/arch/mips/mm/loadmmu.c b/arch/mips/mm/loadmmu.c
index f8730ad26..82f6793c1 100644
--- a/arch/mips/mm/loadmmu.c
+++ b/arch/mips/mm/loadmmu.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
  *
- * $Id: loadmmu.c,v 1.6 1998/03/22 23:27:15 ralf Exp $
+ * $Id: loadmmu.c,v 1.7 1998/04/05 11:23:55 ralf Exp $
  */
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -66,6 +66,7 @@ __initfunc(void loadmmu(void))
 	switch(mips_cputype) {
 	case CPU_R2000:
 	case CPU_R3000:
+	case CPU_R3000A:
 		printk("Loading R[23]00 MMU routines.\n");
 		ld_mmu_r2300();
 		break;
diff --git a/arch/mips/mm/r2300.c b/arch/mips/mm/r2300.c
index 8f8e7ddfa..3c2cec663 100644
--- a/arch/mips/mm/r2300.c
+++ b/arch/mips/mm/r2300.c
@@ -1,8 +1,13 @@
-/* $Id: r2300.c,v 1.6 1998/08/25 09:14:46 ralf Exp $
- *
+/*
  * r2300.c: R2000 and R3000 specific mmu/cache code.
  *
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
+ *
+ * with a lot of changes to make this thing work for R3000s
+ * Copyright (C) 1998 Harald Koerfgen
+ * Copyright (C) 1998 Gleb Raiko & Vladimir Roganov
+ *
+ * $Id: r2300.c,v 1.5 1998/04/05 11:23:55 ralf Exp $
  */
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -11,12 +16,40 @@
 
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/mmu_context.h>
 #include <asm/system.h>
 #include <asm/sgialib.h>
-#include <asm/mmu_context.h>
+#include <asm/mipsregs.h>
+#include <asm/io.h>
+/*
+ * Temporarily disabled
+ *
+#include <asm/wbflush.h>
+ */
+
+/*
+ * According to the paper written by D. Miller about Linux cache & TLB
+ * flush implementation, DMA/Driver coherence should be done at the 
+ * driver layer.  Thus, normally, we don't need flush dcache for R3000.
+ * Define this if driver does not handle cache consistency during DMA ops.
+ */
+#undef DO_DCACHE_FLUSH
+
+/*
+ *  Unified cache space description structure
+ */
+static struct cache_space {
+	unsigned long ca_flags;  /* Cache space access flags */
+	int           size;      /* Cache space size */
+} icache, dcache;
+
+#undef DEBUG_TLB
+#undef DEBUG_CACHE
 
 extern unsigned long mips_tlb_entries;
 
+#define NTLB_ENTRIES       64  /* Fixed on all R23000 variants... */
+
 /* page functions */
 void r2300_clear_page(unsigned long page)
 {
@@ -94,80 +127,425 @@ static void r2300_copy_page(unsigned long to, unsigned long from)
 		 "I" (PAGE_SIZE));
 }
 
-/* Cache operations. */
-static inline void r2300_flush_cache_all(void) { }
-static void r2300_flush_cache_mm(struct mm_struct *mm) { }
+__initfunc(static unsigned long size_cache(unsigned long ca_flags))
+{
+	unsigned long flags, status, dummy, size;
+	volatile unsigned long *p;
+
+	p = (volatile unsigned long *) KSEG0;
+
+	save_and_cli(flags);
+
+	/* isolate cache space */
+	write_32bit_cp0_register(CP0_STATUS, (ca_flags|flags)&~ST0_IEC);
+
+	*p = 0xa5a55a5a;
+	dummy = *p;
+	status = read_32bit_cp0_register(CP0_STATUS);
+
+	if (dummy != 0xa5a55a5a || (status & (1<<19))) {
+		size = 0;
+	} else {
+		for (size = 512; size <= 0x40000; size <<= 1)
+			*(p + size) = 0;
+		*p = -1;
+		for (size = 512; 
+		     (size <= 0x40000) && (*(p + size) == 0); 
+		     size <<= 1)
+			;
+		if (size > 0x40000)
+			size = 0;
+	}
+	restore_flags(flags);
+
+	return size * sizeof(*p);
+}
+
+__initfunc(static void probe_dcache(void))
+{
+	dcache.size = size_cache(dcache.ca_flags = ST0_DE);
+	printk("Data cache %dkb\n", dcache.size >> 10);
+}
+
+__initfunc(static void probe_icache(void))
+{
+	icache.size = size_cache(icache.ca_flags = ST0_DE|ST0_CE);
+	printk("Instruction cache %dkb\n", icache.size >> 10);
+}
+
+static inline unsigned long get_phys_page (unsigned long page,
+					   struct mm_struct *mm)
+{
+        page &= PAGE_MASK;
+	if (page >= KSEG0 && page < KSEG1) {
+		/*
+		 *  We already have physical address
+		 */
+		return page;
+	} else {
+		if (!mm) {
+			printk ("get_phys_page: vaddr without mm\n");
+			return 0;
+		} else {
+			/* 
+			 *  Find a physical page using mm_struct
+			 */
+			pgd_t *page_dir;
+			pmd_t *page_middle;
+			pte_t *page_table, pte;
+
+			unsigned long address = page;
+
+			page_dir = pgd_offset(mm, address);
+			if (pgd_none(*page_dir))
+				return 0; 
+			page_middle = pmd_offset(page_dir, address);
+			if (pmd_none(*page_middle))
+				return 0; 
+			page_table = pte_offset(page_middle, address);
+			pte = *page_table;
+			if (!pte_present(pte))
+				return 0; 
+			return pte_page(pte);
+	}
+	}
+}
+
+static inline void flush_cache_space_page(struct cache_space *space,
+					  unsigned long page)
+{
+	register unsigned long i, flags, size = space->size;
+	register volatile unsigned char *p = (volatile unsigned char*) page;
+
+#ifndef DO_DCACHE_FLUSH
+	if (space == &dcache)
+		return;
+#endif
+	if (size > PAGE_SIZE)
+		size = PAGE_SIZE;
+
+	save_and_cli(flags);
+
+	/* isolate cache space */
+	write_32bit_cp0_register(CP0_STATUS, (space->ca_flags|flags)&~ST0_IEC);
+
+	for (i = 0; i < size; i += 64) {
+		asm ( 	"sb\t$0,(%0)\n\t"
+			"sb\t$0,4(%0)\n\t"
+			"sb\t$0,8(%0)\n\t"
+			"sb\t$0,12(%0)\n\t"
+			"sb\t$0,16(%0)\n\t"
+			"sb\t$0,20(%0)\n\t"
+			"sb\t$0,24(%0)\n\t"
+			"sb\t$0,28(%0)\n\t"
+ 		        "sb\t$0,32(%0)\n\t"
+ 			"sb\t$0,36(%0)\n\t"
+ 			"sb\t$0,40(%0)\n\t"
+ 			"sb\t$0,44(%0)\n\t"
+ 			"sb\t$0,48(%0)\n\t"
+ 			"sb\t$0,52(%0)\n\t"
+ 			"sb\t$0,56(%0)\n\t"
+ 			"sb\t$0,60(%0)\n\t"
+			: : "r" (p) );
+		p += 64;
+	}
+
+	restore_flags(flags);
+}
+
+static inline void flush_cache_space_all(struct cache_space *space)
+{
+	unsigned long page = KSEG0;
+	int size = space->size;
+
+#ifndef DO_DCACHE_FLUSH
+	if (space == &dcache)
+		return;
+#endif
+	while(size > 0) { 
+		flush_cache_space_page(space, page);
+		page += PAGE_SIZE; size -= PAGE_SIZE;
+        }
+}
+
+static inline void r2300_flush_cache_all(void)
+{
+	flush_cache_space_all(&dcache);
+	flush_cache_space_all(&icache);
+}
+ 
+static void r2300_flush_cache_mm(struct mm_struct *mm)
+{
+	if(mm->context == 0) 
+		return;
+#ifdef DEBUG_CACHE
+		printk("cmm[%d]", (int)mm->context);
+#endif
+	/*
+	 *  This function is called not offen, so it looks
+	 *  enough good to flush all caches than scan mm_struct,
+	 *  count pages to flush (and, very probably, flush more
+	 *  than cache space size :-)
+ */
+	flush_cache_all();
+}
+
 static void r2300_flush_cache_range(struct mm_struct *mm,
 				    unsigned long start,
 				    unsigned long end)
 {
+	/*
+	 *  In general, we need to flush both i- & d- caches here.
+	 *  Optimization: if cache space is less than given range,
+	 *  it is more quickly to flush all cache than all pages in range.
+	 */
+
+	unsigned long page;
+	int icache_done = 0, dcache_done = 0;
+
+	if(mm->context == 0) 
+		return;
+#ifdef DEBUG_CACHE
+	printk("crange[%d]", (int)mm->context);
+#endif
+	if (end - start >= icache.size) {
+		flush_cache_space_all(&icache);
+		icache_done = 1;
+	}
+	if (end - start >= dcache.size) {
+		flush_cache_space_all(&dcache);
+		dcache_done = 1;
+	}
+	if (icache_done && dcache_done)
+		return;
+
+	for (page = start; page < end; page += PAGE_SIZE) {
+		unsigned long phys_page = get_phys_page(page, mm);
+		
+		if (phys_page) {
+			if (!icache_done) 
+				flush_cache_space_page(&icache, phys_page);
+			if (!dcache_done) 
+				flush_cache_space_page(&dcache, phys_page);
+		}
+	}
 }
 
 static void r2300_flush_cache_page(struct vm_area_struct *vma,
 				   unsigned long page)
 {
+	struct mm_struct *mm = vma->vm_mm;
+
+	if(mm->context == 0)
+		return;
+#ifdef DEBUG_CACHE
+	printk("cpage[%d,%08lx]", (int)mm->context, page);
+#endif
+	/*
+	 *  User changes page, so we need to check:
+         *     is icache page flush needed ?
+	 *  It looks we don't need to flush dcache,
+	 *  due it is write-transparent on R3000
+	 */
+	if (vma->vm_flags & VM_EXEC) {
+		unsigned long phys_page = get_phys_page(page, vma->vm_mm);
+		if (phys_page)
+			flush_cache_space_page(&icache, phys_page); 
+	}
 }
 
 static void r2300_flush_page_to_ram(unsigned long page)
 {
-	/* XXX What we want to do here is perform a displacement
-	 * XXX flush because there are circumstances where you do
-	 * XXX indeed want to remove stale data from the cache.
-	 * XXX (DMA operations for example, where the cache cannot
-	 * XXX  "see" this data get changed.)
+	/*
+	 *  We need to flush both i- & d- caches :-(
+	 */
+	unsigned long phys_page = get_phys_page(page, NULL);
+#ifdef DEBUG_CACHE
+	printk("cram[%08lx]", page);
+#endif
+	if (phys_page) {
+		flush_cache_space_page(&icache, phys_page);
+		flush_cache_space_page(&dcache, phys_page);
+	}
+}
+
+static void r3k_dma_cache_wback_inv(unsigned long start, unsigned long size)
+{
+	register unsigned long i, flags;
+	register volatile unsigned char *p = (volatile unsigned char*) start;
+
+/*
+ * Temporarily disabled
+	wbflush();
 	 */
+
+	/*
+	 * Invalidate dcache
+	 */
+	if (size < 64)
+		size = 64;
+
+	if (size > dcache.size)
+		size = dcache.size;
+
+	save_and_cli(flags);
+
+	/* isolate cache space */
+	write_32bit_cp0_register(CP0_STATUS, (ST0_DE|flags)&~ST0_IEC);
+
+	for (i = 0; i < size; i += 64) {
+		asm ( 	"sb\t$0,(%0)\n\t"
+			"sb\t$0,4(%0)\n\t"
+			"sb\t$0,8(%0)\n\t"
+			"sb\t$0,12(%0)\n\t"
+			"sb\t$0,16(%0)\n\t"
+			"sb\t$0,20(%0)\n\t"
+			"sb\t$0,24(%0)\n\t"
+			"sb\t$0,28(%0)\n\t"
+ 		        "sb\t$0,32(%0)\n\t"
+ 			"sb\t$0,36(%0)\n\t"
+ 			"sb\t$0,40(%0)\n\t"
+ 			"sb\t$0,44(%0)\n\t"
+ 			"sb\t$0,48(%0)\n\t"
+ 			"sb\t$0,52(%0)\n\t"
+ 			"sb\t$0,56(%0)\n\t"
+ 			"sb\t$0,60(%0)\n\t"
+			: : "r" (p) );
+		p += 64;
+	}
+
+	restore_flags(flags);
 }
 
 static void r2300_flush_cache_sigtramp(unsigned long page)
 {
+	/*
+	 *  We need only flush i-cache here
+	 *
+	 *  This function receives virtual address (from signal.c),
+	 *  but this moment we have needed mm_struct in 'current'
+	 */
+	unsigned long phys_page = get_phys_page(page, current->mm);
+#ifdef DEBUG_CACHE
+		printk("csigtramp[%08lx]", page);
+#endif
+	if (phys_page)
+		flush_cache_space_page(&icache, phys_page);  
 }
 
 /* TLB operations. */
 static inline void r2300_flush_tlb_all(void)
 {
 	unsigned long flags;
+	unsigned long old_ctx;
 	int entry;
 
+#ifdef DEBUG_TLB
+	printk("[tlball]");
+#endif
+
 	save_and_cli(flags);
+	old_ctx = (get_entryhi() & 0xfc0);
 	write_32bit_cp0_register(CP0_ENTRYLO0, 0);
-	for(entry = 0; entry < mips_tlb_entries; entry++) {
-		write_32bit_cp0_register(CP0_INDEX, entry);
-		write_32bit_cp0_register(CP0_ENTRYHI, ((entry | 0x8) << 12));
+	for(entry = 0; entry < NTLB_ENTRIES; entry++) {
+		write_32bit_cp0_register(CP0_INDEX, entry << 8);
+		write_32bit_cp0_register(CP0_ENTRYHI, ((entry | 0x80000) << 12));
 		__asm__ __volatile__("tlbwi");
 	}
+	set_entryhi(old_ctx);
 	restore_flags(flags);
 }
 
 static void r2300_flush_tlb_mm(struct mm_struct *mm)
 {
+	if(mm->context != 0) {
+		unsigned long flags;
+
+#ifdef DEBUG_TLB
+		printk("[tlbmm<%d>]", mm->context);
+#endif
+		save_and_cli(flags);
+		get_new_mmu_context(mm, asid_cache);
 	if(mm == current->mm)
-		r2300_flush_tlb_all();
+			set_entryhi(mm->context & 0xfc0);
+		restore_flags(flags);
+	}
 }
 
 static void r2300_flush_tlb_range(struct mm_struct *mm, unsigned long start,
 				  unsigned long end)
 {
+	if(mm->context != 0) {
+		unsigned long flags;
+		int size;
+
+#ifdef DEBUG_TLB
+		printk("[tlbrange<%02x,%08lx,%08lx>]", (mm->context & 0xfc0),
+		       start, end);
+#endif
+		save_and_cli(flags);
+		size = (end - start + (PAGE_SIZE - 1)) >> PAGE_SHIFT;
+		if(size <= NTLB_ENTRIES) {
+			int oldpid = (get_entryhi() & 0xfc0);
+			int newpid = (mm->context & 0xfc0);
+
+			start &= PAGE_MASK;
+			end += (PAGE_SIZE - 1);
+			end &= PAGE_MASK;
+			while(start < end) {
+				int idx;
+
+				set_entryhi(start | newpid);
+				start += PAGE_SIZE;
+				tlb_probe();
+				idx = get_index();
+				set_entrylo0(0);
+				set_entryhi(KSEG0);
+				if(idx < 0)
+					continue;
+				tlb_write_indexed();
+			}
+			set_entryhi(oldpid);
+		} else {
+			get_new_mmu_context(mm, asid_cache);
 	if(mm == current->mm)
-		r2300_flush_tlb_all();
+				set_entryhi(mm->context & 0xfc0);
+		}
+		restore_flags(flags);
+	}
 }
 
 static void r2300_flush_tlb_page(struct vm_area_struct *vma, unsigned long page)
 {
-	if(vma->vm_mm == current->mm)
-		r2300_flush_tlb_all();
+	if(vma->vm_mm->context != 0) {
+		unsigned long flags;
+		int oldpid, newpid, idx;
+
+#ifdef DEBUG_TLB
+		printk("[tlbpage<%d,%08lx>]", vma->vm_mm->context, page);
+#endif
+		newpid = (vma->vm_mm->context & 0xfc0);
+		page &= PAGE_MASK;
+		save_and_cli(flags);
+		oldpid = (get_entryhi() & 0xfc0);
+		set_entryhi(page | newpid);
+		tlb_probe();
+		idx = get_index();
+		set_entrylo0(0);
+		set_entryhi(KSEG0);
+		if(idx < 0)
+			goto finish;
+		tlb_write_indexed();
+
+finish:
+		set_entryhi(oldpid);
+		restore_flags(flags);
+	}
 }
 
 /* Load a new root pointer into the TLB. */
 static void r2300_load_pgd(unsigned long pg_dir)
 {
-	unsigned long flags;
-
-	save_and_cli(flags);
-	write_32bit_cp0_register(CP0_ENTRYHI, TLB_ROOT);
-	write_32bit_cp0_register(CP0_INDEX, 0);
-	write_32bit_cp0_register(CP0_ENTRYLO0, ((pg_dir >> 6) | 0x00e0));
-	__asm__ __volatile__("tlbwi");
-	restore_flags(flags);
 }
 
 /*
@@ -199,17 +577,63 @@ static void r2300_pgd_init(unsigned long page)
 		 "=r" (dummy2)
 		:"r" ((unsigned long) invalid_pte_table),
 		 "0" (page),
-		 "1" (USER_PTRS_PER_PGD/8));
+		 "1" (PAGE_SIZE/(sizeof(pmd_t)*8)));
 }
 
 static void r2300_update_mmu_cache(struct vm_area_struct * vma,
 				   unsigned long address, pte_t pte)
 {
-	r2300_flush_tlb_page(vma, address);
-	/*
-	 * FIXME: We should also reload a new entry into the TLB to
-	 * avoid unnecessary exceptions.
-	 */
+	unsigned long flags;
+	pgd_t *pgdp;
+	pmd_t *pmdp;
+	pte_t *ptep;
+	int idx, pid;
+
+	pid = (get_entryhi() & 0xfc0);
+
+#ifdef DEBUG_TLB
+	if((pid != (vma->vm_mm->context & 0xfc0)) || (vma->vm_mm->context == 0)) {
+		printk("update_mmu_cache: Wheee, bogus tlbpid mmpid=%d tlbpid=%d\n",
+		       (int) (vma->vm_mm->context & 0xfc0), pid);
+	}
+#endif
+
+	save_and_cli(flags);
+	address &= PAGE_MASK;
+	set_entryhi(address | (pid));
+	pgdp = pgd_offset(vma->vm_mm, address);
+	tlb_probe();
+	pmdp = pmd_offset(pgdp, address);
+	idx = get_index();
+	ptep = pte_offset(pmdp, address);
+	set_entrylo0(pte_val(*ptep));
+	set_entryhi(address | (pid));
+	if(idx < 0) {
+		tlb_write_random();
+#if 0
+		printk("[MISS]");
+#endif
+	} else {
+		tlb_write_indexed();
+#if 0
+		printk("[HIT]");
+#endif
+	}
+#if 0
+	if(!strcmp(current->comm, "args")) {
+		printk("<");
+		for(idx = 0; idx < NTLB_ENTRIES; idx++) {
+			set_index(idx);
+			tlb_read();
+			address = get_entryhi();
+			if((address & 0xfc0) != 0)
+				printk("[%08lx]", address);
+		}
+		printk(">\n");
+	}
+#endif
+	set_entryhi(pid);
+	restore_flags(flags);
 }
 
 static void r2300_show_regs(struct pt_regs * regs)
@@ -248,6 +672,7 @@ static void r2300_show_regs(struct pt_regs * regs)
 static void r2300_add_wired_entry(unsigned long entrylo0, unsigned long entrylo1,
 				  unsigned long entryhi, unsigned long pagemask)
 {
+printk("r2300_add_wired_entry");
         /*
 	 * FIXME, to be done
 	 */
@@ -255,14 +680,19 @@ static void r2300_add_wired_entry(unsigned long entrylo0, unsigned long entrylo1
 
 static int r2300_user_mode(struct pt_regs *regs)
 {
-	return !(regs->cp0_status & 0x4);
+	return !(regs->cp0_status & ST0_KUP);
 }
 
 __initfunc(void ld_mmu_r2300(void))
 {
+	printk("CPU revision is: %08x\n", read_32bit_cp0_register(CP0_PRID));
+
 	clear_page = r2300_clear_page;
 	copy_page = r2300_copy_page;
 
+	probe_icache();
+	probe_dcache();
+
 	flush_cache_all = r2300_flush_cache_all;
 	flush_cache_mm = r2300_flush_cache_mm;
 	flush_cache_range = r2300_flush_cache_range;
@@ -274,16 +704,19 @@ __initfunc(void ld_mmu_r2300(void))
 	flush_tlb_mm = r2300_flush_tlb_mm;
 	flush_tlb_range = r2300_flush_tlb_range;
 	flush_tlb_page = r2300_flush_tlb_page;
-	r3000_asid_setup();
+
+        dma_cache_wback_inv = r3k_dma_cache_wback_inv;
 
 	load_pgd = r2300_load_pgd;
 	pgd_init = r2300_pgd_init;
 	update_mmu_cache = r2300_update_mmu_cache;
+	r3000_asid_setup();
 
 	show_regs = r2300_show_regs;
     
         add_wired_entry = r2300_add_wired_entry;
 
 	user_mode = r2300_user_mode;
+
 	flush_tlb_all();
 }