11 files changed, 251 insertions, 48 deletions

diff --git a/apps/plugins/cube.c b/apps/plugins/cube.c
index c7f7b63243..3e7e7f1ce2 100644
--- a/apps/plugins/cube.c
+++ b/apps/plugins/cube.c
@@ -534,7 +534,7 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     xlcd_init(rb);
 #elif defined(USE_GSLIB)
     gbuf = (unsigned char *)rb->plugin_get_buffer(&gbuf_size);
-    if (gray_init(rb, gbuf, gbuf_size, true, LCD_WIDTH, LCD_HEIGHT/8, 3, NULL)
+    if (gray_init(rb, gbuf, gbuf_size, true, LCD_WIDTH, LCD_HEIGHT/8, 3, 0, NULL)
         != 3)
     {
         rb->splash(HZ, true, "Couldn't get grayscale buffer");
diff --git a/apps/plugins/doom/i_video.c b/apps/plugins/doom/i_video.c
index 42e1c2610b..f90f7cd82f 100644
--- a/apps/plugins/doom/i_video.c
+++ b/apps/plugins/doom/i_video.c
@@ -16,7 +16,10 @@
  * GNU General Public License for more details.
  *
  * $Log$
- * Revision 1.17  2006/04/22 03:48:15  kkurbjun
+ * Revision 1.18  2006/08/02 00:21:59  amiconn
+ * Grayscale library: LCD linearisation and gamma correction.
+ *
+ * Revision 1.17  2006-04-22 03:48:15  kkurbjun
  * Better video update, add options to startup menu, change default screensize
  *
  * Revision 1.16  2006-04-20 19:39:56  kkurbjun
@@ -484,7 +487,8 @@ void I_InitGraphics(void)
 
 #ifndef HAVE_LCD_COLOR
    gbuf=malloc(GRAYBUFSIZE);
-   gray_init(rb, gbuf, GRAYBUFSIZE, false, LCD_WIDTH, LCD_HEIGHT/8, 32, NULL);
+   gray_init(rb, gbuf, GRAYBUFSIZE, false, LCD_WIDTH, LCD_HEIGHT/8, 32, 
+             3<<7 /* 1.5 */, NULL);
    /* switch on grayscale overlay */
    gray_show(true);
 #endif
diff --git a/apps/plugins/fire.c b/apps/plugins/fire.c
index 7e0c3ba4f5..6207a8894e 100644
--- a/apps/plugins/fire.c
+++ b/apps/plugins/fire.c
@@ -332,7 +332,7 @@ int main(void)
     shades = 256;
 #else
     shades = gray_init(rb, gbuf, gbuf_size, false, LCD_WIDTH, LCD_HEIGHT/8,
-                       32, NULL) + 1;
+                       32, 2<<8, NULL) + 1;
     if(shades <= 1)
     {
         rb->splash(HZ, true, "not enough memory");
diff --git a/apps/plugins/grayscale.c b/apps/plugins/grayscale.c
index 162f8d7b21..ad3ee4f144 100644
--- a/apps/plugins/grayscale.c
+++ b/apps/plugins/grayscale.c
@@ -183,7 +183,7 @@ int main(void)
        H1x0: 160 pixels wide, 30 rows (120 pixels) high, (try to) reserve
        32 bitplanes for 33 shades of grey. */
     shades = gray_init(rb, gbuf, gbuf_size, true, LCD_WIDTH, GFX_HEIGHT/8,
-                       32, NULL) + 1;
+                       32, 0, NULL) + 1;
 
     /* place greyscale overlay 1 row down */
     gray_set_position(0, 1);
diff --git a/apps/plugins/jpeg.c b/apps/plugins/jpeg.c
index 890ee8e252..5166f4ef5e 100644
--- a/apps/plugins/jpeg.c
+++ b/apps/plugins/jpeg.c
@@ -2771,7 +2771,7 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
 #ifdef USEGSLIB
     /* initialize the grayscale buffer: 32 bitplanes for 33 shades of gray. */
     grayscales = gray_init(rb, buf, buf_size, false, LCD_WIDTH, LCD_HEIGHT/8,
-                           32, &graysize) + 1;
+                           32, 2<<8, &graysize) + 1;
     buf += graysize;
     buf_size -= graysize;
     if (grayscales < 33 || buf_size <= 0)
