nios2: convert cache flush to use dm cpu data

Convert cache flush to use dm cpu data.

The original cache flush functions are written in assembly
and use CONFIG_SYS_{I,D}CACHE_SIZE... macros. It is difficult
to convert to use cache configuration in dm cpu data which is
extracted from device tree.

The cacheflush.c of Linux nios2 arch uses cpuinfo structure,
which is very close to our dm cpu data. So we copy and modify
it to arch/nios2/lib/cache.c to replace the old cache.S.

Signed-off-by: Thomas Chou <thomas@wytron.com.tw>

3 files changed, 130 insertions, 73 deletions

diff --git a/arch/nios2/lib/bootm.c b/arch/nios2/lib/bootm.c
index c730a3f..4e5c269 100644
--- a/arch/nios2/lib/bootm.c
+++ b/arch/nios2/lib/bootm.c
@@ -6,9 +6,6 @@
  */
 
 #include <common.h>
-#include <command.h>
-#include <asm/byteorder.h>
-#include <asm/cache.h>
 
 #define NIOS_MAGIC 0x534f494e /* enable command line and initrd passing */
 
@@ -40,8 +37,7 @@ int do_bootm_linux(int flag, int argc, char * const argv[], bootm_headers_t *ima
 
 	/* flushes data and instruction caches before calling the kernel */
 	disable_interrupts();
-	flush_dcache((ulong)kernel, CONFIG_SYS_DCACHE_SIZE);
-	flush_icache((ulong)kernel, CONFIG_SYS_ICACHE_SIZE);
+	flush_dcache_all();
 
 	debug("bootargs=%s @ 0x%lx\n", commandline, (ulong)&commandline);
 	debug("initrd=0x%lx-0x%lx\n", (ulong)initrd_start, (ulong)initrd_end);
diff --git a/arch/nios2/lib/cache.S b/arch/nios2/lib/cache.S
deleted file mode 100644
index 683f005..0000000
--- a/arch/nios2/lib/cache.S
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * (C) Copyright 2004, Psyent Corporation <www.psyent.com>
- * Scott McNutt <smcnutt@psyent.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#include <config.h>
-
-	.text
-
-	.global flush_dcache
-
-flush_dcache:
-	add	r5, r5, r4
-	movhi	r8, %hi(CONFIG_SYS_DCACHELINE_SIZE)
-	ori	r8, r8, %lo(CONFIG_SYS_DCACHELINE_SIZE)
-0:	flushd	0(r4)
-	add	r4, r4, r8
-	bltu	r4, r5, 0b
-	ret
-
-
-	.global flush_icache
-
-flush_icache:
-	add	r5, r5, r4
-	movhi	r8, %hi(CONFIG_SYS_ICACHELINE_SIZE)
-	ori	r8, r8, %lo(CONFIG_SYS_ICACHELINE_SIZE)
-1:	flushi	r4
-	add	r4, r4, r8
-	bltu	r4, r5, 1b
-	ret
-
-	.global flush_dcache_range
-
-flush_dcache_range:
-	movhi	r8, %hi(CONFIG_SYS_DCACHELINE_SIZE)
-	ori	r8, r8, %lo(CONFIG_SYS_DCACHELINE_SIZE)
-0:	flushd	0(r4)
-	add	r4, r4, r8
-	bltu	r4, r5, 0b
-	ret
-
-	.global flush_cache
-
-flush_cache:
-	add	r5, r5, r4
-	mov	r9, r4
-	mov	r10, r5
-
-	movhi	r8, %hi(CONFIG_SYS_DCACHELINE_SIZE)
-	ori	r8, r8, %lo(CONFIG_SYS_DCACHELINE_SIZE)
-0:	flushd	0(r4)
-	add	r4, r4, r8
-	bltu	r4, r5, 0b
-
-	mov	r4, r9
-	mov	r5, r10
-	movhi	r8, %hi(CONFIG_SYS_ICACHELINE_SIZE)
-	ori	r8, r8, %lo(CONFIG_SYS_ICACHELINE_SIZE)
-1:	flushi	r4
-	add	r4, r4, r8
-	bltu	r4, r5, 1b
-
-	sync
-	flushp
-	ret
diff --git a/arch/nios2/lib/cache.c b/arch/nios2/lib/cache.c
new file mode 100644
index 0000000..7c74e1a
--- /dev/null
+++ b/arch/nios2/lib/cache.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2015 Thomas Chou <thomas@wytron.com.tw>
+ * Copyright (C) 2009, Wind River Systems Inc
+ * Implemented by fredrik.markstrom@gmail.com and ivarholmqvist@gmail.com
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/cache.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static void __flush_dcache(unsigned long start, unsigned long end)
+{
+	unsigned long addr;
+
+	start &= ~(gd->arch.dcache_line_size - 1);
+	end += (gd->arch.dcache_line_size - 1);
+	end &= ~(gd->arch.dcache_line_size - 1);
+
+	for (addr = start; addr < end; addr += gd->arch.dcache_line_size) {
+		__asm__ __volatile__ ("   flushda 0(%0)\n"
+					: /* Outputs */
+					: /* Inputs  */ "r"(addr)
+					/* : No clobber */);
+	}
+}
+
+static void __flush_dcache_all(unsigned long start, unsigned long end)
+{
+	unsigned long addr;
+
+	start &= ~(gd->arch.dcache_line_size - 1);
+	end += (gd->arch.dcache_line_size - 1);
+	end &= ~(gd->arch.dcache_line_size - 1);
+
+	if (end > start + gd->arch.dcache_size)
+		end = start + gd->arch.dcache_size;
+
+	for (addr = start; addr < end; addr += gd->arch.dcache_line_size) {
+		__asm__ __volatile__ ("   flushd 0(%0)\n"
+					: /* Outputs */
+					: /* Inputs  */ "r"(addr)
+					/* : No clobber */);
+	}
+}
+
+static void __invalidate_dcache(unsigned long start, unsigned long end)
+{
+	unsigned long addr;
+
+	start &= ~(gd->arch.dcache_line_size - 1);
+	end += (gd->arch.dcache_line_size - 1);
+	end &= ~(gd->arch.dcache_line_size - 1);
+
+	for (addr = start; addr < end; addr += gd->arch.dcache_line_size) {
+		__asm__ __volatile__ ("   initda 0(%0)\n"
+					: /* Outputs */
+					: /* Inputs  */ "r"(addr)
+					/* : No clobber */);
+	}
+}
+
+static void __flush_icache(unsigned long start, unsigned long end)
+{
+	unsigned long addr;
+
+	start &= ~(gd->arch.icache_line_size - 1);
+	end += (gd->arch.icache_line_size - 1);
+	end &= ~(gd->arch.icache_line_size - 1);
+
+	if (end > start + gd->arch.icache_size)
+		end = start + gd->arch.icache_size;
+
+	for (addr = start; addr < end; addr += gd->arch.icache_line_size) {
+		__asm__ __volatile__ ("   flushi %0\n"
+					: /* Outputs */
+					: /* Inputs  */ "r"(addr)
+					/* : No clobber */);
+	}
+	__asm__ __volatile(" flushp\n");
+}
+
+void flush_dcache_all(void)
+{
+	__flush_dcache_all(0, gd->arch.dcache_size);
+	__flush_icache(0, gd->arch.icache_size);
+}
+
+void flush_dcache_range(unsigned long start, unsigned long end)
+{
+	if (gd->arch.has_initda)
+		__flush_dcache(start, end);
+	else
+		__flush_dcache_all(start, end);
+}
+
+void flush_cache(unsigned long start, unsigned long size)
+{
+	if (gd->arch.has_initda)
+		__flush_dcache(start, start + size);
+	else
+		__flush_dcache_all(start, start + size);
+	__flush_icache(start, start + size);
+}
+
+void invalidate_dcache_range(unsigned long start, unsigned long end)
+{
+	if (gd->arch.has_initda)
+		__invalidate_dcache(start, end);
+	else
+		__flush_dcache_all(start, end);
+}
+
+int dcache_status(void)
+{
+	return 1;
+}
+
+void dcache_enable(void)
+{
+	flush_dcache_all();
+}
+
+void dcache_disable(void)
+{
+	flush_dcache_all();
+}