[U-Boot] [PATCH] arm64: Add memcpy_{from, to}io() and memset_io() helpers

[Vignesh Raghavendra]

Provide optimized memcpy_{from,to}io() and memset_io(). This is required
when moving large amount of data to and from IO regions such as IP
registers or accessing memory mapped flashes.

Code is borrowed from Linux Kernel v5.4.

Signed-off-by: Vignesh Raghavendra <vigneshr at ti.com>
---
 arch/arm/include/asm/io.h | 104 ++++++++++++++++++++++++++++++++++++--
 1 file changed, 101 insertions(+), 3 deletions(-)

diff --git a/arch/arm/include/asm/io.h b/arch/arm/include/asm/io.h
index e6d27b69f936..fcfb53f0d9d8 100644
--- a/arch/arm/include/asm/io.h
+++ b/arch/arm/include/asm/io.h
@@ -23,6 +23,7 @@
 #ifdef __KERNEL__
 
 #include <linux/types.h>
+#include <linux/kernel.h>
 #include <asm/byteorder.h>
 #include <asm/memory.h>
 #include <asm/barriers.h>
@@ -315,6 +316,95 @@ extern void _memset_io(unsigned long, int, size_t);
 
 extern void __readwrite_bug(const char *fn);
 
+/* Optimized copy functions to read from/write to IO sapce */
+#ifdef CONFIG_ARM64
+/*
+ * Copy data from IO memory space to "real" memory space.
+ */
+static inline
+void __memcpy_fromio(void *to, const volatile void __iomem *from, size_t count)
+{
+	while (count && !IS_ALIGNED((unsigned long)from, 8)) {
+		*(u8 *)to = __raw_readb(from);
+		from++;
+		to++;
+		count--;
+	}
+
+	while (count >= 8) {
+		*(u64 *)to = __raw_readq(from);
+		from += 8;
+		to += 8;
+		count -= 8;
+	}
+
+	while (count) {
+		*(u8 *)to = __raw_readb(from);
+		from++;
+		to++;
+		count--;
+	}
+}
+
+/*
+ * Copy data from "real" memory space to IO memory space.
+ */
+static inline
+void __memcpy_toio(volatile void __iomem *to, const void *from, size_t count)
+{
+	while (count && !IS_ALIGNED((unsigned long)to, 8)) {
+		__raw_writeb(*(u8 *)from, to);
+		from++;
+		to++;
+		count--;
+	}
+
+	while (count >= 8) {
+		__raw_writeq(*(u64 *)from, to);
+		from += 8;
+		to += 8;
+		count -= 8;
+	}
+
+	while (count) {
+		__raw_writeb(*(u8 *)from, to);
+		from++;
+		to++;
+		count--;
+	}
+}
+
+/*
+ * "memset" on IO memory space.
+ */
+static inline
+void __memset_io(volatile void __iomem *dst, int c, size_t count)
+{
+	u64 qc = (u8)c;
+
+	qc |= qc << 8;
+	qc |= qc << 16;
+	qc |= qc << 32;
+
+	while (count && !IS_ALIGNED((unsigned long)dst, 8)) {
+		__raw_writeb(c, dst);
+		dst++;
+		count--;
+	}
+
+	while (count >= 8) {
+		__raw_writeq(qc, dst);
+		dst += 8;
+		count -= 8;
+	}
+
+	while (count) {
+		__raw_writeb(c, dst);
+		dst++;
+		count--;
+	}
+}
+#endif /* CONFIG_ARM64 */
 /*
  * If this architecture has PCI memory IO, then define the read/write
  * macros.  These should only be used with the cookie passed from
@@ -355,9 +445,17 @@ out:
 }
 
 #else
-#define memset_io(a, b, c)		memset((void *)(a), (b), (c))
-#define memcpy_fromio(a, b, c)		memcpy((a), (void *)(b), (c))
-#define memcpy_toio(a, b, c)		memcpy((void *)(a), (b), (c))
+
+#ifdef CONFIG_ARM64
+#define memset_io(a, b, c)		__memset_io((a), (b), (c))
+#define memcpy_fromio(a, b, c)		__memcpy_fromio((a), (b), (c))
+#define memcpy_toio(a, b, c)		__memcpy_toio((a), (b), (c))
+#else
+#define memset_io(a, b, c)             memset((void *)(a), (b), (c))
+#define memcpy_fromio(a, b, c)         memcpy((a), (void *)(b), (c))
+#define memcpy_toio(a, b, c)           memcpy((void *)(a), (b), (c))
+#endif
+
 
 #if !defined(readb)
 
-- 
2.23.0