2 files changed, 69 insertions, 5 deletions

diff --git a/arch/s390/include/asm/io.h b/arch/s390/include/asm/io.h
index b7ff6afc3caa..27216d317991 100644
--- a/arch/s390/include/asm/io.h
+++ b/arch/s390/include/asm/io.h
@@ -38,11 +38,8 @@ static inline void * phys_to_virt(unsigned long address)
         return (void *) address;
 }
 
-/*
- * Convert a physical pointer to a virtual kernel pointer for /dev/mem
- * access
- */
-#define xlate_dev_mem_ptr(p)    __va(p)
+void *xlate_dev_mem_ptr(unsigned long phys);
+void unxlate_dev_mem_ptr(unsigned long phys, void *addr);
 
 /*
  * Convert a virtual cached pointer to an uncached pointer
diff --git a/arch/s390/mm/maccess.c b/arch/s390/mm/maccess.c
index e1335dc2b1b7..795a0a9bb2eb 100644
--- a/arch/s390/mm/maccess.c
+++ b/arch/s390/mm/maccess.c
@@ -12,6 +12,7 @@
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/gfp.h>
+#include <linux/cpu.h>
 #include <asm/ctl_reg.h>
 
 /*
@@ -166,3 +167,69 @@ out:
         free_page((unsigned long) buf);
         return rc;
 }
+
+/*
+ * Check if physical address is within prefix or zero page
+ */
+static int is_swapped(unsigned long addr)
+{
+        unsigned long lc;
+        int cpu;
+
+        if (addr < sizeof(struct _lowcore))
+                return 1;
+        for_each_online_cpu(cpu) {
+                lc = (unsigned long) lowcore_ptr[cpu];
+                if (addr > lc + sizeof(struct _lowcore) - 1 || addr < lc)
+                        continue;
+                return 1;
+        }
+        return 0;
+}
+
+/*
+ * Return swapped prefix or zero page address
+ */
+static unsigned long get_swapped(unsigned long addr)
+{
+        unsigned long prefix = store_prefix();
+
+        if (addr < sizeof(struct _lowcore))
+                return addr + prefix;
+        if (addr >= prefix && addr < prefix + sizeof(struct _lowcore))
+                return addr - prefix;
+        return addr;
+}
+
+/*
+ * Convert a physical pointer for /dev/mem access
+ *
+ * For swapped prefix pages a new buffer is returned that contains a copy of
+ * the absolute memory. The buffer size is maximum one page large.
+ */
+void *xlate_dev_mem_ptr(unsigned long addr)
+{
+        void *bounce = (void *) addr;
+        unsigned long size;
+
+        get_online_cpus();
+        preempt_disable();
+        if (is_swapped(addr)) {
+                size = PAGE_SIZE - (addr & ~PAGE_MASK);
+                bounce = (void *) __get_free_page(GFP_ATOMIC);
+                if (bounce)
+                        memcpy_real(bounce, (void *) get_swapped(addr), size);
+        }
+        preempt_enable();
+        put_online_cpus();
+        return bounce;
+}
+
+/*
+ * Free converted buffer for /dev/mem access (if necessary)
+ */
+void unxlate_dev_mem_ptr(unsigned long addr, void *buf)
+{
+        if ((void *) addr != buf)
+                free_page((unsigned long) buf);
+}