2.1 --- a/stage2/lcd.c Mon Jan 25 22:18:53 2016 +0100
2.2 +++ b/stage2/lcd.c Tue Jan 26 00:10:43 2016 +0100
2.3 @@ -54,14 +54,24 @@
2.4 return (u8 *) (lcd_base + v * get_line_length()) + h;
2.5 }
2.6
2.7 +static u8 *get_pixel4(unsigned short h, unsigned short v)
2.8 +{
2.9 + return (u8 *) (lcd_base + v * get_line_length()) + h / 2;
2.10 +}
2.11 +
2.12 +static inline unsigned short div(unsigned short num, unsigned short denom, unsigned short scale)
2.13 +{
2.14 + return (scale * num) / denom;
2.15 +}
2.16 +
2.17 static unsigned long pixel(unsigned short h, unsigned short v, u8 rmax, u8 gmax, u8 bmax, u8 rshift, u8 gshift, u8 bshift)
2.18 {
2.19 unsigned short v_max = panel_info.vl_row;
2.20 unsigned short h_max = panel_info.vl_col;
2.21
2.22 - return (((rmax * (h_max - h - 1)) / h_max) << rshift) |
2.23 - ((((gmax * h) / (h_max - 1) + (gmax * (v_max - v - 1)) / v_max) / 2) << gshift) |
2.24 - (((bmax * v) / (v_max - 1)) << bshift);
2.25 + return (((rmax - div(h, h_max, rmax + 1) + rmax - div(v, v_max, rmax + 1)) / 2) << rshift) |
2.26 + ((gmax - div(v, v_max, gmax + 1)) << gshift) |
2.27 + (((div(h, h_max, bmax + 1) + div(v, v_max, bmax + 1)) / 2) << bshift);
2.28 }
2.29
2.30 static void test_pixel32(unsigned short h, unsigned short v)
2.31 @@ -82,6 +92,13 @@
2.32 *pix = (u8) pixel(h, v, 7, 7, 3, 5, 2, 0);
2.33 }
2.34
2.35 +static void test_pixel4(unsigned short h, unsigned short v)
2.36 +{
2.37 + u8 *pix = get_pixel4(h, v);
2.38 + u8 mask = h & 1 ? 0xf0 : 0x0f;
2.39 + *pix = (*pix & mask) | ((u8) pixel(h, v, 1, 2, 1, 3, 1, 0) << (h & 1 ? 0 : 4));
2.40 +}
2.41 +
2.42 void test_pixel(unsigned short h, unsigned short v)
2.43 {
2.44 switch (panel_info.vl_bpix)
2.45 @@ -94,6 +111,10 @@
2.46 test_pixel8(h, v);
2.47 break;
2.48
2.49 + case LCD_COLOR4:
2.50 + test_pixel4(h, v);
2.51 + break;
2.52 +
2.53 case LCD_COLOR16:
2.54 default:
2.55 test_pixel16_565(h, v);
2.56 @@ -119,6 +140,13 @@
2.57 *pix = 0;
2.58 }
2.59
2.60 +void clear_pixel4(unsigned short h, unsigned short v)
2.61 +{
2.62 + u8 *pix = get_pixel4(h, v);
2.63 + u8 mask = h & 1 ? 0xf0 : 0x0f;
2.64 + *pix = *pix & mask;
2.65 +}
2.66 +
2.67 void clear_pixel(unsigned short h, unsigned short v)
2.68 {
2.69 switch (panel_info.vl_bpix)
2.70 @@ -131,6 +159,10 @@
2.71 clear_pixel8(h, v);
2.72 break;
2.73
2.74 + case LCD_COLOR4:
2.75 + clear_pixel4(h, v);
2.76 + break;
2.77 +
2.78 case LCD_COLOR16:
2.79 default:
2.80 clear_pixel16(h, v);