1 files changed, 22 insertions, 5 deletions

diff --git a/miscutils/fbsplash.c b/miscutils/fbsplash.c
index 988439b25..37ca66559 100644
--- a/miscutils/fbsplash.c
+++ b/miscutils/fbsplash.c
@@ -50,6 +50,10 @@ struct globals {
         struct fb_var_screeninfo scr_var;
         struct fb_fix_screeninfo scr_fix;
         unsigned bytes_per_pixel;
+        // cached (8 - scr_var.COLOR.length):
+        unsigned red_shift;
+        unsigned green_shift;
+        unsigned blue_shift;
 };
 #define G (*ptr_to_globals)
 #define INIT_G() do { \
@@ -139,6 +143,9 @@ static void fb_open(const char *strfb_device)
                 break;
         }
 
+        G.red_shift   = 8 - G.scr_var.red.length;
+        G.green_shift = 8 - G.scr_var.green.length;
+        G.blue_shift  = 8 - G.scr_var.blue.length;
         G.bytes_per_pixel = (G.scr_var.bits_per_pixel + 7) >> 3;
 
         // map the device in memory
@@ -155,10 +162,13 @@ static void fb_open(const char *strfb_device)
 
 
 /**
- * Return pixel value of the passed RGB color
+ * Return pixel value of the passed RGB color.
+ * This is performance critical fn.
  */
 static unsigned fb_pixel_value(unsigned r, unsigned g, unsigned b)
 {
+        /* We assume that the r,g,b values are <= 255 */
+
         if (G.bytes_per_pixel == 1) {
                 r = r        & 0xe0; // 3-bit red
                 g = (g >> 3) & 0x1c; // 3-bit green
@@ -166,10 +176,17 @@ static unsigned fb_pixel_value(unsigned r, unsigned g, unsigned b)
                 return r + g + b;
         }
         if (G.bytes_per_pixel == 2) {
-                r = (r & 0xf8) << 8; // 5-bit red
-                g = (g & 0xfc) << 3; // 6-bit green
-                b =  b >> 3;         // 5-bit blue
-                return r + g + b;
+                // ARM PL110 on Integrator/CP has RGBA5551 bit arrangement.
+                // We want to support bit locations like that.
+                //
+                // First shift out unused bits
+                r = r >> G.red_shift;
+                g = g >> G.green_shift;
+                b = b >> G.blue_shift;
+                // Then shift the remaining bits to their offset
+                return (r << G.scr_var.red.offset) +
+                        (g << G.scr_var.green.offset) +
+                        (b << G.scr_var.blue.offset);
         }
         // RGB 888
         return b + (g << 8) + (r << 16);