patch-2.4.22 linux-2.4.22/include/asm-mips64/r4kcache.h

• Lines: 697
• Date: 2003-08-25 04:44:44.000000000 -0700
• Orig file: linux-2.4.21/include/asm-mips64/r4kcache.h
• Orig date: 2002-08-02 17:39:45.000000000 -0700

diff -urN linux-2.4.21/include/asm-mips64/r4kcache.h linux-2.4.22/include/asm-mips64/r4kcache.h
@@ -6,93 +6,62 @@
  * Inline assembly cache operations.
  *
  * Copyright (C) 1996 David S. Miller (dm@engr.sgi.com)
- *
- * FIXME: Handle split L2 caches.
+ * Copyright (C) 1997 - 2002 Ralf Baechle (ralf@gnu.org)
  */
-#ifndef _ASM_R4KCACHE_H
-#define _ASM_R4KCACHE_H
+#ifndef __ASM_R4KCACHE_H
+#define __ASM_R4KCACHE_H
 
 #include <asm/asm.h>
-#include <asm/r4kcacheops.h>
+#include <asm/cacheops.h>
+
+#define cache_op(op,addr)						\
+	__asm__ __volatile__(						\
+	"	.set	noreorder		\n"			\
+	"	.set	mips3\n\t		\n"			\
+	"	cache	%0, %1			\n"			\
+	"	.set	mips0			\n"			\
+	"	.set	reorder"					\
+	:								\
+	: "i" (op), "m" (*(unsigned char *)(addr)))
 
 static inline void flush_icache_line_indexed(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Index_Invalidate_I));
+	cache_op(Index_Invalidate_I, addr);
 }
 
 static inline void flush_dcache_line_indexed(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Index_Writeback_Inv_D));
+	cache_op(Index_Writeback_Inv_D, addr);
 }
 
 static inline void flush_scache_line_indexed(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Index_Writeback_Inv_SD));
+	cache_op(Index_Writeback_Inv_SD, addr);
 }
 
 static inline void flush_icache_line(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Hit_Invalidate_I));
+	cache_op(Hit_Invalidate_I, addr);
 }
 
 static inline void flush_dcache_line(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Hit_Writeback_Inv_D));
+	cache_op(Hit_Writeback_Inv_D, addr);
 }
 
 static inline void invalidate_dcache_line(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Hit_Invalidate_D));
+	cache_op(Hit_Invalidate_D, addr);
 }
 
 static inline void invalidate_scache_line(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Hit_Invalidate_SD));
