1 files changed, 74 insertions, 4 deletions

diff --git a/drivers/video/vfb.c b/drivers/video/vfb.c
index 02cfc6a45b0d..072638a9528a 100644
--- a/drivers/video/vfb.c
+++ b/drivers/video/vfb.c
@@ -37,6 +37,48 @@ static void *videomemory;
 static u_long videomemorysize = VIDEOMEMSIZE;
 module_param(videomemorysize, ulong, 0);
 
+/**********************************************************************
+ *
+ * Memory management
+ *
+ **********************************************************************/
+static void *rvmalloc(unsigned long size)
+{
+        void *mem;
+        unsigned long adr;
+
+        size = PAGE_ALIGN(size);
+        mem = vmalloc_32(size);
+        if (!mem)
+                return NULL;
+
+        memset(mem, 0, size); /* Clear the ram out, no junk to the user */
+        adr = (unsigned long) mem;
+        while (size > 0) {
+                SetPageReserved(vmalloc_to_page((void *)adr));
+                adr += PAGE_SIZE;
+                size -= PAGE_SIZE;
+        }
+
+        return mem;
+}
+
+static void rvfree(void *mem, unsigned long size)
+{
+        unsigned long adr;
+
+        if (!mem)
+                return;
+
+        adr = (unsigned long) mem;
+        while ((long) size > 0) {
+                ClearPageReserved(vmalloc_to_page((void *)adr));
+                adr += PAGE_SIZE;
+                size -= PAGE_SIZE;
+        }
+        vfree(mem);
+}
+
 static struct fb_var_screeninfo vfb_default __initdata = {
         .xres =         640,
         .yres =         480,
@@ -371,7 +413,33 @@ static int vfb_pan_display(struct fb_var_screeninfo *var,
 static int vfb_mmap(struct fb_info *info,
                     struct vm_area_struct *vma)
 {
-        return -EINVAL;
+        unsigned long start = vma->vm_start;
+        unsigned long size = vma->vm_end - vma->vm_start;
+        unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+        unsigned long page, pos;
+
+        if (offset + size > info->fix.smem_len) {
+                return -EINVAL;
+        }
+
+        pos = (unsigned long)info->fix.smem_start + offset;
+
+        while (size > 0) {
+                page = vmalloc_to_pfn((void *)pos);
+                if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED)) {
+                        return -EAGAIN;
+                }
+                start += PAGE_SIZE;
+                pos += PAGE_SIZE;
+                if (size > PAGE_SIZE)
+                        size -= PAGE_SIZE;
+                else
+                        size = 0;
+        }
+
+        vma->vm_flags |= VM_RESERVED;   /* avoid to swap out this VMA */
+        return 0;
+
 }
 
 #ifndef MODULE
@@ -406,7 +474,7 @@ static int __init vfb_probe(struct platform_device *dev)
         /*
          * For real video cards we use ioremap.
          */
-        if (!(videomemory = vmalloc(videomemorysize)))
+        if (!(videomemory = rvmalloc(videomemorysize)))
                 return retval;
 
         /*
@@ -429,6 +497,8 @@ static int __init vfb_probe(struct platform_device *dev)
 
         if (!retval || (retval == 4))
                 info->var = vfb_default;
+        vfb_fix.smem_start = (unsigned long) videomemory;
+        vfb_fix.smem_len = videomemorysize;
         info->fix = vfb_fix;
         info->pseudo_palette = info->par;
         info->par = NULL;
@@ -452,7 +522,7 @@ err2:
 err1:
         framebuffer_release(info);
 err:
-        vfree(videomemory);
+        rvfree(videomemory, videomemorysize);
         return retval;
 }
 
@@ -462,7 +532,7 @@ static int vfb_remove(struct platform_device *dev)
 
         if (info) {
                 unregister_framebuffer(info);
-                vfree(videomemory);
+                rvfree(videomemory, videomemorysize);
                 framebuffer_release(info);
         }
         return 0;