diff --git a/apps/plugins/lib/gray.h b/apps/plugins/lib/gray.h
index 96dec346cf..398462fd81 100644
--- a/apps/plugins/lib/gray.h
+++ b/apps/plugins/lib/gray.h
@@ -38,7 +38,8 @@
 
 /* Library initialisation and release */
 int  gray_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
-               bool buffered, int width, int bheight, int depth, long *buf_taken);
+               bool buffered, int width, int bheight, int depth, int gamma,
+               long *buf_taken);
 void gray_release(void);
 
 /* Special functions */
@@ -138,9 +139,12 @@ struct _gray_info
 #endif
     unsigned char *cur_buffer; /* start of current chunky pixel buffer */
     unsigned char *back_buffer;/* start of chunky pixel back buffer */
+    unsigned char idxtable[256]; /* calculated brightness -> index translation table */
+    int fg_index;              /* current foreground index */
+    int bg_index;              /* current background index */
+    int fg_brightness;         /* current foreground brightness, for returning */
+    int bg_brightness;         /* current background brightness, for returning */
     int drawmode;              /* current draw mode */
-    int fg_brightness;         /* current foreground brightness */
-    int bg_brightness;         /* current background brightness */
     int curfont;               /* current selected font */
 };
 
diff --git a/apps/plugins/lib/gray_core.c b/apps/plugins/lib/gray_core.c
index 6cc33e05da..ef41e7e215 100644
--- a/apps/plugins/lib/gray_core.c
+++ b/apps/plugins/lib/gray_core.c
@@ -33,7 +33,117 @@ struct plugin_api *_gray_rb = NULL; /* global api struct pointer */
 struct _gray_info _gray_info;       /* global info structure */
 #ifndef SIMULATOR
 short _gray_random_buffer;          /* buffer for random number generator */