+	cache_op(Hit_Invalidate_SD, addr);
 }
 
 static inline void flush_scache_line(unsigned long addr)
 {
-	__asm__ __volatile__(
-		".set noreorder\n\t"
-		"cache %1, (%0)\n\t"
-		".set reorder"
-		:
-		: "r" (addr), "i" (Hit_Writeback_Inv_SD));
+	cache_op(Hit_Writeback_Inv_SD, addr);
 }
 
 /*
@@ -102,31 +71,50 @@
 {
 	__asm__ __volatile__(
 		".set noreorder\n\t"
-		"1:\tcache %1,(%0)\n"
-		"2:\t.set reorder\n\t"
+		".set mips3\n"
+		"1:\tcache %0,(%1)\n"
+		"2:\t.set mips0\n\t"
+		".set reorder\n\t"
 		".section\t__ex_table,\"a\"\n\t"
-		".dword\t1b,2b\n\t"
+		STR(PTR)"\t1b,2b\n\t"
 		".previous"
 		:
-		: "r" (addr), "i" (Hit_Invalidate_I));
+		: "i" (Hit_Invalidate_I), "r" (addr));
 }
 
+/*
+ * R10000 / R12000 hazard - these processors don't support the Hit_Writeback_D
+ * cacheop so we use Hit_Writeback_Inv_D which is supported by all R4000-style
+ * caches.  We're talking about one cacheline unnecessarily getting invalidated
+ * here so the penaltiy isn't overly hard.
+ */
 static inline void protected_writeback_dcache_line(unsigned long addr)
 {
 	__asm__ __volatile__(
 		".set noreorder\n\t"
-		"1:\tcache %1,(%0)\n"
-		"2:\t.set reorder\n\t"
+		".set mips3\n"
+		"1:\tcache %0,(%1)\n"
+		"2:\t.set mips0\n\t"
+		".set reorder\n\t"
 		".section\t__ex_table,\"a\"\n\t"
-		".dword\t1b,2b\n\t"
+		STR(PTR)"\t1b,2b\n\t"
 		".previous"
 		:
-		: "r" (addr), "i" (Hit_Writeback_D));
+		: "i" (Hit_Writeback_Inv_D), "r" (addr));
+}
+
+/*
+ * This one is RM7000-specific
+ */
+static inline void invalidate_tcache_page(unsigned long addr)
+{
+	cache_op(Page_Invalidate_T, addr);
 }
 
 #define cache16_unroll32(base,op)				\
 	__asm__ __volatile__("					\
 		.set noreorder;					\
+		.set mips3;					\
 		cache %1, 0x000(%0); cache %1, 0x010(%0);	\
 		cache %1, 0x020(%0); cache %1, 0x030(%0);	\
 		cache %1, 0x040(%0); cache %1, 0x050(%0);	\
@@ -143,6 +131,7 @@
 		cache %1, 0x1a0(%0); cache %1, 0x1b0(%0);	\
 		cache %1, 0x1c0(%0); cache %1, 0x1d0(%0);	\
 		cache %1, 0x1e0(%0); cache %1, 0x1f0(%0);	\
+		.set mips0;					\
 		.set reorder"					\
 		:						\
 		: "r" (base),					\
@@ -151,20 +140,23 @@
 static inline void blast_dcache16(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = (start + dcache_size);
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Writeback_Inv_D);
-		start += 0x200;
-	}
+	unsigned long end = start + current_cpu_data.dcache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.dcache.waybit;
+	unsigned long ws_end = current_cpu_data.dcache.ways << 
+	                       current_cpu_data.dcache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200)
+			cache16_unroll32(addr|ws,Index_Writeback_Inv_D);
 }
 
 static inline void blast_dcache16_page(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
+	unsigned long end = start + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache16_unroll32(start,Hit_Writeback_Inv_D);
 		start += 0x200;
 	}
@@ -173,31 +165,37 @@
 static inline void blast_dcache16_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Writeback_Inv_D);
-		start += 0x200;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.dcache.waybit;
+	unsigned long ws_end = current_cpu_data.dcache.ways <<
+	                       current_cpu_data.dcache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200) 
+			cache16_unroll32(addr|ws,Index_Writeback_Inv_D);
 }
 
 static inline void blast_icache16(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = (start + icache_size);
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Invalidate_I);
-		start += 0x200;
-	}
+	unsigned long end = start + current_cpu_data.icache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200) 
+			cache16_unroll32(addr|ws,Index_Invalidate_I);
 }
 
 static inline void blast_icache16_page(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
+	unsigned long end = start + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache16_unroll32(start,Hit_Invalidate_I);
 		start += 0x200;
 	}
@@ -206,23 +204,29 @@
 static inline void blast_icache16_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Invalidate_I);
-		start += 0x200;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200) 
+			cache16_unroll32(addr|ws,Index_Invalidate_I);
 }
 
 static inline void blast_scache16(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = KSEG0 + scache_size;
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x200;
-	}
+	unsigned long end = start + current_cpu_data.scache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways << 
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200)
+			cache16_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 static inline void blast_scache16_page(unsigned long page)
@@ -230,7 +234,7 @@
 	unsigned long start = page;
 	unsigned long end = page + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache16_unroll32(start,Hit_Writeback_Inv_SD);
 		start += 0x200;
 	}
@@ -239,17 +243,21 @@
 static inline void blast_scache16_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = page + PAGE_SIZE;
-
-	while(start < end) {
-		cache16_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x200;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways <<
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x200) 
+			cache16_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 #define cache32_unroll32(base,op)				\
 	__asm__ __volatile__("					\
 		.set noreorder;					\
+		.set mips3;					\
 		cache %1, 0x000(%0); cache %1, 0x020(%0);	\
 		cache %1, 0x040(%0); cache %1, 0x060(%0);	\
 		cache %1, 0x080(%0); cache %1, 0x0a0(%0);	\
@@ -266,6 +274,7 @@
 		cache %1, 0x340(%0); cache %1, 0x360(%0);	\
 		cache %1, 0x380(%0); cache %1, 0x3a0(%0);	\
 		cache %1, 0x3c0(%0); cache %1, 0x3e0(%0);	\
+		.set mips0;					\
 		.set reorder"					\
 		:						\
 		: "r" (base),					\
@@ -274,38 +283,23 @@
 static inline void blast_dcache32(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = (start + dcache_size);
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Writeback_Inv_D);
-		start += 0x400;
-	}
+	unsigned long end = start + current_cpu_data.dcache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.dcache.waybit;
+	unsigned long ws_end = current_cpu_data.dcache.ways <<
+	                       current_cpu_data.dcache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x400) 
+			cache32_unroll32(addr|ws,Index_Writeback_Inv_D);
 }
 
