1.1 --- a/stage2/jzlcd.c Mon Jan 25 22:18:53 2016 +0100
1.2 +++ b/stage2/jzlcd.c Tue Jan 26 00:10:43 2016 +0100
1.3 @@ -463,18 +463,34 @@
1.4
1.5 static inline u16 rgb8_to_rgb16(u8 rgb)
1.6 {
1.7 - return ((rgb & 0xe0) << 8) | ((rgb & 0x1c) << 6) | ((rgb & 0x03) << 3);
1.8 + return ((((rgb & 0xe0) >> 5) * 4) << 11) | ((((rgb & 0x1c) >> 2) * 9) << 6) | ((rgb & 0x03) * 10);
1.9 +}
1.10 +
1.11 +static inline u16 rgb4_to_rgb16(u8 rgb)
1.12 +{
1.13 + return ((((rgb & 8) >> 3) * 0x1f) << 11) | ((((rgb & 6) >> 1) * 0x15) << 5) | ((rgb & 1) * 0x1f);
1.14 }
1.15
1.16 static void lcd_init_palette(vidinfo_t *vid)
1.17 {
1.18 u16 *palette = (u16 *) lcd_get_palette(get_memory_size());
1.19 - u16 *end = (u16 *) ((unsigned long) palette + lcd_get_palette_size(vid));
1.20 - u16 value = 0;
1.21 + u16 *end = (u16 *) palette + NCOLORS(vid->vl_bpix);
1.22 + u8 value = 0;
1.23
1.24 while (palette < end)
1.25 {
1.26 - *palette = rgb8_to_rgb16(value);
1.27 + switch (vid->vl_bpix)
1.28 + {
1.29 + case LCD_COLOR4:
1.30 + *palette = rgb4_to_rgb16(value);
1.31 + break;
1.32 +
1.33 + case LCD_COLOR8:
1.34 + default:
1.35 + *palette = rgb8_to_rgb16(value);
1.36 + break;
1.37 + }
1.38 +
1.39 value++;
1.40 palette++;
1.41 }