+
+#if CONFIG_LCD == LCD_SSD1815
+/* measured and interpolated curve */
+static const unsigned char lcdlinear[256] = {
+      0,   3,   5,   8,  11,  13,  16,  18,
+     21,  23,  26,  28,  31,  33,  36,  38,
+     40,  42,  45,  47,  49,  51,  53,  55,
+     57,  59,  60,  62,  64,  66,  67,  69,
+     70,  72,  73,  74,  76,  77,  78,  79,
+     81,  82,  83,  84,  85,  86,  87,  88,
+     88,  89,  90,  91,  92,  92,  93,  94,
+     95,  95,  96,  97,  97,  98,  99,  99,
+    100, 101, 102, 102, 103, 104, 104, 105,
+    106, 106, 107, 107, 108, 109, 109, 110,
+    111, 111, 112, 113, 113, 114, 114, 115,
+    116, 116, 117, 117, 118, 119, 119, 120,
+    120, 121, 121, 122, 122, 123, 123, 124,
+    124, 125, 125, 126, 126, 127, 127, 128,
+    128, 128, 129, 129, 130, 130, 131, 131,
+    132, 132, 133, 133, 133, 134, 134, 135,
+    135, 136, 136, 137, 137, 138, 138, 138,
+    139, 139, 140, 140, 141, 141, 142, 142,
+    143, 143, 144, 144, 145, 145, 146, 146,
+    147, 147, 148, 148, 148, 149, 149, 150,
+    150, 151, 151, 152, 152, 153, 153, 153,
+    154, 154, 155, 155, 156, 156, 157, 157,
+    158, 158, 158, 159, 159, 160, 160, 161,
+    161, 162, 162, 163, 163, 164, 164, 165,
+    165, 166, 167, 167, 168, 168, 169, 169,
+    170, 171, 171, 172, 173, 173, 174, 175,
+    176, 176, 177, 178, 179, 180, 181, 181,
+    182, 183, 184, 185, 186, 188, 189, 190,
+    191, 192, 194, 195, 196, 198, 199, 201,
+    202, 204, 205, 207, 209, 211, 213, 215,
+    217, 219, 222, 224, 226, 229, 231, 234,
+    236, 239, 242, 244, 247, 250, 252, 255
+};
+#elif CONFIG_LCD == LCD_S1D15E06
+/* measured and interpolated curve */
+static const unsigned char lcdlinear[256] = {
+      0,   5,  11,  16,  21,  27,  32,  37,
+     42,  47,  51,  56,  60,  64,  68,  72,
+     75,  78,  81,  84,  87,  89,  91,  93,
+     95,  96,  98,  99, 101, 102, 103, 104,
+    105, 106, 107, 108, 109, 110, 111, 111,
+    112, 113, 113, 114, 115, 115, 116, 117,
+    117, 118, 118, 119, 119, 120, 120, 121,
+    121, 122, 122, 123, 123, 124, 124, 125,
+    125, 126, 126, 127, 127, 127, 128, 128,
+    129, 129, 130, 130, 131, 131, 132, 132,
+    133, 133, 134, 134, 135, 135, 136, 136,
+    137, 137, 138, 138, 138, 139, 139, 140,
+    140, 141, 141, 141, 142, 142, 143, 143,
+    143, 144, 144, 145, 145, 145, 146, 146,
+    146, 147, 147, 147, 148, 148, 149, 149,
+    149, 150, 150, 150, 151, 151, 151, 152,
+    152, 153, 153, 153, 154, 154, 155, 155,
+    155, 156, 156, 157, 157, 157, 158, 158,
+    159, 159, 159, 160, 160, 161, 161, 162,
+    162, 162, 163, 163, 164, 164, 164, 165,
+    165, 166, 166, 167, 167, 167, 168, 168,
+    169, 169, 170, 170, 170, 171, 171, 172,
+    172, 173, 173, 174, 174, 175, 175, 176,
+    176, 177, 177, 178, 178, 179, 179, 180,
+    180, 181, 182, 182, 183, 184, 184, 185,
+    186, 186, 187, 188, 188, 189, 190, 191,
+    191, 192, 193, 194, 195, 196, 196, 197,
+    198, 199, 200, 201, 202, 203, 204, 205,
+    206, 207, 208, 209, 210, 211, 213, 214,
+    215, 216, 218, 219, 220, 222, 223, 225,
+    227, 228, 230, 232, 233, 235, 237, 239,
+    241, 243, 245, 247, 249, 251, 253, 255
+};
 #endif
+#else /* SIMULATOR */
+/* undo a (generic) PC display gamma of 2.0 to simulate target behaviour */
+static const unsigned char lcdlinear[256] = {
+      0,  16,  23,  28,  32,  36,  39,  42,
+     45,  48,  50,  53,  55,  58,  60,  62,
+     64,  66,  68,  70,  71,  73,  75,  77,
+     78,  80,  81,  83,  84,  86,  87,  89,
+     90,  92,  93,  94,  96,  97,  98, 100,
+    101, 102, 103, 105, 106, 107, 108, 109,
+    111, 112, 113, 114, 115, 116, 117, 118,
+    119, 121, 122, 123, 124, 125, 126, 127,
+    128, 129, 130, 131, 132, 133, 134, 135,
+    135, 136, 137, 138, 139, 140, 141, 142,
+    143, 144, 145, 145, 146, 147, 148, 149,
+    150, 151, 151, 152, 153, 154, 155, 156,
+    156, 157, 158, 159, 160, 160, 161, 162,
+    163, 164, 164, 165, 166, 167, 167, 168,
+    169, 170, 170, 171, 172, 173, 173, 174,
+    175, 176, 176, 177, 178, 179, 179, 180,
+    181, 181, 182, 183, 183, 184, 185, 186,
+    186, 187, 188, 188, 189, 190, 190, 191,
+    192, 192, 193, 194, 194, 195, 196, 196,
+    197, 198, 198, 199, 199, 200, 201, 201,
+    202, 203, 203, 204, 204, 205, 206, 206,
+    207, 208, 208, 209, 209, 210, 211, 211,
+    212, 212, 213, 214, 214, 215, 215, 216,
+    217, 217, 218, 218, 219, 220, 220, 221,
+    221, 222, 222, 223, 224, 224, 225, 225,
+    226, 226, 227, 228, 228, 229, 229, 230,
+    230, 231, 231, 232, 233, 233, 234, 234,
+    235, 235, 236, 236, 237, 237, 238, 238,
+    239, 240, 240, 241, 241, 242, 242, 243,
+    243, 244, 244, 245, 245, 246, 246, 247,
+    247, 248, 248, 249, 249, 250, 250, 251,
+    251, 252, 252, 253, 253, 254, 254, 255
+};
+#endif /* SIMULATOR */
 
 /* Prototypes */
 static inline void _deferred_update(void) __attribute__ ((always_inline));
