1 files changed, 12 insertions, 15 deletions

diff --git a/drivers/hid/hid-picolcd.c b/drivers/hid/hid-picolcd.c
index dc19501a786f..f7204541591c 100644
--- a/drivers/hid/hid-picolcd.c
+++ b/drivers/hid/hid-picolcd.c
@@ -563,19 +563,18 @@ static int picolcd_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *i
 static int picolcd_set_par(struct fb_info *info)
 {
         struct picolcd_data *data = info->par;
-        u8 *o_fb, *n_fb;
+        u8 *tmp_fb, *o_fb;
         if (info->var.bits_per_pixel == data->fb_bpp)
                 return 0;
         /* switch between 1/8 bit depths */
         if (info->var.bits_per_pixel != 1 && info->var.bits_per_pixel != 8)
                 return -EINVAL;
 
-        o_fb = data->fb_bitmap;
-        n_fb = vmalloc(PICOLCDFB_SIZE*info->var.bits_per_pixel);
-        if (!n_fb)
+        o_fb   = data->fb_bitmap;
+        tmp_fb = kmalloc(PICOLCDFB_SIZE*info->var.bits_per_pixel, GFP_KERNEL);
+        if (!tmp_fb)
                 return -ENOMEM;
 
-        fb_deferred_io_cleanup(info);
         /* translate FB content to new bits-per-pixel */
         if (info->var.bits_per_pixel == 1) {
                 int i, b;
@@ -585,24 +584,22 @@ static int picolcd_set_par(struct fb_info *info)
                                 p <<= 1;
                                 p |= o_fb[i*8+b] ? 0x01 : 0x00;
                         }
+                        tmp_fb[i] = p;
                 }
+                memcpy(o_fb, tmp_fb, PICOLCDFB_SIZE);
                 info->fix.visual = FB_VISUAL_MONO01;
                 info->fix.line_length = PICOLCDFB_WIDTH / 8;
         } else {
                 int i;
+                memcpy(tmp_fb, o_fb, PICOLCDFB_SIZE);
                 for (i = 0; i < PICOLCDFB_SIZE * 8; i++)
-                        n_fb[i] = o_fb[i/8] & (0x01 << (7 - i % 8)) ? 0xff : 0x00;
-                info->fix.visual = FB_VISUAL_TRUECOLOR;
+                        o_fb[i] = tmp_fb[i/8] & (0x01 << (7 - i % 8)) ? 0xff : 0x00;
+                info->fix.visual = FB_VISUAL_DIRECTCOLOR;
                 info->fix.line_length = PICOLCDFB_WIDTH;
         }
 
-        data->fb_bitmap   = n_fb;
+        kfree(tmp_fb);
         data->fb_bpp      = info->var.bits_per_pixel;
-        info->screen_base = (char __force __iomem *)n_fb;
-        info->fix.smem_start = (unsigned long)n_fb;
-        info->fix.smem_len   = PICOLCDFB_SIZE*data->fb_bpp;
-        fb_deferred_io_init(info);
-        vfree(o_fb);
         return 0;
 }
 
@@ -692,7 +689,7 @@ static int picolcd_init_framebuffer(struct picolcd_data *data)
         u8 *fb_bitmap  = NULL;
         u32 *palette;
 
-        fb_bitmap = vmalloc(PICOLCDFB_SIZE*picolcdfb_var.bits_per_pixel);
+        fb_bitmap = vmalloc(PICOLCDFB_SIZE*8);
         if (fb_bitmap == NULL) {
                 dev_err(dev, "can't get a free page for framebuffer\n");
                 goto err_nomem;
@@ -728,7 +725,7 @@ static int picolcd_init_framebuffer(struct picolcd_data *data)
         info->fbops = &picolcdfb_ops;
         info->var = picolcdfb_var;
         info->fix = picolcdfb_fix;
-        info->fix.smem_len   = PICOLCDFB_SIZE;
+        info->fix.smem_len   = PICOLCDFB_SIZE*8;
         info->fix.smem_start = (unsigned long)fb_bitmap;
         info->par = data;
         info->flags = FBINFO_FLAG_DEFAULT;