firmware/asm/lcd-as-memframe.c

   1
   2 #include <string.h>
   3 #include "lcd.h"
   4 void lcd_copy_buffer_rect(fb_data *dst, fb_data *src, int width, int height)
   5 {
   6     do {
   7         memcpy(dst, src, width * sizeof(fb_data));
   8         src += LCD_WIDTH;
   9         dst += LCD_WIDTH;
  10     } while (--height);
  11 }
  12
  13 #define YFAC    (74)
  14 #define RVFAC   (101)
  15 #define GUFAC   (-24)
  16 #define GVFAC   (-51)
  17 #define BUFAC   (128)
  18
  19 static inline int clamp(int val, int min, int max)
  20 {
  21     if (val < min)
  22         val = min;
  23     else if (val > max)
  24         val = max;
  25     return val;
  26 }
  27
  28 extern void lcd_write_yuv420_lines(fb_data *dst,
  29                                    unsigned char const * const src[3],
  30                                    int width,
  31                                    int stride)
  32 {
  33     /* Draw a partial YUV colour bitmap - similiar behavior to lcd_blit_yuv
  34        in the core */
  35     const unsigned char *ysrc, *usrc, *vsrc;
  36     int height = 2, linecounter;
  37     fb_data *row_end;
  38
  39     /* width and height must be >= 2 and an even number */
  40     width &= ~1;
  41     linecounter = height >> 1;
  42
  43 #if LCD_WIDTH >= LCD_HEIGHT
  44     row_end = dst + width;
  45 #else
  46     row_end = dst + LCD_WIDTH * width;
  47 #endif
  48
  49     ysrc = src[0];
  50     usrc = src[1];
  51     vsrc = src[2];
  52
  53     /* stride => amount to jump from end of last row to start of next */
  54     stride -= width;
  55
  56     /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
  57
  58     do
  59     {
  60         int y, cb, cr, rv, guv, bu, r, g, b;
  61
  62         y  = YFAC*(*ysrc++ - 16);
  63         cb = *usrc++ - 128;
  64         cr = *vsrc++ - 128;
  65
  66         rv  =            RVFAC*cr;
  67         guv = GUFAC*cb + GVFAC*cr;
  68         bu  = BUFAC*cb;
  69
  70         r = y + rv;
  71         g = y + guv;
  72         b = y + bu;
  73
  74         if ((unsigned)(r | g | b) > 64*256-1)
  75         {
  76             r = clamp(r, 0, 64*256-1);
  77             g = clamp(g, 0, 64*256-1);
  78             b = clamp(b, 0, 64*256-1);
  79         }
  80
  81         *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
  82
  83 #if LCD_WIDTH >= LCD_HEIGHT
  84         dst++;
  85 #else
  86         dst += LCD_WIDTH;
  87 #endif
  88
  89         y = YFAC*(*ysrc++ - 16);
  90         r = y + rv;
  91         g = y + guv;
  92         b = y + bu;
  93
  94         if ((unsigned)(r | g | b) > 64*256-1)
  95         {
  96             r = clamp(r, 0, 64*256-1);
  97             g = clamp(g, 0, 64*256-1);
  98             b = clamp(b, 0, 64*256-1);
  99         }
 100
 101         *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
 102
 103 #if LCD_WIDTH >= LCD_HEIGHT
 104         dst++;
 105 #else
 106         dst += LCD_WIDTH;
 107 #endif
 108     }
 109     while (dst < row_end);
 110
 111     ysrc    += stride;
 112     usrc    -= width >> 1;
 113     vsrc    -= width >> 1;
 114
 115 #if LCD_WIDTH >= LCD_HEIGHT
 116     row_end += LCD_WIDTH;
 117     dst     += LCD_WIDTH - width;
 118 #else
 119     row_end -= 1;
 120     dst     -= LCD_WIDTH*width + 1;
 121 #endif
 122
 123     do
 124     {
 125         int y, cb, cr, rv, guv, bu, r, g, b;
 126
 127         y  = YFAC*(*ysrc++ - 16);
 128         cb = *usrc++ - 128;
 129         cr = *vsrc++ - 128;
 130
 131         rv  =            RVFAC*cr;
 132         guv = GUFAC*cb + GVFAC*cr;
 133         bu  = BUFAC*cb;
 134
 135         r = y + rv;
 136         g = y + guv;
 137         b = y + bu;
 138
 139         if ((unsigned)(r | g | b) > 64*256-1)
 140         {
 141             r = clamp(r, 0, 64*256-1);
 142             g = clamp(g, 0, 64*256-1);
 143             b = clamp(b, 0, 64*256-1);
 144         }
 145
 146         *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
 147
 148 #if LCD_WIDTH >= LCD_HEIGHT
 149         dst++;
 150 #else
 151         dst += LCD_WIDTH;
 152 #endif
 153
 154         y = YFAC*(*ysrc++ - 16);
 155         r = y + rv;
 156         g = y + guv;
 157         b = y + bu;
 158
 159         if ((unsigned)(r | g | b) > 64*256-1)
 160         {
 161             r = clamp(r, 0, 64*256-1);
 162             g = clamp(g, 0, 64*256-1);
 163             b = clamp(b, 0, 64*256-1);
 164         }
 165
 166         *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
 167
 168 #if LCD_WIDTH >= LCD_HEIGHT
 169         dst++;
 170 #else
 171         dst += LCD_WIDTH;
 172 #endif
 173     }
 174     while (dst < row_end);
 175 }
 176
 177 void lcd_write_yuv420_lines_odither(fb_data *dst,
 178                                    unsigned char const * const src[3],
 179                                    int width, int stride,
 180                                    int x_screen, int y_screen)
 181 __attribute__((alias("lcd_write_yuv420_lines")));