@@ -84,6 +194,75 @@ static void _timer_isr(void)
 }
 #endif /* !SIMULATOR */
 
+/* fixed point exp() */
+static int exp_s16p16(int x)
+{
+    int t;
+    int y = 0x00010000;
+    
+    if (x == 0)
+    {
+        return y;
+    }
+    else if (x > 0)
+    {
+        t = x - 0x58b91; if (t >= 0) x = t, y <<= 8;
+        t = x - 0x2c5c8; if (t >= 0) x = t, y <<= 4;
+        t = x - 0x162e4; if (t >= 0) x = t, y <<= 2;
+        t = x - 0x0b172; if (t >= 0) x = t, y <<= 1;
+    }
+    else
+    {
+        t = x + 0x58b91; if (t < 0) x = t, y >>= 8;
+        t = x + 0x2c5c8; if (t < 0) x = t, y >>= 4;
+        t = x + 0x162e4; if (t < 0) x = t, y >>= 2;
+        t = x + 0x0b172; if (t < 0) x = t, y >>= 1;
+        x += 0x0b172;                      y >>= 1;
+    }
+    t = x - 0x067cd; if (t >= 0) x = t, y += y >> 1;
+    t = x - 0x03920; if (t >= 0) x = t, y += y >> 2;
+    t = x - 0x01e27; if (t >= 0) x = t, y += y >> 3;
+    t = x - 0x00f85; if (t >= 0) x = t, y += y >> 4;
+    t = x - 0x007e1; if (t >= 0) x = t, y += y >> 5;
+    t = x - 0x003f8; if (t >= 0) x = t, y += y >> 6;
+    t = x - 0x001fe; if (t >= 0) x = t, y += y >> 7;
+    if (x & 0x100)               y += y >> 8;
+    if (x & 0x080)               y += y >> 9;
+    if (x & 0x040)               y += y >> 10;
+    if (x & 0x020)               y += y >> 11;
+    if (x & 0x010)               y += y >> 12;
+    if (x & 0x008)               y += y >> 13;
+    if (x & 0x004)               y += y >> 14;
+    if (x & 0x002)               y += y >> 15;
+    if (x & 0x001)               y += y >> 16;
+
+    return y;
+}
+
+/* fixed point log() */
+int log_s16p16(int x)
+{
+    int t;
+    int y = 0xa65af;
+
+    if (x < 0x00008000) x <<=16,                        y -= 0xb1721;
+    if (x < 0x00800000) x <<= 8,                        y -= 0x58b91;
+    if (x < 0x08000000) x <<= 4,                        y -= 0x2c5c8;
+    if (x < 0x20000000) x <<= 2,                        y -= 0x162e4;
+    if (x < 0x40000000) x <<= 1,                        y -= 0x0b172;
+    t = x + (x >> 1); if ((t & 0x80000000) == 0) x = t, y -= 0x067cd;
+    t = x + (x >> 2); if ((t & 0x80000000) == 0) x = t, y -= 0x03920;
+    t = x + (x >> 3); if ((t & 0x80000000) == 0) x = t, y -= 0x01e27;
+    t = x + (x >> 4); if ((t & 0x80000000) == 0) x = t, y -= 0x00f85;
+    t = x + (x >> 5); if ((t & 0x80000000) == 0) x = t, y -= 0x007e1;
+    t = x + (x >> 6); if ((t & 0x80000000) == 0) x = t, y -= 0x003f8;
+    t = x + (x >> 7); if ((t & 0x80000000) == 0) x = t, y -= 0x001fe;
+    x = 0x80000000 - x;
+    y -= x >> 15;
+
+    return y;
+}
+
 /* Initialise the framework and prepare the greyscale display buffer
 
  arguments:
@@ -97,6 +276,8 @@ static void _timer_isr(void)
    width     = width in pixels  (1..LCD_WIDTH)
    bheight   = height in LCD pixel-block units (8 pixels) (1..LCD_HEIGHT/8)
    depth     = number of bitplanes to use (1..32).
+   gamma     = gamma value as s8p8 fixed point. gamma <= 0 means no
+               correction at all, i.e. no LCD linearisation as well.
 
  result:
    = depth  if there was enough memory
@@ -128,12 +309,14 @@ static void _timer_isr(void)
    one situation where it will consume more memory on the sim than on the
    target: if you're allocating a low depth (< 8) without buffering. */
 int gray_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