-/*
- * Call this function only with interrupts disabled or R4600 V2.0 may blow
- * up on you.
- *
- * R4600 v2.0 bug: "The CACHE instructions Hit_Writeback_Inv_D,
- * Hit_Writeback_D, Hit_Invalidate_D and Create_Dirty_Excl_D will only
- * operate correctly if the internal data cache refill buffer is empty.  These
- * CACHE instructions should be separated from any potential data cache miss
- * by a load instruction to an uncached address to empty the response buffer."
- * (Revision 2.0 device errata from IDT available on http://www.idt.com/
- * in .pdf format.)
- */
 static inline void blast_dcache32_page(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
-
-	/*
-	 * Sigh ... workaround for R4600 v1.7 bug.  Explanation see above.
-	 */
-	*(volatile unsigned long *)KSEG1;
+	unsigned long end = start + PAGE_SIZE;
 
-	__asm__ __volatile__("nop;nop;nop;nop");
-	while(start < end) {
+	while (start < end) {
 		cache32_unroll32(start,Hit_Writeback_Inv_D);
 		start += 0x400;
 	}
@@ -314,31 +308,37 @@
 static inline void blast_dcache32_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Writeback_Inv_D);
-		start += 0x400;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.dcache.waybit;
+	unsigned long ws_end = current_cpu_data.dcache.ways <<
+	                       current_cpu_data.dcache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x400) 
+			cache32_unroll32(addr|ws,Index_Writeback_Inv_D);
 }
 
 static inline void blast_icache32(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = (start + icache_size);
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Invalidate_I);
-		start += 0x400;
-	}
+	unsigned long end = start + current_cpu_data.icache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x400) 
+			cache32_unroll32(addr|ws,Index_Invalidate_I);
 }
 
 static inline void blast_icache32_page(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
+	unsigned long end = start + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache32_unroll32(start,Hit_Invalidate_I);
 		start += 0x400;
 	}
@@ -347,23 +347,29 @@
 static inline void blast_icache32_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = (start + PAGE_SIZE);
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Invalidate_I);
-		start += 0x400;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc)
+		for (addr = start; addr < end; addr += 0x400) 
+			cache32_unroll32(addr|ws,Index_Invalidate_I);
 }
 
 static inline void blast_scache32(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = KSEG0 + scache_size;
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x400;
-	}
+	unsigned long end = start + current_cpu_data.scache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways << 
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x400)
+			cache32_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 static inline void blast_scache32_page(unsigned long page)
@@ -371,7 +377,7 @@
 	unsigned long start = page;
 	unsigned long end = page + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache32_unroll32(start,Hit_Writeback_Inv_SD);
 		start += 0x400;
 	}
@@ -380,17 +386,21 @@
 static inline void blast_scache32_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = page + PAGE_SIZE;
-
-	while(start < end) {
-		cache32_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x400;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways <<
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x400) 
+			cache32_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 #define cache64_unroll32(base,op)				\
 	__asm__ __volatile__("					\
 		.set noreorder;					\
+		.set mips3;					\
 		cache %1, 0x000(%0); cache %1, 0x040(%0);	\
 		cache %1, 0x080(%0); cache %1, 0x0c0(%0);	\
 		cache %1, 0x100(%0); cache %1, 0x140(%0);	\
@@ -407,28 +417,71 @@
 		cache %1, 0x680(%0); cache %1, 0x6c0(%0);	\
 		cache %1, 0x700(%0); cache %1, 0x740(%0);	\
 		cache %1, 0x780(%0); cache %1, 0x7c0(%0);	\
+		.set mips0;					\
 		.set reorder"					\
 		:						\
 		: "r" (base),					\
 		  "i" (op));
 
-static inline void blast_scache64(void)
+static inline void blast_icache64(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = KSEG0 + scache_size;
+	unsigned long end = start + current_cpu_data.icache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x800) 
+			cache64_unroll32(addr|ws,Index_Invalidate_I);
+}
 
