3 files changed, 109 insertions, 66 deletions

diff --git a/drivers/video/console/bitblit.c b/drivers/video/console/bitblit.c
index 6550875ef9c5..9f70e512b88b 100644
--- a/drivers/video/console/bitblit.c
+++ b/drivers/video/console/bitblit.c
@@ -103,42 +103,104 @@ static void bit_clear(struct vc_data *vc, struct fb_info *info, int sy,
         info->fbops->fb_fillrect(info, &region);
 }
 
+static inline void bit_putcs_aligned(struct vc_data *vc, struct fb_info *info,
+                                     const u16 *s, u32 attr, u32 cnt,
+                                     u32 d_pitch, u32 s_pitch, u32 cellsize,
+                                     struct fb_image *image, u8 *buf, u8 *dst)
+{
+        u16 charmask = vc->vc_hi_font_mask ? 0x1ff : 0xff;
+        u32 idx = vc->vc_font.width >> 3;
+        u8 *src;
+
+        while (cnt--) {
+                src = vc->vc_font.data + (scr_readw(s++)&
+                                          charmask)*cellsize;
+
+                if (attr) {
+                        update_attr(buf, src, attr, vc);
+                        src = buf;
+                }
+
+                if (likely(idx == 1))
+                        __fb_pad_aligned_buffer(dst, d_pitch, src, idx,
+                                                image->height);
+                else
+                        fb_pad_aligned_buffer(dst, d_pitch, src, idx,
+                                              image->height);
+
+                dst += s_pitch;
+        }
+
+        info->fbops->fb_imageblit(info, image);
+}
+
+static inline void bit_putcs_unaligned(struct vc_data *vc,
+                                       struct fb_info *info, const u16 *s,
+                                       u32 attr, u32 cnt, u32 d_pitch,
+                                       u32 s_pitch, u32 cellsize,
+                                       struct fb_image *image, u8 *buf,
+                                       u8 *dst)
+{
+        u16 charmask = vc->vc_hi_font_mask ? 0x1ff : 0xff;
+        u32 shift_low = 0, mod = vc->vc_font.width % 8;
+        u32 shift_high = 8;
+        u32 idx = vc->vc_font.width >> 3;
+        u8 *src;
+
+        while (cnt--) {
+                src = vc->vc_font.data + (scr_readw(s++)&
+                                          charmask)*cellsize;
+
+                if (attr) {
+                        update_attr(buf, src, attr, vc);
+                        src = buf;
+                }
+
+                fb_pad_unaligned_buffer(dst, d_pitch, src, idx,
+                                        image->height, shift_high,
+                                        shift_low, mod);
+                shift_low += mod;
+                dst += (shift_low >= 8) ? s_pitch : s_pitch - 1;
+                shift_low &= 7;
+                shift_high = 8 - shift_low;
+        }
+
+        info->fbops->fb_imageblit(info, image);
+
+}
+
 static void bit_putcs(struct vc_data *vc, struct fb_info *info,
                       const unsigned short *s, int count, int yy, int xx,
                       int fg, int bg)
 {
-        unsigned short charmask = vc->vc_hi_font_mask ? 0x1ff : 0xff;
-        unsigned int width = (vc->vc_font.width + 7) >> 3;
-        unsigned int cellsize = vc->vc_font.height * width;
-        unsigned int maxcnt = info->pixmap.size/cellsize;
-        unsigned int scan_align = info->pixmap.scan_align - 1;
-        unsigned int buf_align = info->pixmap.buf_align - 1;
-        unsigned int shift_low = 0, mod = vc->vc_font.width % 8;
-        unsigned int shift_high = 8, pitch, cnt, size, i, k;
-        unsigned int idx = vc->vc_font.width >> 3;
-        unsigned int attribute = get_attribute(info, scr_readw(s));
         struct fb_image image;
-        u8 *src, *dst, *buf = NULL;
-
-        if (attribute) {
-                buf = kmalloc(cellsize, GFP_KERNEL);
-                if (!buf)
-                        return;
-        }
+        u32 width = (vc->vc_font.width + 7)/8;
+        u32 cellsize = width * vc->vc_font.height;
+        u32 maxcnt = info->pixmap.size/cellsize;
+        u32 scan_align = info->pixmap.scan_align - 1;
+        u32 buf_align = info->pixmap.buf_align - 1;
+        u32 mod = vc->vc_font.width % 8, cnt, pitch, size;
+        u32 attribute = get_attribute(info, scr_readw(s));
+        u8 *dst, *buf = NULL;
 
         image.fg_color = fg;
         image.bg_color = bg;
-
         image.dx = xx * vc->vc_font.width;
         image.dy = yy * vc->vc_font.height;
         image.height = vc->vc_font.height;
         image.depth = 1;
 
+        if (attribute) {
+                buf = kmalloc(cellsize, GFP_KERNEL);
+                if (!buf)
+                        return;
+        }
+
         while (count) {
                 if (count > maxcnt)
-                        cnt = k = maxcnt;
+                        cnt = maxcnt;
                 else
-                        cnt = k = count;
+                        cnt = count;
 
                 image.width = vc->vc_font.width * cnt;
                 pitch = ((image.width + 7) >> 3) + scan_align;
@@ -147,45 +209,18 @@ static void bit_putcs(struct vc_data *vc, struct fb_info *info,
                 size &= ~buf_align;
                 dst = fb_get_buffer_offset(info, &info->pixmap, size);
                 image.data = dst;
-                if (mod) {
-                        while (k--) {
-                                src = vc->vc_font.data + (scr_readw(s++)&
-                                                          charmask)*cellsize;
-
-                                if (attribute) {
-                                        update_attr(buf, src, attribute, vc);
-                                        src = buf;
-                                }
-
-                                fb_pad_unaligned_buffer(dst, pitch, src, idx,
-                                                image.height, shift_high,
-                                                shift_low, mod);
-                                shift_low += mod;
-                                dst += (shift_low >= 8) ? width : width - 1;
-                                shift_low &= 7;
-                                shift_high = 8 - shift_low;
-                        }
-                } else {
-                        while (k--) {
-                                src = vc->vc_font.data + (scr_readw(s++)&
-                                                          charmask)*cellsize;
-
-                                if (attribute) {
-                                        update_attr(buf, src, attribute, vc);
-                                        src = buf;
-                                }
-
-                                if (idx == 1)
-                                        for(i=0; i < image.height; i++)
-                                                dst[pitch*i] = src[i];
-                                else
-                                        fb_pad_aligned_buffer(dst, pitch, src, idx, image.height);
-                                dst += width;
-                        }
-                }
-                info->fbops->fb_imageblit(info, &image);
+
+                if (!mod)
+                        bit_putcs_aligned(vc, info, s, attribute, cnt, pitch,
+                                          width, cellsize, &image, buf, dst);
+                else
+                        bit_putcs_unaligned(vc, info, s, attribute, cnt,
+                                            pitch, width, cellsize, &image,
+                                            buf, dst);
+
                 image.dx += cnt * vc->vc_font.width;
                 count -= cnt;
+                s += cnt;
         }
 
         /* buf is always NULL except when in monochrome mode, so in this case
@@ -193,6 +228,7 @@ static void bit_putcs(struct vc_data *vc, struct fb_info *info,
            NULL pointers just fine */
         if (unlikely(buf))
                 kfree(buf);
+
 }
 
 static void bit_clear_margins(struct vc_data *vc, struct fb_info *info,
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 71b55070bdb9..70be7009f8af 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -90,15 +90,7 @@ EXPORT_SYMBOL(fb_get_color_depth);
  */
 void fb_pad_aligned_buffer(u8 *dst, u32 d_pitch, u8 *src, u32 s_pitch, u32 height)
 {
-        int i, j;
-
-        for (i = height; i--; ) {
-                /* s_pitch is a few bytes at the most, memcpy is suboptimal */
-                for (j = 0; j < s_pitch; j++)
-                        dst[j] = src[j];
-                src += s_pitch;
-                dst += d_pitch;
-        }
+        __fb_pad_aligned_buffer(dst, d_pitch, src, s_pitch, height);
 }
 EXPORT_SYMBOL(fb_pad_aligned_buffer);
 
diff --git a/include/linux/fb.h b/include/linux/fb.h
index 9a4f035e9fdc..82e39cd0c4fb 100644
--- a/include/linux/fb.h
+++ b/include/linux/fb.h
@@ -833,6 +833,21 @@ extern int fb_new_modelist(struct fb_info *info);
 extern struct fb_info *registered_fb[FB_MAX];
 extern int num_registered_fb;
 
+static inline void __fb_pad_aligned_buffer(u8 *dst, u32 d_pitch,
+                                           u8 *src, u32 s_pitch, u32 height)
+{
+        int i, j;
+
+        d_pitch -= s_pitch;
+
+        for (i = height; i--; ) {
+                /* s_pitch is a few bytes at the most, memcpy is suboptimal */
+                for (j = 0; j < s_pitch; j++)
+                        *dst++ = *src++;
+                dst += d_pitch;
+        }
+}
+
 /* drivers/video/fbsysfs.c */
 extern struct fb_info *framebuffer_alloc(size_t size, struct device *dev);
 extern void framebuffer_release(struct fb_info *info);