drivers/media/platform/vivid/vivid-tpg.c

   1 /*
   2  * vivid-tpg.c - Test Pattern Generator
   3  *
   4  * Note: gen_twopix and tpg_gen_text are based on code from vivi.c. See the
   5  * vivi.c source for the copyright information of those functions.
   6  *
   7  * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
   8  *
   9  * This program is free software; you may redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; version 2 of the License.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  14  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  15  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  16  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  17  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  18  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  19  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  20  * SOFTWARE.
  21  */
  22
  23 #include "vivid-tpg.h"
  24
  25 /* Must remain in sync with enum tpg_pattern */
  26 const char * const tpg_pattern_strings[] = {
  27         "75% Colorbar",
  28         "100% Colorbar",
  29         "CSC Colorbar",
  30         "Horizontal 100% Colorbar",
  31         "100% Color Squares",
  32         "100% Black",
  33         "100% White",
  34         "100% Red",
  35         "100% Green",
  36         "100% Blue",
  37         "16x16 Checkers",
  38         "1x1 Checkers",
  39         "Alternating Hor Lines",
  40         "Alternating Vert Lines",
  41         "One Pixel Wide Cross",
  42         "Two Pixels Wide Cross",
  43         "Ten Pixels Wide Cross",
  44         "Gray Ramp",
  45         "Noise",
  46         NULL
  47 };
  48
  49 /* Must remain in sync with enum tpg_aspect */
  50 const char * const tpg_aspect_strings[] = {
  51         "Source Width x Height",
  52         "4x3",
  53         "14x9",
  54         "16x9",
  55         "16x9 Anamorphic",
  56         NULL
  57 };
  58
  59 /*
  60  * Sine table: sin[0] = 127 * sin(-180 degrees)
  61  *             sin[128] = 127 * sin(0 degrees)
  62  *             sin[256] = 127 * sin(180 degrees)
  63  */
  64 static const s8 sin[257] = {
  65            0,   -4,   -7,  -11,  -13,  -18,  -20,  -22,  -26,  -29,  -33,  -35,  -37,  -41,  -43,  -48,
  66          -50,  -52,  -56,  -58,  -62,  -63,  -65,  -69,  -71,  -75,  -76,  -78,  -82,  -83,  -87,  -88,
  67          -90,  -93,  -94,  -97,  -99, -101, -103, -104, -107, -108, -110, -111, -112, -114, -115, -117,
  68         -118, -119, -120, -121, -122, -123, -123, -124, -125, -125, -126, -126, -127, -127, -127, -127,
  69         -127, -127, -127, -127, -126, -126, -125, -125, -124, -124, -123, -122, -121, -120, -119, -118,
  70         -117, -116, -114, -113, -111, -110, -109, -107, -105, -103, -101, -100,  -97,  -96,  -93,  -91,
  71          -90,  -87,  -85,  -82,  -80,  -76,  -75,  -73,  -69,  -67,  -63,  -62,  -60,  -56,  -54,  -50,
  72          -48,  -46,  -41,  -39,  -35,  -33,  -31,  -26,  -24,  -20,  -18,  -15,  -11,   -9,   -4,   -2,
  73            0,    2,    4,    9,   11,   15,   18,   20,   24,   26,   31,   33,   35,   39,   41,   46,
  74           48,   50,   54,   56,   60,   62,   64,   67,   69,   73,   75,   76,   80,   82,   85,   87,
  75           90,   91,   93,   96,   97,  100,  101,  103,  105,  107,  109,  110,  111,  113,  114,  116,
  76          117,  118,  119,  120,  121,  122,  123,  124,  124,  125,  125,  126,  126,  127,  127,  127,
  77          127,  127,  127,  127,  127,  126,  126,  125,  125,  124,  123,  123,  122,  121,  120,  119,
  78          118,  117,  115,  114,  112,  111,  110,  108,  107,  104,  103,  101,   99,   97,   94,   93,
  79           90,   88,   87,   83,   82,   78,   76,   75,   71,   69,   65,   64,   62,   58,   56,   52,
  80           50,   48,   43,   41,   37,   35,   33,   29,   26,   22,   20,   18,   13,   11,    7,    4,
  81            0,
  82 };
  83
  84 #define cos(idx) sin[((idx) + 64) % sizeof(sin)]
  85
  86 /* Global font descriptor */
  87 static const u8 *font8x16;
  88
  89 void tpg_set_font(const u8 *f)
  90 {
  91         font8x16 = f;
  92 }
  93
  94 void tpg_init(struct tpg_data *tpg, unsigned w, unsigned h)
  95 {
  96         memset(tpg, 0, sizeof(*tpg));
  97         tpg->scaled_width = tpg->src_width = w;
  98         tpg->src_height = tpg->buf_height = h;
  99         tpg->crop.width = tpg->compose.width = w;
 100         tpg->crop.height = tpg->compose.height = h;
 101         tpg->recalc_colors = true;
 102         tpg->recalc_square_border = true;
 103         tpg->brightness = 128;
 104         tpg->contrast = 128;
 105         tpg->saturation = 128;
 106         tpg->hue = 0;
 107         tpg->mv_hor_mode = TPG_MOVE_NONE;
 108         tpg->mv_vert_mode = TPG_MOVE_NONE;
 109         tpg->field = V4L2_FIELD_NONE;
 110         tpg_s_fourcc(tpg, V4L2_PIX_FMT_RGB24);
 111         tpg->colorspace = V4L2_COLORSPACE_SRGB;
 112         tpg->perc_fill = 100;
 113 }
 114
 115 int tpg_alloc(struct tpg_data *tpg, unsigned max_w)
 116 {
 117         unsigned pat;
 118         unsigned plane;
 119
 120         tpg->max_line_width = max_w;
 121         for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++) {
 122                 for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
 123                         unsigned pixelsz = plane ? 1 : 4;
 124
 125                         tpg->lines[pat][plane] = vzalloc(max_w * 2 * pixelsz);
 126                         if (!tpg->lines[pat][plane])
 127                                 return -ENOMEM;
 128                 }
 129         }
 130         for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
 131                 unsigned pixelsz = plane ? 1 : 4;
 132
 133                 tpg->contrast_line[plane] = vzalloc(max_w * pixelsz);
 134                 if (!tpg->contrast_line[plane])
 135                         return -ENOMEM;
 136                 tpg->black_line[plane] = vzalloc(max_w * pixelsz);
 137                 if (!tpg->black_line[plane])
 138                         return -ENOMEM;
 139                 tpg->random_line[plane] = vzalloc(max_w * 2 * pixelsz);
 140                 if (!tpg->random_line[plane])
 141                         return -ENOMEM;
 142         }
 143         return 0;
 144 }
 145
 146 void tpg_free(struct tpg_data *tpg)
 147 {
 148         unsigned pat;
 149         unsigned plane;
 150
 151         for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++)
 152                 for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
 153                         vfree(tpg->lines[pat][plane]);
 154                         tpg->lines[pat][plane] = NULL;
 155                 }
 156         for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
 157                 vfree(tpg->contrast_line[plane]);
 158                 vfree(tpg->black_line[plane]);
 159                 vfree(tpg->random_line[plane]);
 160                 tpg->contrast_line[plane] = NULL;
 161                 tpg->black_line[plane] = NULL;
 162                 tpg->random_line[plane] = NULL;
 163         }
 164 }
 165
 166 bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
 167 {
 168         tpg->fourcc = fourcc;
 169         tpg->planes = 1;
 170         tpg->recalc_colors = true;
 171         switch (fourcc) {
 172         case V4L2_PIX_FMT_RGB565:
 173         case V4L2_PIX_FMT_RGB565X:
 174         case V4L2_PIX_FMT_RGB555:
 175         case V4L2_PIX_FMT_XRGB555:
 176         case V4L2_PIX_FMT_ARGB555:
 177         case V4L2_PIX_FMT_RGB555X:
 178         case V4L2_PIX_FMT_RGB24:
 179         case V4L2_PIX_FMT_BGR24:
 180         case V4L2_PIX_FMT_RGB32:
 181         case V4L2_PIX_FMT_BGR32:
 182         case V4L2_PIX_FMT_XRGB32:
 183         case V4L2_PIX_FMT_XBGR32:
 184         case V4L2_PIX_FMT_ARGB32:
 185         case V4L2_PIX_FMT_ABGR32:
 186                 tpg->is_yuv = false;
 187                 break;
 188         case V4L2_PIX_FMT_NV16M:
 189         case V4L2_PIX_FMT_NV61M:
 190                 tpg->planes = 2;
 191                 /* fall-through */
 192         case V4L2_PIX_FMT_YUYV:
 193         case V4L2_PIX_FMT_UYVY:
 194         case V4L2_PIX_FMT_YVYU:
 195         case V4L2_PIX_FMT_VYUY:
 196                 tpg->is_yuv = true;
 197                 break;
 198         default:
 199                 return false;
 200         }
 201
 202         switch (fourcc) {
 203         case V4L2_PIX_FMT_RGB565:
 204         case V4L2_PIX_FMT_RGB565X:
 205         case V4L2_PIX_FMT_RGB555:
 206         case V4L2_PIX_FMT_XRGB555:
 207         case V4L2_PIX_FMT_ARGB555:
 208         case V4L2_PIX_FMT_RGB555X:
 209         case V4L2_PIX_FMT_YUYV:
 210         case V4L2_PIX_FMT_UYVY:
 211         case V4L2_PIX_FMT_YVYU:
 212         case V4L2_PIX_FMT_VYUY:
 213                 tpg->twopixelsize[0] = 2 * 2;
 214                 break;
 215         case V4L2_PIX_FMT_RGB24:
 216         case V4L2_PIX_FMT_BGR24:
 217                 tpg->twopixelsize[0] = 2 * 3;
 218                 break;
 219         case V4L2_PIX_FMT_RGB32:
 220         case V4L2_PIX_FMT_BGR32:
 221         case V4L2_PIX_FMT_XRGB32:
 222         case V4L2_PIX_FMT_XBGR32:
 223         case V4L2_PIX_FMT_ARGB32:
 224         case V4L2_PIX_FMT_ABGR32:
 225                 tpg->twopixelsize[0] = 2 * 4;
 226                 break;
 227         case V4L2_PIX_FMT_NV16M:
 228         case V4L2_PIX_FMT_NV61M:
 229                 tpg->twopixelsize[0] = 2;
 230                 tpg->twopixelsize[1] = 2;
 231                 break;
 232         }
 233         return true;
 234 }
 235
 236 void tpg_s_crop_compose(struct tpg_data *tpg, const struct v4l2_rect *crop,
 237                 const struct v4l2_rect *compose)
 238 {
 239         tpg->crop = *crop;
 240         tpg->compose = *compose;
 241         tpg->scaled_width = (tpg->src_width * tpg->compose.width +
 242                                  tpg->crop.width - 1) / tpg->crop.width;
 243         tpg->scaled_width &= ~1;
 244         if (tpg->scaled_width > tpg->max_line_width)
 245                 tpg->scaled_width = tpg->max_line_width;
 246         if (tpg->scaled_width < 2)
 247                 tpg->scaled_width = 2;
 248         tpg->recalc_lines = true;
 249 }
 250
 251 void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
 252                        u32 field)
 253 {
 254         unsigned p;
 255
 256         tpg->src_width = width;
 257         tpg->src_height = height;
 258         tpg->field = field;
 259         tpg->buf_height = height;
 260         if (V4L2_FIELD_HAS_T_OR_B(field))
 261                 tpg->buf_height /= 2;
 262         tpg->scaled_width = width;
 263         tpg->crop.top = tpg->crop.left = 0;
 264         tpg->crop.width = width;
 265         tpg->crop.height = height;
 266         tpg->compose.top = tpg->compose.left = 0;
 267         tpg->compose.width = width;
 268         tpg->compose.height = tpg->buf_height;
 269         for (p = 0; p < tpg->planes; p++)
 270                 tpg->bytesperline[p] = width * tpg->twopixelsize[p] / 2;
 271         tpg->recalc_square_border = true;
 272 }
 273
 274 static enum tpg_color tpg_get_textbg_color(struct tpg_data *tpg)
 275 {
 276         switch (tpg->pattern) {
 277         case TPG_PAT_BLACK:
 278                 return TPG_COLOR_100_WHITE;
 279         case TPG_PAT_CSC_COLORBAR:
 280                 return TPG_COLOR_CSC_BLACK;
 281         default:
 282                 return TPG_COLOR_100_BLACK;
 283         }
 284 }
 285
 286 static enum tpg_color tpg_get_textfg_color(struct tpg_data *tpg)
 287 {
 288         switch (tpg->pattern) {
 289         case TPG_PAT_75_COLORBAR:
 290         case TPG_PAT_CSC_COLORBAR:
 291                 return TPG_COLOR_CSC_WHITE;
 292         case TPG_PAT_BLACK:
 293                 return TPG_COLOR_100_BLACK;
 294         default:
 295                 return TPG_COLOR_100_WHITE;
 296         }
 297 }
 298
 299 static u16 color_to_y(struct tpg_data *tpg, int r, int g, int b)
 300 {
 301         switch (tpg->colorspace) {
 302         case V4L2_COLORSPACE_SMPTE170M:
 303         case V4L2_COLORSPACE_470_SYSTEM_M:
 304         case V4L2_COLORSPACE_470_SYSTEM_BG:
 305                 return ((16829 * r + 33039 * g + 6416 * b + 16 * 32768) >> 16) + (16 << 4);
 306         case V4L2_COLORSPACE_SMPTE240M:
 307                 return ((11932 * r + 39455 * g + 4897 * b + 16 * 32768) >> 16) + (16 << 4);
 308         case V4L2_COLORSPACE_REC709:
 309         case V4L2_COLORSPACE_SRGB:
 310         default:
 311                 return ((11966 * r + 40254 * g + 4064 * b + 16 * 32768) >> 16) + (16 << 4);
 312         }
 313 }
 314
 315 static u16 color_to_cb(struct tpg_data *tpg, int r, int g, int b)
 316 {
 317         switch (tpg->colorspace) {
 318         case V4L2_COLORSPACE_SMPTE170M:
 319         case V4L2_COLORSPACE_470_SYSTEM_M:
 320         case V4L2_COLORSPACE_470_SYSTEM_BG:
 321                 return ((-9714 * r - 19070 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
 322         case V4L2_COLORSPACE_SMPTE240M:
 323                 return ((-6684 * r - 22100 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
 324         case V4L2_COLORSPACE_REC709:
 325         case V4L2_COLORSPACE_SRGB:
 326         default:
 327                 return ((-6596 * r - 22189 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
 328         }
 329 }
 330
 331 static u16 color_to_cr(struct tpg_data *tpg, int r, int g, int b)
 332 {
 333         switch (tpg->colorspace) {
 334         case V4L2_COLORSPACE_SMPTE170M:
 335         case V4L2_COLORSPACE_470_SYSTEM_M:
 336         case V4L2_COLORSPACE_470_SYSTEM_BG:
 337                 return ((28784 * r - 24103 * g - 4681 * b + 16 * 32768) >> 16) + (128 << 4);
 338         case V4L2_COLORSPACE_SMPTE240M:
 339                 return ((28784 * r - 25606 * g - 3178 * b + 16 * 32768) >> 16) + (128 << 4);
 340         case V4L2_COLORSPACE_REC709:
 341         case V4L2_COLORSPACE_SRGB:
 342         default:
 343                 return ((28784 * r - 26145 * g - 2639 * b + 16 * 32768) >> 16) + (128 << 4);
 344         }
 345 }
 346
 347 static u16 ycbcr_to_r(struct tpg_data *tpg, int y, int cb, int cr)
 348 {
 349         int r;
 350
 351         y -= 16 << 4;
 352         cb -= 128 << 4;
 353         cr -= 128 << 4;
 354         switch (tpg->colorspace) {
 355         case V4L2_COLORSPACE_SMPTE170M:
 356         case V4L2_COLORSPACE_470_SYSTEM_M:
 357         case V4L2_COLORSPACE_470_SYSTEM_BG:
 358                 r = 4769 * y + 6537 * cr;
 359                 break;
 360         case V4L2_COLORSPACE_SMPTE240M:
 361                 r = 4769 * y + 7376 * cr;
 362                 break;
 363         case V4L2_COLORSPACE_REC709:
 364         case V4L2_COLORSPACE_SRGB:
 365         default:
 366                 r = 4769 * y + 7343 * cr;
 367                 break;
 368         }
 369         return clamp(r >> 12, 0, 0xff0);
 370 }
 371
 372 static u16 ycbcr_to_g(struct tpg_data *tpg, int y, int cb, int cr)
 373 {
 374         int g;
 375
 376         y -= 16 << 4;
 377         cb -= 128 << 4;
 378         cr -= 128 << 4;
 379         switch (tpg->colorspace) {
 380         case V4L2_COLORSPACE_SMPTE170M:
 381         case V4L2_COLORSPACE_470_SYSTEM_M:
 382         case V4L2_COLORSPACE_470_SYSTEM_BG:
 383                 g = 4769 * y - 1605 * cb - 3330 * cr;
 384                 break;
 385         case V4L2_COLORSPACE_SMPTE240M:
 386                 g = 4769 * y - 1055 * cb - 2341 * cr;
 387                 break;
 388         case V4L2_COLORSPACE_REC709:
 389         case V4L2_COLORSPACE_SRGB:
 390         default:
 391                 g = 4769 * y - 873 * cb - 2183 * cr;
 392                 break;
 393         }
 394         return clamp(g >> 12, 0, 0xff0);
 395 }
 396
 397 static u16 ycbcr_to_b(struct tpg_data *tpg, int y, int cb, int cr)
 398 {
 399         int b;
 400
 401         y -= 16 << 4;
 402         cb -= 128 << 4;
 403         cr -= 128 << 4;
 404         switch (tpg->colorspace) {
 405         case V4L2_COLORSPACE_SMPTE170M:
 406         case V4L2_COLORSPACE_470_SYSTEM_M:
 407         case V4L2_COLORSPACE_470_SYSTEM_BG:
 408                 b = 4769 * y + 7343 * cb;
 409                 break;
 410         case V4L2_COLORSPACE_SMPTE240M:
 411                 b = 4769 * y + 8552 * cb;
 412                 break;
 413         case V4L2_COLORSPACE_REC709:
 414         case V4L2_COLORSPACE_SRGB:
 415         default:
 416                 b = 4769 * y + 8652 * cb;
 417                 break;
 418         }
 419         return clamp(b >> 12, 0, 0xff0);
 420 }
 421
 422 /* precalculate color bar values to speed up rendering */
 423 static void precalculate_color(struct tpg_data *tpg, int k)
 424 {
 425         int col = k;
 426         int r = tpg_colors[col].r;
 427         int g = tpg_colors[col].g;
 428         int b = tpg_colors[col].b;
 429
 430         if (k == TPG_COLOR_TEXTBG) {
 431                 col = tpg_get_textbg_color(tpg);
 432
 433                 r = tpg_colors[col].r;
 434                 g = tpg_colors[col].g;
 435                 b = tpg_colors[col].b;
 436         } else if (k == TPG_COLOR_TEXTFG) {
 437                 col = tpg_get_textfg_color(tpg);
 438
 439                 r = tpg_colors[col].r;
 440                 g = tpg_colors[col].g;
 441                 b = tpg_colors[col].b;
 442         } else if (tpg->pattern == TPG_PAT_NOISE) {
 443                 r = g = b = prandom_u32_max(256);
 444         } else if (k == TPG_COLOR_RANDOM) {
 445                 r = g = b = tpg->qual_offset + prandom_u32_max(196);
 446         } else if (k >= TPG_COLOR_RAMP) {
 447                 r = g = b = k - TPG_COLOR_RAMP;
 448         }
 449
 450         if (tpg->pattern == TPG_PAT_CSC_COLORBAR && col <= TPG_COLOR_CSC_BLACK) {
 451                 r = tpg_csc_colors[tpg->colorspace][col].r;
 452                 g = tpg_csc_colors[tpg->colorspace][col].g;
 453                 b = tpg_csc_colors[tpg->colorspace][col].b;
 454         } else {
 455                 r <<= 4;
 456                 g <<= 4;
 457                 b <<= 4;
 458         }
 459         if (tpg->qual == TPG_QUAL_GRAY)
 460                 r = g = b = color_to_y(tpg, r, g, b);
 461
 462         /*
 463          * The assumption is that the RGB output is always full range,
 464          * so only if the rgb_range overrides the 'real' rgb range do
 465          * we need to convert the RGB values.
 466          *
 467          * Currently there is no way of signalling to userspace if you
 468          * are actually giving it limited range RGB (or full range
 469          * YUV for that matter).
 470          *
 471          * Remember that r, g and b are still in the 0 - 0xff0 range.
 472          */
 473         if (tpg->real_rgb_range == V4L2_DV_RGB_RANGE_LIMITED &&
 474             tpg->rgb_range == V4L2_DV_RGB_RANGE_FULL) {
 475                 /*
 476                  * Convert from full range (which is what r, g and b are)
 477                  * to limited range (which is the 'real' RGB range), which
 478                  * is then interpreted as full range.
 479                  */
 480                 r = (r * 219) / 255 + (16 << 4);
 481                 g = (g * 219) / 255 + (16 << 4);
 482                 b = (b * 219) / 255 + (16 << 4);
 483         } else if (tpg->real_rgb_range != V4L2_DV_RGB_RANGE_LIMITED &&
 484                    tpg->rgb_range == V4L2_DV_RGB_RANGE_LIMITED) {
 485                 /*
 486                  * Clamp r, g and b to the limited range and convert to full
 487                  * range since that's what we deliver.
 488                  */
 489                 r = clamp(r, 16 << 4, 235 << 4);
 490                 g = clamp(g, 16 << 4, 235 << 4);
 491                 b = clamp(b, 16 << 4, 235 << 4);
 492                 r = (r - (16 << 4)) * 255 / 219;
 493                 g = (g - (16 << 4)) * 255 / 219;
 494                 b = (b - (16 << 4)) * 255 / 219;
 495         }
 496
 497         if (tpg->brightness != 128 || tpg->contrast != 128 ||
 498             tpg->saturation != 128 || tpg->hue) {
 499                 /* Implement these operations */
 500
 501                 /* First convert to YCbCr */
 502                 int y = color_to_y(tpg, r, g, b);       /* Luma */
 503                 int cb = color_to_cb(tpg, r, g, b);     /* Cb */
 504                 int cr = color_to_cr(tpg, r, g, b);     /* Cr */
 505                 int tmp_cb, tmp_cr;
 506
 507                 y = (16 << 4) + ((y - (16 << 4)) * tpg->contrast) / 128;
 508                 y += (tpg->brightness << 4) - (128 << 4);
 509
 510                 cb -= 128 << 4;
 511                 cr -= 128 << 4;
 512                 tmp_cb = (cb * cos(128 + tpg->hue)) / 127 + (cr * sin[128 + tpg->hue]) / 127;
 513                 tmp_cr = (cr * cos(128 + tpg->hue)) / 127 - (cb * sin[128 + tpg->hue]) / 127;
 514
 515                 cb = (128 << 4) + (tmp_cb * tpg->contrast * tpg->saturation) / (128 * 128);
 516                 cr = (128 << 4) + (tmp_cr * tpg->contrast * tpg->saturation) / (128 * 128);
 517                 if (tpg->is_yuv) {
 518                         tpg->colors[k][0] = clamp(y >> 4, 1, 254);
 519                         tpg->colors[k][1] = clamp(cb >> 4, 1, 254);
 520                         tpg->colors[k][2] = clamp(cr >> 4, 1, 254);
 521                         return;
 522                 }
 523                 r = ycbcr_to_r(tpg, y, cb, cr);
 524                 g = ycbcr_to_g(tpg, y, cb, cr);
 525                 b = ycbcr_to_b(tpg, y, cb, cr);
 526         }
 527
 528         if (tpg->is_yuv) {
 529                 /* Convert to YCbCr */
 530                 u16 y = color_to_y(tpg, r, g, b);       /* Luma */
 531                 u16 cb = color_to_cb(tpg, r, g, b);     /* Cb */
 532                 u16 cr = color_to_cr(tpg, r, g, b);     /* Cr */
 533
 534                 tpg->colors[k][0] = clamp(y >> 4, 1, 254);
 535                 tpg->colors[k][1] = clamp(cb >> 4, 1, 254);
 536                 tpg->colors[k][2] = clamp(cr >> 4, 1, 254);
 537         } else {
 538                 switch (tpg->fourcc) {
 539                 case V4L2_PIX_FMT_RGB565:
 540                 case V4L2_PIX_FMT_RGB565X:
 541                         r >>= 7;
 542                         g >>= 6;
 543                         b >>= 7;
 544                         break;
 545                 case V4L2_PIX_FMT_RGB555:
 546                 case V4L2_PIX_FMT_XRGB555:
 547                 case V4L2_PIX_FMT_ARGB555:
 548                 case V4L2_PIX_FMT_RGB555X:
 549                         r >>= 7;
 550                         g >>= 7;
 551                         b >>= 7;
 552                         break;
 553                 default:
 554                         r >>= 4;
 555                         g >>= 4;
 556                         b >>= 4;
 557                         break;
 558                 }
 559
 560                 tpg->colors[k][0] = r;
 561                 tpg->colors[k][1] = g;
 562                 tpg->colors[k][2] = b;
 563         }
 564 }
 565
 566 static void tpg_precalculate_colors(struct tpg_data *tpg)
 567 {
 568         int k;
 569
 570         for (k = 0; k < TPG_COLOR_MAX; k++)
 571                 precalculate_color(tpg, k);
 572 }
 573
 574 /* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
 575 static void gen_twopix(struct tpg_data *tpg,
 576                 u8 buf[TPG_MAX_PLANES][8], int color, bool odd)
 577 {
 578         unsigned offset = odd * tpg->twopixelsize[0] / 2;
 579         u8 alpha = tpg->alpha_component;
 580         u8 r_y, g_u, b_v;
 581
 582         if (tpg->alpha_red_only && color != TPG_COLOR_CSC_RED &&
 583                                    color != TPG_COLOR_100_RED &&
 584                                    color != TPG_COLOR_75_RED)
 585                 alpha = 0;
 586         if (color == TPG_COLOR_RANDOM)
 587                 precalculate_color(tpg, color);
 588         r_y = tpg->colors[color][0]; /* R or precalculated Y */
 589         g_u = tpg->colors[color][1]; /* G or precalculated U */
 590         b_v = tpg->colors[color][2]; /* B or precalculated V */
 591
 592         switch (tpg->fourcc) {
 593         case V4L2_PIX_FMT_NV16M:
 594                 buf[0][offset] = r_y;
 595                 buf[1][offset] = odd ? b_v : g_u;
 596                 break;
 597         case V4L2_PIX_FMT_NV61M:
 598                 buf[0][offset] = r_y;
 599                 buf[1][offset] = odd ? g_u : b_v;
 600                 break;
 601
 602         case V4L2_PIX_FMT_YUYV:
 603                 buf[0][offset] = r_y;
 604                 buf[0][offset + 1] = odd ? b_v : g_u;
 605                 break;
 606         case V4L2_PIX_FMT_UYVY:
 607                 buf[0][offset] = odd ? b_v : g_u;
 608                 buf[0][offset + 1] = r_y;
 609                 break;
 610         case V4L2_PIX_FMT_YVYU:
 611                 buf[0][offset] = r_y;
 612                 buf[0][offset + 1] = odd ? g_u : b_v;
 613                 break;
 614         case V4L2_PIX_FMT_VYUY:
 615                 buf[0][offset] = odd ? g_u : b_v;
 616                 buf[0][offset + 1] = r_y;
 617                 break;
 618         case V4L2_PIX_FMT_RGB565:
 619                 buf[0][offset] = (g_u << 5) | b_v;
 620                 buf[0][offset + 1] = (r_y << 3) | (g_u >> 3);
 621                 break;
 622         case V4L2_PIX_FMT_RGB565X:
 623                 buf[0][offset] = (r_y << 3) | (g_u >> 3);
 624                 buf[0][offset + 1] = (g_u << 5) | b_v;
 625                 break;
 626         case V4L2_PIX_FMT_RGB555:
 627         case V4L2_PIX_FMT_XRGB555:
 628                 alpha = 0;
 629                 /* fall through */
 630         case V4L2_PIX_FMT_ARGB555:
 631                 buf[0][offset] = (g_u << 5) | b_v;
 632                 buf[0][offset + 1] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
 633                 break;
 634         case V4L2_PIX_FMT_RGB555X:
 635                 buf[0][offset] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
 636                 buf[0][offset + 1] = (g_u << 5) | b_v;
 637                 break;
 638         case V4L2_PIX_FMT_RGB24:
 639                 buf[0][offset] = r_y;
 640                 buf[0][offset + 1] = g_u;
 641                 buf[0][offset + 2] = b_v;
 642                 break;
 643         case V4L2_PIX_FMT_BGR24:
 644                 buf[0][offset] = b_v;
 645                 buf[0][offset + 1] = g_u;
 646                 buf[0][offset + 2] = r_y;
 647                 break;
 648         case V4L2_PIX_FMT_RGB32:
 649         case V4L2_PIX_FMT_XRGB32:
 650                 alpha = 0;
 651                 /* fall through */
 652         case V4L2_PIX_FMT_ARGB32:
 653                 buf[0][offset] = alpha;
 654                 buf[0][offset + 1] = r_y;
 655                 buf[0][offset + 2] = g_u;
 656                 buf[0][offset + 3] = b_v;
 657                 break;
 658         case V4L2_PIX_FMT_BGR32:
 659         case V4L2_PIX_FMT_XBGR32:
 660                 alpha = 0;
 661                 /* fall through */
 662         case V4L2_PIX_FMT_ABGR32:
 663                 buf[0][offset] = b_v;
 664                 buf[0][offset + 1] = g_u;
 665                 buf[0][offset + 2] = r_y;
 666                 buf[0][offset + 3] = alpha;
 667                 break;
 668         }
 669 }
 670
 671 /* Return how many pattern lines are used by the current pattern. */
 672 static unsigned tpg_get_pat_lines(struct tpg_data *tpg)
 673 {
 674         switch (tpg->pattern) {
 675         case TPG_PAT_CHECKERS_16X16:
 676         case TPG_PAT_CHECKERS_1X1:
 677         case TPG_PAT_ALTERNATING_HLINES:
 678         case TPG_PAT_CROSS_1_PIXEL:
 679         case TPG_PAT_CROSS_2_PIXELS:
 680         case TPG_PAT_CROSS_10_PIXELS:
 681                 return 2;
 682         case TPG_PAT_100_COLORSQUARES:
 683         case TPG_PAT_100_HCOLORBAR:
 684                 return 8;
 685         default:
 686                 return 1;
 687         }
 688 }
 689
 690 /* Which pattern line should be used for the given frame line. */
 691 static unsigned tpg_get_pat_line(struct tpg_data *tpg, unsigned line)
 692 {
 693         switch (tpg->pattern) {
 694         case TPG_PAT_CHECKERS_16X16:
 695                 return (line >> 4) & 1;
 696         case TPG_PAT_CHECKERS_1X1:
 697         case TPG_PAT_ALTERNATING_HLINES:
 698                 return line & 1;
 699         case TPG_PAT_100_COLORSQUARES:
 700         case TPG_PAT_100_HCOLORBAR:
 701                 return (line * 8) / tpg->src_height;
 702         case TPG_PAT_CROSS_1_PIXEL:
 703                 return line == tpg->src_height / 2;
 704         case TPG_PAT_CROSS_2_PIXELS:
 705                 return (line + 1) / 2 == tpg->src_height / 4;
 706         case TPG_PAT_CROSS_10_PIXELS:
 707                 return (line + 10) / 20 == tpg->src_height / 40;
 708         default:
 709                 return 0;
 710         }
 711 }
 712
 713 /*
 714  * Which color should be used for the given pattern line and X coordinate.
 715  * Note: x is in the range 0 to 2 * tpg->src_width.
 716  */
 717 static enum tpg_color tpg_get_color(struct tpg_data *tpg, unsigned pat_line, unsigned x)
 718 {
 719         /* Maximum number of bars are TPG_COLOR_MAX - otherwise, the input print code
 720            should be modified */
 721         static const enum tpg_color bars[3][8] = {
 722                 /* Standard ITU-R 75% color bar sequence */
 723                 { TPG_COLOR_CSC_WHITE,   TPG_COLOR_75_YELLOW,
 724                   TPG_COLOR_75_CYAN,     TPG_COLOR_75_GREEN,
 725                   TPG_COLOR_75_MAGENTA,  TPG_COLOR_75_RED,
 726                   TPG_COLOR_75_BLUE,     TPG_COLOR_100_BLACK, },
 727                 /* Standard ITU-R 100% color bar sequence */
 728                 { TPG_COLOR_100_WHITE,   TPG_COLOR_100_YELLOW,
 729                   TPG_COLOR_100_CYAN,    TPG_COLOR_100_GREEN,
 730                   TPG_COLOR_100_MAGENTA, TPG_COLOR_100_RED,
 731                   TPG_COLOR_100_BLUE,    TPG_COLOR_100_BLACK, },
 732                 /* Color bar sequence suitable to test CSC */
 733                 { TPG_COLOR_CSC_WHITE,   TPG_COLOR_CSC_YELLOW,
 734                   TPG_COLOR_CSC_CYAN,    TPG_COLOR_CSC_GREEN,
 735                   TPG_COLOR_CSC_MAGENTA, TPG_COLOR_CSC_RED,
 736                   TPG_COLOR_CSC_BLUE,    TPG_COLOR_CSC_BLACK, },
 737         };
 738
 739         switch (tpg->pattern) {
 740         case TPG_PAT_75_COLORBAR:
 741         case TPG_PAT_100_COLORBAR:
 742         case TPG_PAT_CSC_COLORBAR:
 743                 return bars[tpg->pattern][((x * 8) / tpg->src_width) % 8];
 744         case TPG_PAT_100_COLORSQUARES:
 745                 return bars[1][(pat_line + (x * 8) / tpg->src_width) % 8];
 746         case TPG_PAT_100_HCOLORBAR:
 747                 return bars[1][pat_line];
 748         case TPG_PAT_BLACK:
 749                 return TPG_COLOR_100_BLACK;
 750         case TPG_PAT_WHITE:
 751                 return TPG_COLOR_100_WHITE;
 752         case TPG_PAT_RED:
 753                 return TPG_COLOR_100_RED;
 754         case TPG_PAT_GREEN:
 755                 return TPG_COLOR_100_GREEN;
 756         case TPG_PAT_BLUE:
 757                 return TPG_COLOR_100_BLUE;
 758         case TPG_PAT_CHECKERS_16X16:
 759                 return (((x >> 4) & 1) ^ (pat_line & 1)) ?
 760                         TPG_COLOR_100_BLACK : TPG_COLOR_100_WHITE;
 761         case TPG_PAT_CHECKERS_1X1:
 762                 return ((x & 1) ^ (pat_line & 1)) ?
 763                         TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
 764         case TPG_PAT_ALTERNATING_HLINES:
 765                 return pat_line ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
 766         case TPG_PAT_ALTERNATING_VLINES:
 767                 return (x & 1) ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
 768         case TPG_PAT_CROSS_1_PIXEL:
 769                 if (pat_line || (x % tpg->src_width) == tpg->src_width / 2)
 770                         return TPG_COLOR_100_BLACK;
 771                 return TPG_COLOR_100_WHITE;
 772         case TPG_PAT_CROSS_2_PIXELS:
 773                 if (pat_line || ((x % tpg->src_width) + 1) / 2 == tpg->src_width / 4)
 774                         return TPG_COLOR_100_BLACK;
 775                 return TPG_COLOR_100_WHITE;
 776         case TPG_PAT_CROSS_10_PIXELS:
 777                 if (pat_line || ((x % tpg->src_width) + 10) / 20 == tpg->src_width / 40)
 778                         return TPG_COLOR_100_BLACK;
 779                 return TPG_COLOR_100_WHITE;
 780         case TPG_PAT_GRAY_RAMP:
 781                 return TPG_COLOR_RAMP + ((x % tpg->src_width) * 256) / tpg->src_width;
 782         default:
 783                 return TPG_COLOR_100_RED;
 784         }
 785 }
 786
 787 /*
 788  * Given the pixel aspect ratio and video aspect ratio calculate the
 789  * coordinates of a centered square and the coordinates of the border of
 790  * the active video area. The coordinates are relative to the source
 791  * frame rectangle.
 792  */
 793 static void tpg_calculate_square_border(struct tpg_data *tpg)
 794 {
 795         unsigned w = tpg->src_width;
 796         unsigned h = tpg->src_height;
 797         unsigned sq_w, sq_h;
 798
 799         sq_w = (w * 2 / 5) & ~1;
 800         if (((w - sq_w) / 2) & 1)
 801                 sq_w += 2;
 802         sq_h = sq_w;
 803         tpg->square.width = sq_w;
 804         if (tpg->vid_aspect == TPG_VIDEO_ASPECT_16X9_ANAMORPHIC) {
 805                 unsigned ana_sq_w = (sq_w / 4) * 3;
 806
 807                 if (((w - ana_sq_w) / 2) & 1)
 808                         ana_sq_w += 2;
 809                 tpg->square.width = ana_sq_w;
 810         }
 811         tpg->square.left = (w - tpg->square.width) / 2;
 812         if (tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC)
 813                 sq_h = sq_w * 10 / 11;
 814         else if (tpg->pix_aspect == TPG_PIXEL_ASPECT_PAL)
 815                 sq_h = sq_w * 59 / 54;
 816         tpg->square.height = sq_h;
 817         tpg->square.top = (h - sq_h) / 2;
 818         tpg->border.left = 0;
 819         tpg->border.width = w;
 820         tpg->border.top = 0;
 821         tpg->border.height = h;
 822         switch (tpg->vid_aspect) {
 823         case TPG_VIDEO_ASPECT_4X3:
 824                 if (tpg->pix_aspect)
 825                         return;
 826                 if (3 * w >= 4 * h) {
 827                         tpg->border.width = ((4 * h) / 3) & ~1;
 828                         if (((w - tpg->border.width) / 2) & ~1)
 829                                 tpg->border.width -= 2;
 830                         tpg->border.left = (w - tpg->border.width) / 2;
 831                         break;
 832                 }
 833                 tpg->border.height = ((3 * w) / 4) & ~1;
 834                 tpg->border.top = (h - tpg->border.height) / 2;
 835                 break;
 836         case TPG_VIDEO_ASPECT_14X9_CENTRE:
 837                 if (tpg->pix_aspect) {
 838                         tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 420 : 506;
 839                         tpg->border.top = (h - tpg->border.height) / 2;
 840                         break;
 841                 }
 842                 if (9 * w >= 14 * h) {
 843                         tpg->border.width = ((14 * h) / 9) & ~1;
 844                         if (((w - tpg->border.width) / 2) & ~1)
 845                                 tpg->border.width -= 2;
 846                         tpg->border.left = (w - tpg->border.width) / 2;
 847                         break;
 848                 }
 849                 tpg->border.height = ((9 * w) / 14) & ~1;
 850                 tpg->border.top = (h - tpg->border.height) / 2;
 851                 break;
 852         case TPG_VIDEO_ASPECT_16X9_CENTRE:
 853                 if (tpg->pix_aspect) {
 854                         tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 368 : 442;
 855                         tpg->border.top = (h - tpg->border.height) / 2;
 856                         break;
 857                 }
 858                 if (9 * w >= 16 * h) {
 859                         tpg->border.width = ((16 * h) / 9) & ~1;
 860                         if (((w - tpg->border.width) / 2) & ~1)
 861                                 tpg->border.width -= 2;
 862                         tpg->border.left = (w - tpg->border.width) / 2;
 863                         break;
 864                 }
 865                 tpg->border.height = ((9 * w) / 16) & ~1;
 866                 tpg->border.top = (h - tpg->border.height) / 2;
 867                 break;
 868         default:
 869                 break;
 870         }
 871 }
 872
 873 static void tpg_precalculate_line(struct tpg_data *tpg)
 874 {
 875         enum tpg_color contrast;
 876         unsigned pat;
 877         unsigned p;
 878         unsigned x;
 879
 880         switch (tpg->pattern) {
 881         case TPG_PAT_GREEN:
 882                 contrast = TPG_COLOR_100_RED;
 883                 break;
 884         case TPG_PAT_CSC_COLORBAR:
 885                 contrast = TPG_COLOR_CSC_GREEN;
 886                 break;
 887         default:
 888                 contrast = TPG_COLOR_100_GREEN;
 889                 break;
 890         }
 891
 892         for (pat = 0; pat < tpg_get_pat_lines(tpg); pat++) {
 893                 /* Coarse scaling with Bresenham */
 894                 unsigned int_part = tpg->src_width / tpg->scaled_width;
 895                 unsigned fract_part = tpg->src_width % tpg->scaled_width;
 896                 unsigned src_x = 0;
 897                 unsigned error = 0;
 898
 899                 for (x = 0; x < tpg->scaled_width * 2; x += 2) {
 900                         unsigned real_x = src_x;
 901                         enum tpg_color color1, color2;
 902                         u8 pix[TPG_MAX_PLANES][8];
 903
 904                         real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
 905                         color1 = tpg_get_color(tpg, pat, real_x);
 906
 907                         src_x += int_part;
 908                         error += fract_part;
 909                         if (error >= tpg->scaled_width) {
 910                                 error -= tpg->scaled_width;
 911                                 src_x++;
 912                         }
 913
 914                         real_x = src_x;
 915                         real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
 916                         color2 = tpg_get_color(tpg, pat, real_x);
 917
 918                         src_x += int_part;
 919                         error += fract_part;
 920                         if (error >= tpg->scaled_width) {
 921                                 error -= tpg->scaled_width;
 922                                 src_x++;
 923                         }
 924
 925                         gen_twopix(tpg, pix, tpg->hflip ? color2 : color1, 0);
 926                         gen_twopix(tpg, pix, tpg->hflip ? color1 : color2, 1);
 927                         for (p = 0; p < tpg->planes; p++) {
 928                                 unsigned twopixsize = tpg->twopixelsize[p];
 929                                 u8 *pos = tpg->lines[pat][p] + x * twopixsize / 2;
 930
 931                                 memcpy(pos, pix[p], twopixsize);
 932                         }
 933                 }
 934         }
 935         for (x = 0; x < tpg->scaled_width; x += 2) {
 936                 u8 pix[TPG_MAX_PLANES][8];
 937
 938                 gen_twopix(tpg, pix, contrast, 0);
 939                 gen_twopix(tpg, pix, contrast, 1);
 940                 for (p = 0; p < tpg->planes; p++) {
 941                         unsigned twopixsize = tpg->twopixelsize[p];
 942                         u8 *pos = tpg->contrast_line[p] + x * twopixsize / 2;
 943
 944                         memcpy(pos, pix[p], twopixsize);
 945                 }
 946         }
 947         for (x = 0; x < tpg->scaled_width; x += 2) {
 948                 u8 pix[TPG_MAX_PLANES][8];
 949
 950                 gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 0);
 951                 gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 1);
 952                 for (p = 0; p < tpg->planes; p++) {
 953                         unsigned twopixsize = tpg->twopixelsize[p];
 954                         u8 *pos = tpg->black_line[p] + x * twopixsize / 2;
 955
 956                         memcpy(pos, pix[p], twopixsize);
 957                 }
 958         }
 959         for (x = 0; x < tpg->scaled_width * 2; x += 2) {
 960                 u8 pix[TPG_MAX_PLANES][8];
 961
 962                 gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 0);
 963                 gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 1);
 964                 for (p = 0; p < tpg->planes; p++) {
 965                         unsigned twopixsize = tpg->twopixelsize[p];
 966                         u8 *pos = tpg->random_line[p] + x * twopixsize / 2;
 967
 968                         memcpy(pos, pix[p], twopixsize);
 969                 }
 970         }
 971         gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 0);
 972         gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 1);
 973         gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 0);
 974         gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 1);
 975 }
 976
 977 /* need this to do rgb24 rendering */
 978 typedef struct { u16 __; u8 _; } __packed x24;
 979
 980 void tpg_gen_text(struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
 981                 int y, int x, char *text)
 982 {
 983         int line;
 984         unsigned step = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
 985         unsigned div = step;
 986         unsigned first = 0;
 987         unsigned len = strlen(text);
 988         unsigned p;
 989
 990         if (font8x16 == NULL || basep == NULL)
 991                 return;
 992
 993         /* Checks if it is possible to show string */
 994         if (y + 16 >= tpg->compose.height || x + 8 >= tpg->compose.width)
 995                 return;
 996
 997         if (len > (tpg->compose.width - x) / 8)
 998                 len = (tpg->compose.width - x) / 8;
 999         if (tpg->vflip)
1000                 y = tpg->compose.height - y - 16;
1001         if (tpg->hflip)
1002                 x = tpg->compose.width - x - 8;
1003         y += tpg->compose.top;
1004         x += tpg->compose.left;
1005         if (tpg->field == V4L2_FIELD_BOTTOM)
1006                 first = 1;
1007         else if (tpg->field == V4L2_FIELD_SEQ_TB || tpg->field == V4L2_FIELD_SEQ_BT)
1008                 div = 2;
1009
1010         for (p = 0; p < tpg->planes; p++) {
1011                 /* Print stream time */
1012 #define PRINTSTR(PIXTYPE) do {  \
1013         PIXTYPE fg;     \
1014         PIXTYPE bg;     \
1015         memcpy(&fg, tpg->textfg[p], sizeof(PIXTYPE));   \
1016         memcpy(&bg, tpg->textbg[p], sizeof(PIXTYPE));   \
1017         \
1018         for (line = first; line < 16; line += step) {   \
1019                 int l = tpg->vflip ? 15 - line : line; \
1020                 PIXTYPE *pos = (PIXTYPE *)(basep[p][line & 1] + \
1021                                ((y * step + l) / div) * tpg->bytesperline[p] + \
1022                                x * sizeof(PIXTYPE));    \
1023                 unsigned s;     \
1024         \
1025                 for (s = 0; s < len; s++) {     \
1026                         u8 chr = font8x16[text[s] * 16 + line]; \
1027         \
1028                         if (tpg->hflip) { \
1029                                 pos[7] = (chr & (0x01 << 7) ? fg : bg); \
1030                                 pos[6] = (chr & (0x01 << 6) ? fg : bg); \
1031                                 pos[5] = (chr & (0x01 << 5) ? fg : bg); \
1032                                 pos[4] = (chr & (0x01 << 4) ? fg : bg); \
1033                                 pos[3] = (chr & (0x01 << 3) ? fg : bg); \
1034                                 pos[2] = (chr & (0x01 << 2) ? fg : bg); \
1035                                 pos[1] = (chr & (0x01 << 1) ? fg : bg); \
1036                                 pos[0] = (chr & (0x01 << 0) ? fg : bg); \
1037                         } else { \
1038                                 pos[0] = (chr & (0x01 << 7) ? fg : bg); \
1039                                 pos[1] = (chr & (0x01 << 6) ? fg : bg); \
1040                                 pos[2] = (chr & (0x01 << 5) ? fg : bg); \
1041                                 pos[3] = (chr & (0x01 << 4) ? fg : bg); \
1042                                 pos[4] = (chr & (0x01 << 3) ? fg : bg); \
1043                                 pos[5] = (chr & (0x01 << 2) ? fg : bg); \
1044                                 pos[6] = (chr & (0x01 << 1) ? fg : bg); \
1045                                 pos[7] = (chr & (0x01 << 0) ? fg : bg); \
1046                         } \
1047         \
1048                         pos += tpg->hflip ? -8 : 8;     \
1049                 }       \
1050         }       \
1051 } while (0)
1052
1053                 switch (tpg->twopixelsize[p]) {
1054                 case 2:
1055                         PRINTSTR(u8); break;
1056                 case 4:
1057                         PRINTSTR(u16); break;
1058                 case 6:
1059                         PRINTSTR(x24); break;
1060                 case 8:
1061                         PRINTSTR(u32); break;
1062                 }
1063         }
1064 }
1065
1066 void tpg_update_mv_step(struct tpg_data *tpg)
1067 {
1068         int factor = tpg->mv_hor_mode > TPG_MOVE_NONE ? -1 : 1;
1069
1070         if (tpg->hflip)
1071                 factor = -factor;
1072         switch (tpg->mv_hor_mode) {
1073         case TPG_MOVE_NEG_FAST:
1074         case TPG_MOVE_POS_FAST:
1075                 tpg->mv_hor_step = ((tpg->src_width + 319) / 320) * 4;
1076                 break;
1077         case TPG_MOVE_NEG:
1078         case TPG_MOVE_POS:
1079                 tpg->mv_hor_step = ((tpg->src_width + 639) / 640) * 4;
1080                 break;
1081         case TPG_MOVE_NEG_SLOW:
1082         case TPG_MOVE_POS_SLOW:
1083                 tpg->mv_hor_step = 2;
1084                 break;
1085         case TPG_MOVE_NONE:
1086                 tpg->mv_hor_step = 0;
1087                 break;
1088         }
1089         if (factor < 0)
1090                 tpg->mv_hor_step = tpg->src_width - tpg->mv_hor_step;
1091
1092         factor = tpg->mv_vert_mode > TPG_MOVE_NONE ? -1 : 1;
1093         switch (tpg->mv_vert_mode) {
1094         case TPG_MOVE_NEG_FAST:
1095         case TPG_MOVE_POS_FAST:
1096                 tpg->mv_vert_step = ((tpg->src_width + 319) / 320) * 4;
1097                 break;
1098         case TPG_MOVE_NEG:
1099         case TPG_MOVE_POS:
1100                 tpg->mv_vert_step = ((tpg->src_width + 639) / 640) * 4;
1101                 break;
1102         case TPG_MOVE_NEG_SLOW:
1103         case TPG_MOVE_POS_SLOW:
1104                 tpg->mv_vert_step = 1;
1105                 break;
1106         case TPG_MOVE_NONE:
1107                 tpg->mv_vert_step = 0;
1108                 break;
1109         }
1110         if (factor < 0)
1111                 tpg->mv_vert_step = tpg->src_height - tpg->mv_vert_step;
1112 }
1113
1114 /* Map the line number relative to the crop rectangle to a frame line number */
1115 static unsigned tpg_calc_frameline(struct tpg_data *tpg, unsigned src_y,
1116                                     unsigned field)
1117 {
1118         switch (field) {
1119         case V4L2_FIELD_TOP:
1120                 return tpg->crop.top + src_y * 2;
1121         case V4L2_FIELD_BOTTOM:
1122                 return tpg->crop.top + src_y * 2 + 1;
1123         default:
1124                 return src_y + tpg->crop.top;
1125         }
1126 }
1127
1128 /*
1129  * Map the line number relative to the compose rectangle to a destination
1130  * buffer line number.
1131  */
1132 static unsigned tpg_calc_buffer_line(struct tpg_data *tpg, unsigned y,
1133                                     unsigned field)
1134 {
1135         y += tpg->compose.top;
1136         switch (field) {
1137         case V4L2_FIELD_SEQ_TB:
1138                 if (y & 1)
1139                         return tpg->buf_height / 2 + y / 2;
1140                 return y / 2;
1141         case V4L2_FIELD_SEQ_BT:
1142                 if (y & 1)
1143                         return y / 2;
1144                 return tpg->buf_height / 2 + y / 2;
1145         default:
1146                 return y;
1147         }
1148 }
1149
1150 static void tpg_recalc(struct tpg_data *tpg)
1151 {
1152         if (tpg->recalc_colors) {
1153                 tpg->recalc_colors = false;
1154                 tpg->recalc_lines = true;
1155                 tpg_precalculate_colors(tpg);
1156         }
1157         if (tpg->recalc_square_border) {
1158                 tpg->recalc_square_border = false;
1159                 tpg_calculate_square_border(tpg);
1160         }
1161         if (tpg->recalc_lines) {
1162                 tpg->recalc_lines = false;
1163                 tpg_precalculate_line(tpg);
1164         }
1165 }
1166
1167 void tpg_calc_text_basep(struct tpg_data *tpg,
1168                 u8 *basep[TPG_MAX_PLANES][2], unsigned p, u8 *vbuf)
1169 {
1170         unsigned stride = tpg->bytesperline[p];
1171
1172         tpg_recalc(tpg);
1173
1174         basep[p][0] = vbuf;
1175         basep[p][1] = vbuf;
1176         if (tpg->field == V4L2_FIELD_SEQ_TB)
1177                 basep[p][1] += tpg->buf_height * stride / 2;
1178         else if (tpg->field == V4L2_FIELD_SEQ_BT)
1179                 basep[p][0] += tpg->buf_height * stride / 2;
1180 }
1181
1182 void tpg_fillbuffer(struct tpg_data *tpg, v4l2_std_id std, unsigned p, u8 *vbuf)
1183 {
1184         bool is_tv = std;
1185         bool is_60hz = is_tv && (std & V4L2_STD_525_60);
1186         unsigned mv_hor_old = tpg->mv_hor_count % tpg->src_width;
1187         unsigned mv_hor_new = (tpg->mv_hor_count + tpg->mv_hor_step) % tpg->src_width;
1188         unsigned mv_vert_old = tpg->mv_vert_count % tpg->src_height;
1189         unsigned mv_vert_new = (tpg->mv_vert_count + tpg->mv_vert_step) % tpg->src_height;
1190         unsigned wss_width;
1191         unsigned f;
1192         int hmax = (tpg->compose.height * tpg->perc_fill) / 100;
1193         int h;
1194         unsigned twopixsize = tpg->twopixelsize[p];
1195         unsigned img_width = tpg->compose.width * twopixsize / 2;
1196         unsigned line_offset;
1197         unsigned left_pillar_width = 0;
1198         unsigned right_pillar_start = img_width;
1199         unsigned stride = tpg->bytesperline[p];
1200         unsigned factor = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
1201         u8 *orig_vbuf = vbuf;
1202
1203         /* Coarse scaling with Bresenham */
1204         unsigned int_part = (tpg->crop.height / factor) / tpg->compose.height;
1205         unsigned fract_part = (tpg->crop.height / factor) % tpg->compose.height;
1206         unsigned src_y = 0;
1207         unsigned error = 0;
1208
1209         tpg_recalc(tpg);
1210
1211         mv_hor_old = (mv_hor_old * tpg->scaled_width / tpg->src_width) & ~1;
1212         mv_hor_new = (mv_hor_new * tpg->scaled_width / tpg->src_width) & ~1;
1213         wss_width = tpg->crop.left < tpg->src_width / 2 ?
1214                         tpg->src_width / 2 - tpg->crop.left : 0;
1215         if (wss_width > tpg->crop.width)
1216                 wss_width = tpg->crop.width;
1217         wss_width = wss_width * tpg->scaled_width / tpg->src_width;
1218
1219         vbuf += tpg->compose.left * twopixsize / 2;
1220         line_offset = tpg->crop.left * tpg->scaled_width / tpg->src_width;
1221         line_offset = (line_offset & ~1) * twopixsize / 2;
1222         if (tpg->crop.left < tpg->border.left) {
1223                 left_pillar_width = tpg->border.left - tpg->crop.left;
1224                 if (left_pillar_width > tpg->crop.width)
1225                         left_pillar_width = tpg->crop.width;
1226                 left_pillar_width = (left_pillar_width * tpg->scaled_width) / tpg->src_width;
1227                 left_pillar_width = (left_pillar_width & ~1) * twopixsize / 2;
1228         }
1229         if (tpg->crop.left + tpg->crop.width > tpg->border.left + tpg->border.width) {
1230                 right_pillar_start = tpg->border.left + tpg->border.width - tpg->crop.left;
1231                 right_pillar_start = (right_pillar_start * tpg->scaled_width) / tpg->src_width;
1232                 right_pillar_start = (right_pillar_start & ~1) * twopixsize / 2;
1233                 if (right_pillar_start > img_width)
1234                         right_pillar_start = img_width;
1235         }
1236
1237         f = tpg->field == (is_60hz ? V4L2_FIELD_TOP : V4L2_FIELD_BOTTOM);
1238
1239         for (h = 0; h < tpg->compose.height; h++) {
1240                 bool even;
1241                 bool fill_blank = false;
1242                 unsigned frame_line;
1243                 unsigned buf_line;
1244                 unsigned pat_line_old;
1245                 unsigned pat_line_new;
1246                 u8 *linestart_older;
1247                 u8 *linestart_newer;
1248                 u8 *linestart_top;
1249                 u8 *linestart_bottom;
1250
1251                 frame_line = tpg_calc_frameline(tpg, src_y, tpg->field);
1252                 even = !(frame_line & 1);
1253                 buf_line = tpg_calc_buffer_line(tpg, h, tpg->field);
1254                 src_y += int_part;
1255                 error += fract_part;
1256                 if (error >= tpg->compose.height) {
1257                         error -= tpg->compose.height;
1258                         src_y++;
1259                 }
1260
1261                 if (h >= hmax) {
1262                         if (hmax == tpg->compose.height)
1263                                 continue;
1264                         if (!tpg->perc_fill_blank)
1265                                 continue;
1266                         fill_blank = true;
1267                 }
1268
1269                 if (tpg->vflip)
1270                         frame_line = tpg->src_height - frame_line - 1;
1271
1272                 if (fill_blank) {
1273                         linestart_older = tpg->contrast_line[p];
1274                         linestart_newer = tpg->contrast_line[p];
1275                 } else if (tpg->qual != TPG_QUAL_NOISE &&
1276                            (frame_line < tpg->border.top ||
1277                             frame_line >= tpg->border.top + tpg->border.height)) {
1278                         linestart_older = tpg->black_line[p];
1279                         linestart_newer = tpg->black_line[p];
1280                 } else if (tpg->pattern == TPG_PAT_NOISE || tpg->qual == TPG_QUAL_NOISE) {
1281                         linestart_older = tpg->random_line[p] +
1282                                           twopixsize * prandom_u32_max(tpg->src_width / 2);
1283                         linestart_newer = tpg->random_line[p] +
1284                                           twopixsize * prandom_u32_max(tpg->src_width / 2);
1285                 } else {
1286                         pat_line_old = tpg_get_pat_line(tpg,
1287                                                 (frame_line + mv_vert_old) % tpg->src_height);
1288                         pat_line_new = tpg_get_pat_line(tpg,
1289                                                 (frame_line + mv_vert_new) % tpg->src_height);
1290                         linestart_older = tpg->lines[pat_line_old][p] +
1291                                           mv_hor_old * twopixsize / 2;
1292                         linestart_newer = tpg->lines[pat_line_new][p] +
1293                                           mv_hor_new * twopixsize / 2;
1294                         linestart_older += line_offset;
1295                         linestart_newer += line_offset;
1296                 }
1297                 if (is_60hz) {
1298                         linestart_top = linestart_newer;
1299                         linestart_bottom = linestart_older;
1300                 } else {
1301                         linestart_top = linestart_older;
1302                         linestart_bottom = linestart_newer;
1303                 }
1304
1305                 switch (tpg->field) {
1306                 case V4L2_FIELD_INTERLACED:
1307                 case V4L2_FIELD_INTERLACED_TB:
1308                 case V4L2_FIELD_SEQ_TB:
1309                 case V4L2_FIELD_SEQ_BT:
1310                         if (even)
1311                                 memcpy(vbuf + buf_line * stride, linestart_top, img_width);
1312                         else
1313                                 memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
1314                         break;
1315                 case V4L2_FIELD_INTERLACED_BT:
1316                         if (even)
1317                                 memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
1318                         else
1319                                 memcpy(vbuf + buf_line * stride, linestart_top, img_width);
1320                         break;
1321                 case V4L2_FIELD_TOP:
1322                         memcpy(vbuf + buf_line * stride, linestart_top, img_width);
1323                         break;
1324                 case V4L2_FIELD_BOTTOM:
1325                         memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
1326                         break;
1327                 case V4L2_FIELD_NONE:
1328                 default:
1329                         memcpy(vbuf + buf_line * stride, linestart_older, img_width);
1330                         break;
1331                 }
1332
1333                 if (is_tv && !is_60hz && frame_line == 0 && wss_width) {
1334                         /*
1335                          * Replace the first half of the top line of a 50 Hz frame
1336                          * with random data to simulate a WSS signal.
1337                          */
1338                         u8 *wss = tpg->random_line[p] +
1339                                   twopixsize * prandom_u32_max(tpg->src_width / 2);
1340
1341                         memcpy(vbuf + buf_line * stride, wss, wss_width * twopixsize / 2);
1342                 }
1343         }
1344
1345         vbuf = orig_vbuf;
1346         vbuf += tpg->compose.left * twopixsize / 2;
1347         src_y = 0;
1348         error = 0;
1349         for (h = 0; h < tpg->compose.height; h++) {
1350                 unsigned frame_line = tpg_calc_frameline(tpg, src_y, tpg->field);
1351                 unsigned buf_line = tpg_calc_buffer_line(tpg, h, tpg->field);
1352                 const struct v4l2_rect *sq = &tpg->square;
1353                 const struct v4l2_rect *b = &tpg->border;
1354                 const struct v4l2_rect *c = &tpg->crop;
1355
1356                 src_y += int_part;
1357                 error += fract_part;
1358                 if (error >= tpg->compose.height) {
1359                         error -= tpg->compose.height;
1360                         src_y++;
1361                 }
1362
1363                 if (tpg->show_border && frame_line >= b->top &&
1364                     frame_line < b->top + b->height) {
1365                         unsigned bottom = b->top + b->height - 1;
1366                         unsigned left = left_pillar_width;
1367                         unsigned right = right_pillar_start;
1368
1369                         if (frame_line == b->top || frame_line == b->top + 1 ||
1370                             frame_line == bottom || frame_line == bottom - 1) {
1371                                 memcpy(vbuf + buf_line * stride + left, tpg->contrast_line[p],
1372                                                 right - left);
1373                         } else {
1374                                 if (b->left >= c->left &&
1375                                     b->left < c->left + c->width)
1376                                         memcpy(vbuf + buf_line * stride + left,
1377                                                 tpg->contrast_line[p], twopixsize);
1378                                 if (b->left + b->width > c->left &&
1379                                     b->left + b->width <= c->left + c->width)
1380                                         memcpy(vbuf + buf_line * stride + right - twopixsize,
1381                                                 tpg->contrast_line[p], twopixsize);
1382                         }
1383                 }
1384                 if (tpg->qual != TPG_QUAL_NOISE && frame_line >= b->top &&
1385                     frame_line < b->top + b->height) {
1386                         memcpy(vbuf + buf_line * stride, tpg->black_line[p], left_pillar_width);
1387                         memcpy(vbuf + buf_line * stride + right_pillar_start, tpg->black_line[p],
1388                                img_width - right_pillar_start);
1389                 }
1390                 if (tpg->show_square && frame_line >= sq->top &&
1391                     frame_line < sq->top + sq->height &&
1392                     sq->left < c->left + c->width &&
1393                     sq->left + sq->width >= c->left) {
1394                         unsigned left = sq->left;
1395                         unsigned width = sq->width;
1396
1397                         if (c->left > left) {
1398                                 width -= c->left - left;
1399                                 left = c->left;
1400                         }
1401                         if (c->left + c->width < left + width)
1402                                 width -= left + width - c->left - c->width;
1403                         left -= c->left;
1404                         left = (left * tpg->scaled_width) / tpg->src_width;
1405                         left = (left & ~1) * twopixsize / 2;
1406                         width = (width * tpg->scaled_width) / tpg->src_width;
1407                         width = (width & ~1) * twopixsize / 2;
1408                         memcpy(vbuf + buf_line * stride + left, tpg->contrast_line[p], width);
1409                 }
1410                 if (tpg->insert_sav) {
1411                         unsigned offset = (tpg->compose.width / 6) * twopixsize;
1412                         u8 *p = vbuf + buf_line * stride + offset;
1413                         unsigned vact = 0, hact = 0;
1414
1415                         p[0] = 0xff;
1416                         p[1] = 0;
1417                         p[2] = 0;
1418                         p[3] = 0x80 | (f << 6) | (vact << 5) | (hact << 4) |
1419                                 ((hact ^ vact) << 3) |
1420                                 ((hact ^ f) << 2) |
1421                                 ((f ^ vact) << 1) |
1422                                 (hact ^ vact ^ f);
1423                 }
1424                 if (tpg->insert_eav) {
1425                         unsigned offset = (tpg->compose.width / 6) * 2 * twopixsize;
1426                         u8 *p = vbuf + buf_line * stride + offset;
1427                         unsigned vact = 0, hact = 1;
1428
1429                         p[0] = 0xff;
1430                         p[1] = 0;
1431                         p[2] = 0;
1432                         p[3] = 0x80 | (f << 6) | (vact << 5) | (hact << 4) |
1433                                 ((hact ^ vact) << 3) |
1434                                 ((hact ^ f) << 2) |
1435                                 ((f ^ vact) << 1) |
1436                                 (hact ^ vact ^ f);
1437                 }
1438         }
1439 }