-              bool buffered, int width, int bheight, int depth, long *buf_taken)
+              bool buffered, int width, int bheight, int depth, int gamma,
+              long *buf_taken)
 {
-    int possible_depth;
+    int possible_depth, i;
     long plane_size, buftaken;
+    unsigned data;
 #ifndef SIMULATOR
-    int i, j;
+    int j;
 #endif
 
     _gray_rb = newrb;
@@ -240,8 +423,33 @@ int gray_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
     }
 #endif
 
+    /* precalculate the value -> pattern index conversion table, taking 
+       linearisation and gamma correction into account */
+    if (gamma <= 0)
+    {
+        for (i = 0; i < 256; i++)
+        {
+            data = MULU16(depth, lcdlinear[i]) + 127;
+            _gray_info.idxtable[i] = (data + (data >> 8)) >> 8;
+                                      /* approx. data / 255 */
+        }
+    }
+    else
+    {
+        for (i = 0; i < 256; i++)
+        {
+            data = exp_s16p16(gamma * (log_s16p16(i * 257 + 1) >> 8));
+            data = (data - (data >> 8)) >> 8; /* approx. data /= 257 */
+            data = MULU16(depth, lcdlinear[data]) + 127;
+            _gray_info.idxtable[i] = (data + (data >> 8)) >> 8;
+                                      /* approx. data / 255 */
+        }
+    }
+
+    _gray_info.fg_index = 0;
+    _gray_info.bg_index = depth;
     _gray_info.fg_brightness = 0;
-    _gray_info.bg_brightness = depth;
+    _gray_info.bg_brightness = 255;
     _gray_info.drawmode = DRMODE_SOLID;
     _gray_info.curfont = FONT_SYSFIXED;
 
diff --git a/apps/plugins/lib/gray_draw.c b/apps/plugins/lib/gray_draw.c
index 6779b6eeb5..7e1197bd4b 100644
--- a/apps/plugins/lib/gray_draw.c
+++ b/apps/plugins/lib/gray_draw.c
@@ -32,12 +32,12 @@
 
 static void setpixel(unsigned char *address)
 {
-    *address = _gray_info.fg_brightness;
+    *address = _gray_info.fg_index;
 }
 
 static void clearpixel(unsigned char *address)
 {
-    *address = _gray_info.bg_brightness;
+    *address = _gray_info.bg_index;
 }
 
 static void flippixel(unsigned char *address)