-	while(start < end) {
-		cache64_unroll32(start,Index_Writeback_Inv_SD);
+static inline void blast_icache64_page(unsigned long page)
+{
+	unsigned long start = page;
+	unsigned long end = start + PAGE_SIZE;
+
+	while (start < end) {
+		cache64_unroll32(start,Hit_Invalidate_I);
 		start += 0x800;
 	}
 }
 
+static inline void blast_icache64_page_indexed(unsigned long page)
+{
+	unsigned long start = page;
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.icache.waybit;
+	unsigned long ws_end = current_cpu_data.icache.ways <<
+	                       current_cpu_data.icache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc)
+		for (addr = start; addr < end; addr += 0x800) 
+			cache64_unroll32(addr|ws,Index_Invalidate_I);
+}
+
+static inline void blast_scache64(void)
+{
+	unsigned long start = KSEG0;
+	unsigned long end = start + current_cpu_data.scache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways << 
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x800)
+			cache64_unroll32(addr|ws,Index_Writeback_Inv_SD);
+}
+
 static inline void blast_scache64_page(unsigned long page)
 {
 	unsigned long start = page;
 	unsigned long end = page + PAGE_SIZE;
 
-	while(start < end) {
+	while (start < end) {
 		cache64_unroll32(start,Hit_Writeback_Inv_SD);
 		start += 0x800;
 	}
@@ -437,17 +490,21 @@
 static inline void blast_scache64_page_indexed(unsigned long page)
 {
 	unsigned long start = page;
-	unsigned long end = page + PAGE_SIZE;
-
-	while(start < end) {
-		cache64_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x800;
-	}
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways <<
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x800) 
+			cache64_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 #define cache128_unroll32(base,op)				\
 	__asm__ __volatile__("					\
 		.set noreorder;					\
+		.set mips3;					\
 		cache %1, 0x000(%0); cache %1, 0x080(%0);	\
 		cache %1, 0x100(%0); cache %1, 0x180(%0);	\
 		cache %1, 0x200(%0); cache %1, 0x280(%0);	\
@@ -464,6 +521,7 @@
 		cache %1, 0xd00(%0); cache %1, 0xd80(%0);	\
 		cache %1, 0xe00(%0); cache %1, 0xe80(%0);	\
 		cache %1, 0xf00(%0); cache %1, 0xf80(%0);	\
+		.set mips0;					\
 		.set reorder"					\
 		:						\
 		: "r" (base),					\
@@ -472,22 +530,40 @@
 static inline void blast_scache128(void)
 {
 	unsigned long start = KSEG0;
-	unsigned long end = KSEG0 + scache_size;
-
-	while(start < end) {
-		cache128_unroll32(start,Index_Writeback_Inv_SD);
-		start += 0x1000;
-	}
+	unsigned long end = start + current_cpu_data.scache.waysize;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways << 
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x1000)
+			cache128_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 static inline void blast_scache128_page(unsigned long page)
 {
-	cache128_unroll32(page,Hit_Writeback_Inv_SD);
+	unsigned long start = page;
+	unsigned long end = page + PAGE_SIZE;
+
+	while (start < end) {
+		cache128_unroll32(start,Hit_Writeback_Inv_SD);
+		start += 0x1000;
+	}
 }
 
 static inline void blast_scache128_page_indexed(unsigned long page)
 {
-	cache128_unroll32(page,Index_Writeback_Inv_SD);
+	unsigned long start = page;
+	unsigned long end = start + PAGE_SIZE;
+	unsigned long ws_inc = 1UL << current_cpu_data.scache.waybit;
+	unsigned long ws_end = current_cpu_data.scache.ways <<
+	                       current_cpu_data.scache.waybit;
+	unsigned long ws, addr;
+
+	for (ws = 0; ws < ws_end; ws += ws_inc) 
+		for (addr = start; addr < end; addr += 0x1000) 
+			cache128_unroll32(addr|ws,Index_Writeback_Inv_SD);
 }
 
 #endif /* __ASM_R4KCACHE_H */