arch/x86/include/asm/io.h |   6 ++
 include/linux/io.h        |   2 +
 lib/iomap_copy.c          | 153 ++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 161 insertions(+)

diff --git a/arch/x86/include/asm/io.h b/arch/x86/include/asm/io.h
index 832da8229cc7..3b9206ee25b8 100644
--- a/arch/x86/include/asm/io.h
+++ b/arch/x86/include/asm/io.h
@@ -92,6 +92,12 @@ build_mmio_write(__writel, "l", unsigned int, "r", )
 
 #define mmiowb() barrier()
 
+void __iowrite_copy(void __iomem *to, const void *from, size_t count);
+void __ioread_copy(void *to, const void __iomem *from, size_t count);
+
+#define memcpy_toio __iowrite_copy
+#define memcpy_fromio __ioread_copy
+
 #ifdef CONFIG_X86_64
 
 build_mmio_read(readq, "q", u64, "=r", :"memory")
diff --git a/include/linux/io.h b/include/linux/io.h
index 32e30e8fb9db..642f78970018 100644
--- a/include/linux/io.h
+++ b/include/linux/io.h
@@ -28,6 +28,8 @@
 struct device;
 struct resource;
 
+void __ioread_copy(void *to, const void __iomem *from, size_t count);
+void __iowrite_copy(void __iomem *to, const void *from, size_t count);
 __visible void __iowrite32_copy(void __iomem *to, const void *from, size_t count);
 void __ioread32_copy(void *to, const void __iomem *from, size_t count);
 void __iowrite64_copy(void __iomem *to, const void *from, size_t count);
diff --git a/lib/iomap_copy.c b/lib/iomap_copy.c
index b8f1d6cbb200..8edc359dda62 100644
--- a/lib/iomap_copy.c
+++ b/lib/iomap_copy.c
@@ -17,6 +17,159 @@
 
 #include <linux/export.h>
 #include <linux/io.h>
+#include <asm/unaligned.h>
+
+static inline bool iomem_align(const void __iomem *ptr, int size, int count)
+{
+	return count >= size && (__force unsigned long)ptr & size;
+}
+
+
+/**
+ * __iowrite_copy - copy data to MMIO space
+ * @to: destination, in MMIO space
+ * @from: source
+ * @count: number of bytes to copy.
+ *
+ * Copy arbitrarily aligned data from kernel space to MMIO space,
+ * using reasonable chunking.
+ */
+void __attribute__((weak)) __iowrite_copy(void __iomem *to,
+					  const void *from,
+					  size_t count)
+{
+	if (iomem_align(to, 1, count)) {
+		unsigned char data = *(unsigned char *)from;
+		__raw_writeb(data, to);
+		from++;
+		to++;
+		count--;
+	}
+	if (iomem_align(to, 2, count)) {
+		unsigned short data = get_unaligned((unsigned short *)from);
+		__raw_writew(data, to);
+		from += 2;
+		to += 2;
+		count -= 2;
+	}
+#ifdef CONFIG_64BIT
+	if (iomem_align(to, 4, count)) {
+		unsigned int data = get_unaligned((unsigned int *)from);
+		__raw_writel(data, to);
+		from += 4;
+		to += 4;
+		count -= 4;
+	}
+#endif
+	while (count >= sizeof(unsigned long)) {
+		unsigned long data = get_unaligned((unsigned long *)from);
+#ifdef CONFIG_64BIT
+		__raw_writeq(data, to);
+#else
+		__raw_writel(data, to);
+#endif
+		from += sizeof(unsigned long);
+		to += sizeof(unsigned long);
+		count -= sizeof(unsigned long);
+	}
+
+#ifdef CONFIG_64BIT
+	if (count >= 4) {
+		unsigned int data = get_unaligned((unsigned int *)from);
+		__raw_writel(data, to);
+		from += 4;
+		to += 4;
+		count -= 4;
+	}
+#endif
+
+	if (count >= 2) {
+		unsigned short data = get_unaligned((unsigned short *)from);
+		__raw_writew(data, to);
+		from += 2;
+		to += 2;
+		count -= 2;
+	}
+
+	if (count) {
+		unsigned char data = *(unsigned char *)from;
+		__raw_writeb(data, to);
+	}
+}
+EXPORT_SYMBOL_GPL(__iowrite_copy);
+
+/**
+ * __ioread_copy - copy data from MMIO space
+ * @to: destination
+ * @from: source, in MMIO space
+ * @count: number of bytes to copy.
+ *
+ * Copy arbitrarily aligned data from MMIO space to kernel space,
+ * using reasonable chunking.
+ */
+void __attribute__((weak)) __ioread_copy(void *to,
+					 const void __iomem *from,
+					 size_t count)
+{
+	if (iomem_align(from, 1, count)) {
+		unsigned char data = __raw_readb(from);
+		put_unaligned(data, (unsigned char *) to);
+		from++;
+		to++;
+		count--;
+	}
+	if (iomem_align(to, 2, count)) {
+		unsigned short data = __raw_readw(from);
+		put_unaligned(data, (unsigned short *) to);
+		from += 2;
+		to += 2;
+		count -= 2;
+	}
+#ifdef CONFIG_64BIT
+	if (iomem_align(to, 4, count)) {
+		unsigned int data = __raw_readl(from);
+		put_unaligned(data, (unsigned int *) to);
+		from += 4;
+		to += 4;
+		count -= 4;
+	}
+#endif
+	while (count >= sizeof(unsigned long)) {
+#ifdef CONFIG_64BIT
+		unsigned long data = __raw_readq(from);
+#else
+		unsigned long data = __raw_readl(from);
+#endif
+		put_unaligned(data, (unsigned long *) to);
+		from += sizeof(unsigned long);
+		to += sizeof(unsigned long);
+		count -= sizeof(unsigned long);
+	}
+
+#ifdef CONFIG_64BIT
+	if (count >= 4) {
+		unsigned int data = __raw_readl(from);
+		put_unaligned(data, (unsigned int *) to);
+		from += 4;
+		to += 4;
+		count -= 4;
+	}
+#endif
+
+	if (count >= 2) {
+		unsigned short data = __raw_readw(from);
+		put_unaligned(data, (unsigned short *) to);
+		from += 2;
+		to += 2;
+		count -= 2;
+	}
+
+	if (count) {
+		unsigned char data = __raw_readb(from);
+		put_unaligned(data, (unsigned char *) to);
+	}
+}
+EXPORT_SYMBOL_GPL(__ioread_copy);
 
 /**
  * __iowrite32_copy - copy data to MMIO space, in 32-bit units