@@ -61,7 +61,7 @@ void (* const _gray_pixelfuncs[8])(unsigned char *address) = {
 void gray_clear_display(void)
 {
     int brightness = (_gray_info.drawmode & DRMODE_INVERSEVID) ?
-                     _gray_info.fg_brightness : _gray_info.bg_brightness;
+                     _gray_info.fg_index : _gray_info.bg_index;
 
     _gray_rb->memset(_gray_info.cur_buffer, brightness,
                      MULU16(_gray_info.width, _gray_info.height));
@@ -219,7 +219,7 @@ void gray_vline(int x, int y1, int y2)
         if (_gray_info.drawmode & DRMODE_BG)
         {
             fillopt = true;
-            bits = _gray_info.bg_brightness;
+            bits = _gray_info.bg_index;
         }
     }
     else
@@ -227,7 +227,7 @@ void gray_vline(int x, int y1, int y2)
         if (_gray_info.drawmode & DRMODE_FG)
         {
             fillopt = true;
-            bits = _gray_info.fg_brightness;
+            bits = _gray_info.fg_index;
         }
     }
     pfunc = _gray_pixelfuncs[_gray_info.drawmode];
@@ -293,7 +293,7 @@ void gray_fillrect(int x, int y, int width, int height)
         if (_gray_info.drawmode & DRMODE_BG)
         {
             fillopt = true;
-            bits = _gray_info.bg_brightness;
+            bits = _gray_info.bg_index;
         }
     }
     else
@@ -301,7 +301,7 @@ void gray_fillrect(int x, int y, int width, int height)
         if (_gray_info.drawmode & DRMODE_FG)
         {
             fillopt = true;
-            bits = _gray_info.fg_brightness;
+            bits = _gray_info.fg_index;
         }
     }
     pfunc = _gray_pixelfuncs[_gray_info.drawmode];
