[U-Boot] [[Patch V2] mips: 04/16] add cpu/mips64/cpu.c

[Zhizhou Zhang <etou.zh@gmail.com>]

add icache and dcache probe here. but scache was not probed. For
I don't know how to determine it implemented or not.
Signed-off-by: Zhizhou Zhang <etou.zh@gmail.com>
---
 arch/mips/cpu/mips64/cpu.c |  133 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 133 insertions(+)
 create mode 100644 arch/mips/cpu/mips64/cpu.c

diff --git a/arch/mips/cpu/mips64/cpu.c b/arch/mips/cpu/mips64/cpu.c
new file mode 100644
index 0000000..ea9c28a
--- /dev/null
+++ b/arch/mips/cpu/mips64/cpu.c
@@ -0,0 +1,133 @@
+/*
+ * (C) Copyright 2003
+ * Wolfgang Denk, DENX Software Engineering, <wd@denx.de>
+ * Zhi-zhou Zhang <etou.zh@gmail.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <command.h>
+#include <netdev.h>
+#include <asm/mipsregs.h>
+#include <asm/cacheops.h>
+#include <asm/reboot.h>
+
+#define cache_op(op,addr)						\
+	__asm__ __volatile__(						\
+	"	.set	push					\n"	\
+	"	.set	noreorder				\n"	\
+	"	.set	mips64\n\t				\n"	\
+	"	cache	%0, %1					\n"	\
+	"	.set	pop					\n"	\
+	:								\
+	: "i" (op), "R" (*(unsigned char *)(addr)))
+
+void __attribute__((weak)) _machine_restart(void)
+{
+}
+
+int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	_machine_restart();
+
+	fprintf(stderr, "*** reset failed ***\n");
+	return 0;
+}
+
+static struct cache_desc icache, dcache;
+
+void cache_probe()
+{
+	int config, lsize;
+
+	config = read_c0_config1();
+	lsize = (config >> 19) & 7;
+	if (lsize) { /* icache present */
+		icache.linesz = 2 << lsize;
+		icache.sets = 32 << (((config >> 22) + 1) & 7);
+		icache.ways = 1 + ((config >> 16) & 7);
+		icache.size = icache.sets *
+		icache.ways *
+		icache.linesz;
+	}
+
+	lsize = (config >> 10) & 7;
+	if (lsize) { /* dcache present */
+		dcache.linesz = 2 << lsize;
+		dcache.sets = 32 << (((config >> 13) + 1) & 7);
+		dcache.ways = 1 + ((config >> 7) & 7);
+		dcache.size = dcache.sets *
+		dcache.ways *
+		dcache.linesz;
+	}
+} 
+
+void flush_cache(ulong start_addr, ulong size)
+{
+	unsigned long addr, aend;
+
+	/* aend will be miscalculated when size is zero, so we return here */
+	if (size == 0)
+		return;
+
+	addr = start_addr & ~(icache.linesz - 1);
+	aend = (start_addr + size - 1) & ~(icache.linesz - 1);
+	while (1) {
+		cache_op(Hit_Invalidate_I, addr);
+		if (addr == aend)
+			break;
+		addr += icache.linesz;
+	}
+
+	addr = start_addr & ~(dcache.linesz - 1);
+	aend = (start_addr + size - 1) & ~(dcache.linesz - 1);
+	while (1) {
+		cache_op(Hit_Writeback_Inv_D, addr);
+		if (addr == aend)
+			break;
+		addr += dcache.linesz;
+	}
+}
+
+void flush_dcache_range(ulong start_addr, ulong stop)
+{
+	unsigned long addr = start_addr & ~(dcache.linesz - 1);
+	unsigned long aend = (stop - 1) & ~(dcache.linesz - 1);
+
+	while (1) {
+		cache_op(Hit_Writeback_Inv_D, addr);
+		if (addr == aend)
+			break;
+		addr += dcache.linesz;
+	}
+}
+
+void invalidate_dcache_range(ulong start_addr, ulong stop)
+{
+	unsigned long addr = start_addr & ~(dcache.linesz - 1);
+	unsigned long aend = (stop - 1) & ~(dcache.linesz - 1);
+
+	while (1) {
+		cache_op(Hit_Invalidate_D, addr);
+		if (addr == aend)
+			break;
+		addr += dcache.linesz;
+	}
+}
-- 
1.7.9.5