@@ -525,8 +525,7 @@ void gray_gray_bitmap_part(const unsigned char *src, int src_x, int src_y,
         dst_end = dst_col + height;
         do
         {
-            unsigned data = MULU16(_gray_info.depth, *src_col) + 127;
-            *dst_col++ = (data + (data >> 8)) >> 8; /* approx. data / 255 */
+            *dst_col++ = _gray_info.idxtable[*src_col];
             src_col += stride;
         }
         while (dst_col < dst_end);
@@ -642,13 +641,8 @@ static void _writearray(unsigned char *address, const unsigned char *src,
 
         "mov.b   @%[src],r0  \n"  /* load src byte */
         "extu.b  r0,r0       \n"  /* extend unsigned */
-        "mulu    %[dpth],r0  \n"  /* macl = byte * depth; */
-        "sts     macl,r1     \n"  /* r1 = macl; */
-        "add     #127,r1     \n"  /* byte += 127; */
-        "mov     r1,r0       \n"
-        "shlr8   r1          \n"
-        "add     r1,r0       \n"  /* byte += byte >> 8; */
-        "shlr8   r0          \n"  /* byte >>= 8; */
+        "mov.b   @(r0,%[trns]),r0\n"  /* idxtable into pattern index */
+        "extu.b  r0,r0       \n"  /* extend unsigned */
         "shll2   r0          \n"
         "mov.l   @(r0,%[bpat]),r4\n"  /* r4 = bitpattern[byte]; */
 
@@ -693,7 +687,8 @@ static void _writearray(unsigned char *address, const unsigned char *src,
         [stri]"r"(stride),
         [dpth]"r"(_gray_info.depth),
         [bpat]"r"(_gray_info.bitpattern),
-        [rmsk]"r"(_gray_info.randmask)
+        [rmsk]"r"(_gray_info.randmask),
+        [trns]"r"(_gray_info.idxtable)
         : /* clobbers */
         "r0", "r1", "r3", "r4", "r5", "macl", "pr"
     );
@@ -811,12 +806,7 @@ static void _writearray(unsigned char *address, const unsigned char *src,
 
         "clr.l   %%d0        \n"
         "move.b  (%[src]),%%d0   \n"  /* load src byte */
-        "mulu.w  %[dpth],%%d0\n"  /* byte = byte * depth; */
-        "add.l   #127,%%d0   \n"  /* byte += 127; */
-        "move.l  %%d0,%%d1   \n"
-        "lsr.l   #8,%%d1     \n"
-        "add.l   %%d1,%%d0   \n"  /* byte += byte >> 8; */
-        "lsr.l   #8,%%d0     \n"  /* byte >>= 8; */
+        "move.b  (%%d0:l:1,%[trns]),%%d0\n" /* idxtable into pattern index */
         "move.l  (%%d0:l:4,%[bpat]),%%d2\n" /* d2 = bitpattern[byte]; */
 
         "mulu.w  #75,%[rnd]  \n"  /* multiply by 75 */
@@ -852,6 +842,7 @@ static void _writearray(unsigned char *address, const unsigned char *src,
         : /* inputs */
         [stri]"r"(stride),
         [bpat]"a"(_gray_info.bitpattern),
+        [trns]"a"(_gray_info.idxtable),
         [dpth]"d"(_gray_info.depth),
         [rmsk]"d"(_gray_info.randmask)
         : /* clobbers */
diff --git a/apps/plugins/lib/gray_parm.c b/apps/plugins/lib/gray_parm.c
index c6305421c1..a6064e3ad3 100644
--- a/apps/plugins/lib/gray_parm.c
+++ b/apps/plugins/lib/gray_parm.c
@@ -61,31 +61,27 @@ int  gray_get_drawmode(void)
 /* Set the foreground shade for subsequent drawing operations */
 void gray_set_foreground(unsigned brightness)
 {
-    unsigned data = MULU16(_gray_info.depth, brightness & 0xFF) + 127;
-
-    _gray_info.fg_brightness = (data + (data >> 8)) >> 8; /* approx. data / 255 */
+    _gray_info.fg_brightness = brightness;
+    _gray_info.fg_index = _gray_info.idxtable[brightness]; 
 }
 
 /* Return the current foreground shade */
 unsigned gray_get_foreground(void)
 {
-    return (_gray_info.fg_brightness * 255 + (_gray_info.depth >> 1))
-           / _gray_info.depth;
+    return _gray_info.fg_brightness;
 }
 
 /* Set the background shade for subsequent drawing operations */
 void gray_set_background(unsigned brightness)
 {
-    unsigned data = MULU16(_gray_info.depth, brightness & 0xFF) + 127;
-
-    _gray_info.bg_brightness = (data + (data >> 8)) >> 8; /* approx. data / 255 */
+    _gray_info.bg_brightness = brightness;
+    _gray_info.bg_index = _gray_info.idxtable[brightness];
 }
 
 /* Return the current background shade */
 unsigned gray_get_background(void)
 {
-    return (_gray_info.bg_brightness * 255 + (_gray_info.depth >> 1))
-           / _gray_info.depth;
+    return _gray_info.bg_brightness;
 }
 
 /* Set draw mode, foreground and background shades at once */
diff --git a/apps/plugins/mandelbrot.c b/apps/plugins/mandelbrot.c
index bd0994a5ba..3841e2625a 100644
--- a/apps/plugins/mandelbrot.c
+++ b/apps/plugins/mandelbrot.c
@@ -474,7 +474,7 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     /* initialize the grayscale buffer:
      * 8 bitplanes for 9 shades of gray.*/
     grayscales = gray_init(rb, gbuf, gbuf_size, false, LCD_WIDTH, LCD_HEIGHT/8,
-                           8, NULL) + 1;
+                           8, 0, NULL) + 1;
     if (grayscales != 9) {
         rb->snprintf(buff, sizeof(buff), "%d", grayscales);
         rb->lcd_puts(0, 1, buff);
diff --git a/apps/plugins/plasma.c b/apps/plugins/plasma.c
index e2738eceab..4e8b38db05 100644
--- a/apps/plugins/plasma.c
+++ b/apps/plugins/plasma.c
@@ -237,7 +237,7 @@ int main(void)
     /* get the remainder of the plugin buffer */
     gbuf = (unsigned char *) rb->plugin_get_buffer(&gbuf_size);
 
-    gray_init(rb, gbuf, gbuf_size, false, LCD_WIDTH, LCD_HEIGHT/8, 32, NULL);
+    gray_init(rb, gbuf, gbuf_size, false, LCD_WIDTH, LCD_HEIGHT/8, 32, 2<<8, NULL);
     /* switch on grayscale overlay */
     gray_show(true);
 #endif