stand/common/gfx_fb.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause
   3  *
   4  * Copyright 2020 Toomas Soome
   5  * Copyright 2019 OmniOS Community Edition (OmniOSce) Association.
   6  * Copyright 2020 RackTop Systems, Inc.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  *
  29  * $FreeBSD$
  30  */
  31
  32 /*
  33  * The workhorse here is gfxfb_blt(). It is implemented to mimic UEFI
  34  * GOP Blt, and allows us to fill the rectangle on screen, copy
  35  * rectangle from video to buffer and buffer to video and video to video.
  36  * Such implementation does allow us to have almost identical implementation
  37  * for both BIOS VBE and UEFI.
  38  *
  39  * ALL pixel data is assumed to be 32-bit BGRA (byte order Blue, Green, Red,
  40  * Alpha) format, this allows us to only handle RGB data and not to worry
  41  * about mixing RGB with indexed colors.
  42  * Data exchange between memory buffer and video will translate BGRA
  43  * and native format as following:
  44  *
  45  * 32-bit to/from 32-bit is trivial case.
  46  * 32-bit to/from 24-bit is also simple - we just drop the alpha channel.
  47  * 32-bit to/from 16-bit is more complicated, because we nee to handle
  48  * data loss from 32-bit to 16-bit. While reading/writing from/to video, we
  49  * need to apply masks of 16-bit color components. This will preserve
  50  * colors for terminal text. For 32-bit truecolor PMG images, we need to
  51  * translate 32-bit colors to 15/16 bit colors and this means data loss.
  52  * There are different algorithms how to perform such color space reduction,
  53  * we are currently using bitwise right shift to reduce color space and so far
  54  * this technique seems to be sufficient (see also gfx_fb_putimage(), the
  55  * end of for loop).
  56  * 32-bit to/from 8-bit is the most troublesome because 8-bit colors are
  57  * indexed. From video, we do get color indexes, and we do translate
  58  * color index values to RGB. To write to video, we again need to translate
  59  * RGB to color index. Additionally, we need to translate between VGA and
  60  * console colors.
  61  *
  62  * Our internal color data is represented using BGRA format. But the hardware
  63  * used indexed colors for 8-bit colors (0-255) and for this mode we do
  64  * need to perform translation to/from BGRA and index values.
  65  *
  66  *                   - paletteentry RGB <-> index -
  67  * BGRA BUFFER <----/                              \ - VIDEO
  68  *                  \                              /
  69  *                   -  RGB (16/24/32)            -
  70  *
  71  * To perform index to RGB translation, we use palette table generated
  72  * from when we set up 8-bit mode video. We cannot read palette data from
  73  * the hardware, because not all hardware supports reading it.
  74  *
  75  * BGRA to index is implemented in rgb_to_color_index() by searching
  76  * palette array for closest match of RBG values.
  77  *
  78  * Note: In 8-bit mode, We do store first 16 colors to palette registers
  79  * in VGA color order, this serves two purposes; firstly,
  80  * if palette update is not supported, we still have correct 16 colors.
  81  * Secondly, the kernel does get correct 16 colors when some other boot
  82  * loader is used. However, the palette map for 8-bit colors is using
  83  * console color ordering - this does allow us to skip translation
  84  * from VGA colors to console colors, while we are reading RGB data.
  85  */
  86
  87 #include <sys/cdefs.h>
  88 #include <sys/param.h>
  89 #include <stand.h>
  90 #include <teken.h>
  91 #include <gfx_fb.h>
  92 #include <sys/font.h>
  93 #include <sys/stdint.h>
  94 #include <sys/endian.h>
  95 #include <pnglite.h>
  96 #include <bootstrap.h>
  97 #include <lz4.h>
  98 #if defined(EFI)
  99 #include <efi.h>
 100 #include <efilib.h>
 101 #else
 102 #include <vbe.h>
 103 #endif
 104
 105 /* VGA text mode does use bold font. */
 106 #if !defined(VGA_8X16_FONT)
 107 #define VGA_8X16_FONT           "/boot/fonts/8x16b.fnt"
 108 #endif
 109 #if !defined(DEFAULT_8X16_FONT)
 110 #define DEFAULT_8X16_FONT       "/boot/fonts/8x16.fnt"
 111 #endif
 112
 113 /*
 114  * Must be sorted by font size in descending order
 115  */
 116 font_list_t fonts = STAILQ_HEAD_INITIALIZER(fonts);
 117
 118 #define DEFAULT_FONT_DATA       font_data_8x16
 119 extern vt_font_bitmap_data_t    font_data_8x16;
 120 teken_gfx_t gfx_state = { 0 };
 121
 122 static struct {
 123         unsigned char r;        /* Red percentage value. */
 124         unsigned char g;        /* Green percentage value. */
 125         unsigned char b;        /* Blue percentage value. */
 126 } color_def[NCOLORS] = {
 127         {0,     0,      0},     /* black */
 128         {50,    0,      0},     /* dark red */
 129         {0,     50,     0},     /* dark green */
 130         {77,    63,     0},     /* dark yellow */
 131         {20,    40,     64},    /* dark blue */
 132         {50,    0,      50},    /* dark magenta */
 133         {0,     50,     50},    /* dark cyan */
 134         {75,    75,     75},    /* light gray */
 135
 136         {18,    20,     21},    /* dark gray */
 137         {100,   0,      0},     /* light red */
 138         {0,     100,    0},     /* light green */
 139         {100,   100,    0},     /* light yellow */
 140         {45,    62,     81},    /* light blue */
 141         {100,   0,      100},   /* light magenta */
 142         {0,     100,    100},   /* light cyan */
 143         {100,   100,    100},   /* white */
 144 };
 145 uint32_t cmap[NCMAP];
 146
 147 /*
 148  * Between console's palette and VGA's one:
 149  *  - blue and red are swapped (1 <-> 4)
 150  *  - yellow and cyan are swapped (3 <-> 6)
 151  */
 152 const int cons_to_vga_colors[NCOLORS] = {
 153         0,  4,  2,  6,  1,  5,  3,  7,
 154         8, 12, 10, 14,  9, 13, 11, 15
 155 };
 156
 157 static const int vga_to_cons_colors[NCOLORS] = {
 158         0,  1,  2,  3,  4,  5,  6,  7,
 159         8,  9, 10, 11,  12, 13, 14, 15
 160 };
 161
 162 struct text_pixel *screen_buffer;
 163 #if defined(EFI)
 164 static EFI_GRAPHICS_OUTPUT_BLT_PIXEL *GlyphBuffer;
 165 #else
 166 static struct paletteentry *GlyphBuffer;
 167 #endif
 168 static size_t GlyphBufferSize;
 169
 170 static bool insert_font(char *, FONT_FLAGS);
 171 static int font_set(struct env_var *, int, const void *);
 172 static void * allocate_glyphbuffer(uint32_t, uint32_t);
 173 static void gfx_fb_cursor_draw(teken_gfx_t *, const teken_pos_t *, bool);
 174
 175 /*
 176  * Initialize gfx framework.
 177  */
 178 void
 179 gfx_framework_init(void)
 180 {
 181         /*
 182          * Setup font list to have builtin font.
 183          */
 184         (void) insert_font(NULL, FONT_BUILTIN);
 185 }
 186
 187 static uint8_t *
 188 gfx_get_fb_address(void)
 189 {
 190         return (ptov((uint32_t)gfx_state.tg_fb.fb_addr));
 191 }
 192
 193 /*
 194  * Utility function to parse gfx mode line strings.
 195  */
 196 bool
 197 gfx_parse_mode_str(char *str, int *x, int *y, int *depth)
 198 {
 199         char *p, *end;
 200
 201         errno = 0;
 202         p = str;
 203         *x = strtoul(p, &end, 0);
 204         if (*x == 0 || errno != 0)
 205                 return (false);
 206         if (*end != 'x')
 207                 return (false);
 208         p = end + 1;
 209         *y = strtoul(p, &end, 0);
 210         if (*y == 0 || errno != 0)
 211                 return (false);
 212         if (*end != 'x') {
 213                 *depth = -1;    /* auto select */
 214         } else {
 215                 p = end + 1;
 216                 *depth = strtoul(p, &end, 0);
 217                 if (*depth == 0 || errno != 0 || *end != '\0')
 218                         return (false);
 219         }
 220
 221         return (true);
 222 }
 223
 224 static uint32_t
 225 rgb_color_map(uint8_t index, uint32_t rmax, int roffset,
 226     uint32_t gmax, int goffset, uint32_t bmax, int boffset)
 227 {
 228         uint32_t color, code, gray, level;
 229
 230         if (index < NCOLORS) {
 231 #define CF(_f, _i) ((_f ## max * color_def[(_i)]._f / 100) << _f ## offset)
 232                 return (CF(r, index) | CF(g, index) | CF(b, index));
 233 #undef  CF
 234         }
 235
 236 #define CF(_f, _c) ((_f ## max & _c) << _f ## offset)
 237         /* 6x6x6 color cube */
 238         if (index > 15 && index < 232) {
 239                 uint32_t red, green, blue;
 240
 241                 for (red = 0; red < 6; red++) {
 242                         for (green = 0; green < 6; green++) {
 243                                 for (blue = 0; blue < 6; blue++) {
 244                                         code = 16 + (red * 36) +
 245                                             (green * 6) + blue;
 246                                         if (code != index)
 247                                                 continue;
 248                                         red = red ? (red * 40 + 55) : 0;
 249                                         green = green ? (green * 40 + 55) : 0;
 250                                         blue = blue ? (blue * 40 + 55) : 0;
 251                                         color = CF(r, red);
 252                                         color |= CF(g, green);
 253                                         color |= CF(b, blue);
 254                                         return (color);
 255                                 }
 256                         }
 257                 }
 258         }
 259
 260         /* colors 232-255 are a grayscale ramp */
 261         for (gray = 0; gray < 24; gray++) {
 262                 level = (gray * 10) + 8;
 263                 code = 232 + gray;
 264                 if (code == index)
 265                         break;
 266         }
 267         return (CF(r, level) | CF(g, level) | CF(b, level));
 268 #undef  CF
 269 }
 270
 271 /*
 272  * Support for color mapping.
 273  * For 8, 24 and 32 bit depth, use mask size 8.
 274  * 15/16 bit depth needs to use mask size from mode,
 275  * or we will lose color information from 32-bit to 15/16 bit translation.
 276  */
 277 uint32_t
 278 gfx_fb_color_map(uint8_t index)
 279 {
 280         int rmask, gmask, bmask;
 281         int roff, goff, boff, bpp;
 282
 283         roff = ffs(gfx_state.tg_fb.fb_mask_red) - 1;
 284         goff = ffs(gfx_state.tg_fb.fb_mask_green) - 1;
 285         boff = ffs(gfx_state.tg_fb.fb_mask_blue) - 1;
 286         bpp = roundup2(gfx_state.tg_fb.fb_bpp, 8) >> 3;
 287
 288         if (bpp == 2)
 289                 rmask = gfx_state.tg_fb.fb_mask_red >> roff;
 290         else
 291                 rmask = 0xff;
 292
 293         if (bpp == 2)
 294                 gmask = gfx_state.tg_fb.fb_mask_green >> goff;
 295         else
 296                 gmask = 0xff;
 297
 298         if (bpp == 2)
 299                 bmask = gfx_state.tg_fb.fb_mask_blue >> boff;
 300         else
 301                 bmask = 0xff;
 302
 303         return (rgb_color_map(index, rmask, 16, gmask, 8, bmask, 0));
 304 }
 305
 306 /*
 307  * Get indexed color from RGB. This function is used to write data to video
 308  * memory when the adapter is set to use indexed colors.
 309  * Since UEFI does only support 32-bit colors, we do not implement it for
 310  * UEFI because there is no need for it and we do not have palette array
 311  * for UEFI.
 312  */
 313 static uint8_t
 314 rgb_to_color_index(uint8_t r, uint8_t g, uint8_t b)
 315 {
 316 #if !defined(EFI)
 317         uint32_t color, best, dist, k;
 318         int diff;
 319
 320         color = 0;
 321         best = 255 * 255 * 255;
 322         for (k = 0; k < NCMAP; k++) {
 323                 diff = r - pe8[k].Red;
 324                 dist = diff * diff;
 325                 diff = g - pe8[k].Green;
 326                 dist += diff * diff;
 327                 diff = b - pe8[k].Blue;
 328                 dist += diff * diff;
 329
 330                 /* Exact match, exit the loop */
 331                 if (dist == 0)
 332                         break;
 333
 334                 if (dist < best) {
 335                         color = k;
 336                         best = dist;
 337                 }
 338         }
 339         if (k == NCMAP)
 340                 k = color;
 341         return (k);
 342 #else
 343         (void) r;
 344         (void) g;
 345         (void) b;
 346         return (0);
 347 #endif
 348 }
 349
 350 int
 351 generate_cons_palette(uint32_t *palette, int format,
 352     uint32_t rmax, int roffset, uint32_t gmax, int goffset,
 353     uint32_t bmax, int boffset)
 354 {
 355         int i;
 356
 357         switch (format) {
 358         case COLOR_FORMAT_VGA:
 359                 for (i = 0; i < NCOLORS; i++)
 360                         palette[i] = cons_to_vga_colors[i];
 361                 for (; i < NCMAP; i++)
 362                         palette[i] = i;
 363                 break;
 364         case COLOR_FORMAT_RGB:
 365                 for (i = 0; i < NCMAP; i++)
 366                         palette[i] = rgb_color_map(i, rmax, roffset,
 367                             gmax, goffset, bmax, boffset);
 368                 break;
 369         default:
 370                 return (ENODEV);
 371         }
 372
 373         return (0);
 374 }
 375
 376 static void
 377 gfx_mem_wr1(uint8_t *base, size_t size, uint32_t o, uint8_t v)
 378 {
 379
 380         if (o >= size)
 381                 return;
 382         *(uint8_t *)(base + o) = v;
 383 }
 384
 385 static void
 386 gfx_mem_wr2(uint8_t *base, size_t size, uint32_t o, uint16_t v)
 387 {
 388
 389         if (o >= size)
 390                 return;
 391         *(uint16_t *)(base + o) = v;
 392 }
 393
 394 static void
 395 gfx_mem_wr4(uint8_t *base, size_t size, uint32_t o, uint32_t v)
 396 {
 397
 398         if (o >= size)
 399                 return;
 400         *(uint32_t *)(base + o) = v;
 401 }
 402
 403 static int gfxfb_blt_fill(void *BltBuffer,
 404     uint32_t DestinationX, uint32_t DestinationY,
 405     uint32_t Width, uint32_t Height)
 406 {
 407 #if defined(EFI)
 408         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *p;
 409 #else
 410         struct paletteentry *p;
 411 #endif
 412         uint32_t data, bpp, pitch, y, x;
 413         int roff, goff, boff;
 414         size_t size;
 415         off_t off;
 416         uint8_t *destination;
 417
 418         if (BltBuffer == NULL)
 419                 return (EINVAL);
 420
 421         if (DestinationY + Height > gfx_state.tg_fb.fb_height)
 422                 return (EINVAL);
 423
 424         if (DestinationX + Width > gfx_state.tg_fb.fb_width)
 425                 return (EINVAL);
 426
 427         if (Width == 0 || Height == 0)
 428                 return (EINVAL);
 429
 430         p = BltBuffer;
 431         roff = ffs(gfx_state.tg_fb.fb_mask_red) - 1;
 432         goff = ffs(gfx_state.tg_fb.fb_mask_green) - 1;
 433         boff = ffs(gfx_state.tg_fb.fb_mask_blue) - 1;
 434
 435         if (gfx_state.tg_fb.fb_bpp == 8) {
 436                 data = rgb_to_color_index(p->Red, p->Green, p->Blue);
 437         } else {
 438                 data = (p->Red &
 439                     (gfx_state.tg_fb.fb_mask_red >> roff)) << roff;
 440                 data |= (p->Green &
 441                     (gfx_state.tg_fb.fb_mask_green >> goff)) << goff;
 442                 data |= (p->Blue &
 443                     (gfx_state.tg_fb.fb_mask_blue >> boff)) << boff;
 444         }
 445
 446         bpp = roundup2(gfx_state.tg_fb.fb_bpp, 8) >> 3;
 447         pitch = gfx_state.tg_fb.fb_stride * bpp;
 448         destination = gfx_get_fb_address();
 449         size = gfx_state.tg_fb.fb_size;
 450
 451         for (y = DestinationY; y < Height + DestinationY; y++) {
 452                 off = y * pitch + DestinationX * bpp;
 453                 for (x = 0; x < Width; x++) {
 454                         switch (bpp) {
 455                         case 1:
 456                                 gfx_mem_wr1(destination, size, off,
 457                                     (data < NCOLORS) ?
 458                                     cons_to_vga_colors[data] : data);
 459                                 break;
 460                         case 2:
 461                                 gfx_mem_wr2(destination, size, off, data);
 462                                 break;
 463                         case 3:
 464                                 gfx_mem_wr1(destination, size, off,
 465                                     (data >> 16) & 0xff);
 466                                 gfx_mem_wr1(destination, size, off + 1,
 467                                     (data >> 8) & 0xff);
 468                                 gfx_mem_wr1(destination, size, off + 2,
 469                                     data & 0xff);
 470                                 break;
 471                         case 4:
 472                                 gfx_mem_wr4(destination, size, off, data);
 473                                 break;
 474                         default:
 475                                 return (EINVAL);
 476                         }
 477                         off += bpp;
 478                 }
 479         }
 480
 481         return (0);
 482 }
 483
 484 static int
 485 gfxfb_blt_video_to_buffer(void *BltBuffer, uint32_t SourceX, uint32_t SourceY,
 486     uint32_t DestinationX, uint32_t DestinationY,
 487     uint32_t Width, uint32_t Height, uint32_t Delta)
 488 {
 489 #if defined(EFI)
 490         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *p;
 491 #else
 492         struct paletteentry *p;
 493 #endif
 494         uint32_t x, sy, dy;
 495         uint32_t bpp, pitch, copybytes;
 496         off_t off;
 497         uint8_t *source, *destination, *sb;
 498         uint8_t rm, rp, gm, gp, bm, bp;
 499         bool bgra;
 500
 501         if (BltBuffer == NULL)
 502                 return (EINVAL);
 503
 504         if (SourceY + Height >
 505             gfx_state.tg_fb.fb_height)
 506                 return (EINVAL);
 507
 508         if (SourceX + Width > gfx_state.tg_fb.fb_width)
 509                 return (EINVAL);
 510
 511         if (Width == 0 || Height == 0)
 512                 return (EINVAL);
 513
 514         if (Delta == 0)
 515                 Delta = Width * sizeof (*p);
 516
 517         bpp = roundup2(gfx_state.tg_fb.fb_bpp, 8) >> 3;
 518         pitch = gfx_state.tg_fb.fb_stride * bpp;
 519
 520         copybytes = Width * bpp;
 521
 522         rp = ffs(gfx_state.tg_fb.fb_mask_red) - 1;
 523         gp = ffs(gfx_state.tg_fb.fb_mask_green) - 1;
 524         bp = ffs(gfx_state.tg_fb.fb_mask_blue) - 1;
 525         rm = gfx_state.tg_fb.fb_mask_red >> rp;
 526         gm = gfx_state.tg_fb.fb_mask_green >> gp;
 527         bm = gfx_state.tg_fb.fb_mask_blue >> bp;
 528
 529         /* If FB pixel format is BGRA, we can use direct copy. */
 530         bgra = bpp == 4 &&
 531             ffs(rm) - 1 == 8 && rp == 16 &&
 532             ffs(gm) - 1 == 8 && gp == 8 &&
 533             ffs(bm) - 1 == 8 && bp == 0;
 534
 535         for (sy = SourceY, dy = DestinationY; dy < Height + DestinationY;
 536             sy++, dy++) {
 537                 off = sy * pitch + SourceX * bpp;
 538                 source = gfx_get_fb_address() + off;
 539                 destination = (uint8_t *)BltBuffer + dy * Delta +
 540                     DestinationX * sizeof (*p);
 541
 542                 if (bgra) {
 543                         bcopy(source, destination, copybytes);
 544                 } else {
 545                         for (x = 0; x < Width; x++) {
 546                                 uint32_t c = 0;
 547
 548                                 p = (void *)(destination + x * sizeof (*p));
 549                                 sb = source + x * bpp;
 550                                 switch (bpp) {
 551                                 case 1:
 552                                         c = *sb;
 553                                         break;
 554                                 case 2:
 555                                         c = *(uint16_t *)sb;
 556                                         break;
 557                                 case 3:
 558                                         c = sb[0] << 16 | sb[1] << 8 | sb[2];
 559                                         break;
 560                                 case 4:
 561                                         c = *(uint32_t *)sb;
 562                                         break;
 563                                 default:
 564                                         return (EINVAL);
 565                                 }
 566
 567                                 if (bpp == 1) {
 568                                         *(uint32_t *)p = gfx_fb_color_map(
 569                                             (c < 16) ?
 570                                             vga_to_cons_colors[c] : c);
 571                                 } else {
 572                                         p->Red = (c >> rp) & rm;
 573                                         p->Green = (c >> gp) & gm;
 574                                         p->Blue = (c >> bp) & bm;
 575                                         p->Reserved = 0;
 576                                 }
 577                         }
 578                 }
 579         }
 580
 581         return (0);
 582 }
 583
 584 static int
 585 gfxfb_blt_buffer_to_video(void *BltBuffer, uint32_t SourceX, uint32_t SourceY,
 586     uint32_t DestinationX, uint32_t DestinationY,
 587     uint32_t Width, uint32_t Height, uint32_t Delta)
 588 {
 589 #if defined(EFI)
 590         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *p;
 591 #else
 592         struct paletteentry *p;
 593 #endif
 594         uint32_t x, sy, dy;
 595         uint32_t bpp, pitch, copybytes;
 596         off_t off;
 597         uint8_t *source, *destination;
 598         uint8_t rm, rp, gm, gp, bm, bp;
 599         bool bgra;
 600
 601         if (BltBuffer == NULL)
 602                 return (EINVAL);
 603
 604         if (DestinationY + Height >
 605             gfx_state.tg_fb.fb_height)
 606                 return (EINVAL);
 607
 608         if (DestinationX + Width > gfx_state.tg_fb.fb_width)
 609                 return (EINVAL);
 610
 611         if (Width == 0 || Height == 0)
 612                 return (EINVAL);
 613
 614         if (Delta == 0)
 615                 Delta = Width * sizeof (*p);
 616
 617         bpp = roundup2(gfx_state.tg_fb.fb_bpp, 8) >> 3;
 618         pitch = gfx_state.tg_fb.fb_stride * bpp;
 619
 620         copybytes = Width * bpp;
 621
 622         rp = ffs(gfx_state.tg_fb.fb_mask_red) - 1;
 623         gp = ffs(gfx_state.tg_fb.fb_mask_green) - 1;
 624         bp = ffs(gfx_state.tg_fb.fb_mask_blue) - 1;
 625         rm = gfx_state.tg_fb.fb_mask_red >> rp;
 626         gm = gfx_state.tg_fb.fb_mask_green >> gp;
 627         bm = gfx_state.tg_fb.fb_mask_blue >> bp;
 628
 629         /* If FB pixel format is BGRA, we can use direct copy. */
 630         bgra = bpp == 4 &&
 631             ffs(rm) - 1 == 8 && rp == 16 &&
 632             ffs(gm) - 1 == 8 && gp == 8 &&
 633             ffs(bm) - 1 == 8 && bp == 0;
 634
 635         for (sy = SourceY, dy = DestinationY; sy < Height + SourceY;
 636             sy++, dy++) {
 637                 off = dy * pitch + DestinationX * bpp;
 638                 destination = gfx_get_fb_address() + off;
 639
 640                 if (bgra) {
 641                         source = (uint8_t *)BltBuffer + sy * Delta +
 642                             SourceX * sizeof (*p);
 643                         bcopy(source, destination, copybytes);
 644                 } else {
 645                         for (x = 0; x < Width; x++) {
 646                                 uint32_t c;
 647
 648                                 p = (void *)((uint8_t *)BltBuffer +
 649                                     sy * Delta +
 650                                     (SourceX + x) * sizeof (*p));
 651                                 if (bpp == 1) {
 652                                         c = rgb_to_color_index(p->Red,
 653                                             p->Green, p->Blue);
 654                                 } else {
 655                                         c = (p->Red & rm) << rp |
 656                                             (p->Green & gm) << gp |
 657                                             (p->Blue & bm) << bp;
 658                                 }
 659                                 off = x * bpp;
 660                                 switch (bpp) {
 661                                 case 1:
 662                                         gfx_mem_wr1(destination, copybytes,
 663                                             off, (c < 16) ?
 664                                             cons_to_vga_colors[c] : c);
 665                                         break;
 666                                 case 2:
 667                                         gfx_mem_wr2(destination, copybytes,
 668                                             off, c);
 669                                         break;
 670                                 case 3:
 671                                         gfx_mem_wr1(destination, copybytes,
 672                                             off, (c >> 16) & 0xff);
 673                                         gfx_mem_wr1(destination, copybytes,
 674                                             off + 1, (c >> 8) & 0xff);
 675                                         gfx_mem_wr1(destination, copybytes,
 676                                             off + 2, c & 0xff);
 677                                         break;
 678                                 case 4:
 679                                         gfx_mem_wr4(destination, copybytes,
 680                                             x * bpp, c);
 681                                         break;
 682                                 default:
 683                                         return (EINVAL);
 684                                 }
 685                         }
 686                 }
 687         }
 688
 689         return (0);
 690 }
 691
 692 static int
 693 gfxfb_blt_video_to_video(uint32_t SourceX, uint32_t SourceY,
 694     uint32_t DestinationX, uint32_t DestinationY,
 695     uint32_t Width, uint32_t Height)
 696 {
 697         uint32_t bpp, copybytes;
 698         int pitch;
 699         uint8_t *source, *destination;
 700         off_t off;
 701
 702         if (SourceY + Height >
 703             gfx_state.tg_fb.fb_height)
 704                 return (EINVAL);
 705
 706         if (SourceX + Width > gfx_state.tg_fb.fb_width)
 707                 return (EINVAL);
 708
 709         if (DestinationY + Height >
 710             gfx_state.tg_fb.fb_height)
 711                 return (EINVAL);
 712
 713         if (DestinationX + Width > gfx_state.tg_fb.fb_width)
 714                 return (EINVAL);
 715
 716         if (Width == 0 || Height == 0)
 717                 return (EINVAL);
 718
 719         bpp = roundup2(gfx_state.tg_fb.fb_bpp, 8) >> 3;
 720         pitch = gfx_state.tg_fb.fb_stride * bpp;
 721
 722         copybytes = Width * bpp;
 723
 724         off = SourceY * pitch + SourceX * bpp;
 725         source = gfx_get_fb_address() + off;
 726         off = DestinationY * pitch + DestinationX * bpp;
 727         destination = gfx_get_fb_address() + off;
 728
 729         if ((uintptr_t)destination > (uintptr_t)source) {
 730                 source += Height * pitch;
 731                 destination += Height * pitch;
 732                 pitch = -pitch;
 733         }
 734
 735         while (Height-- > 0) {
 736                 bcopy(source, destination, copybytes);
 737                 source += pitch;
 738                 destination += pitch;
 739         }
 740
 741         return (0);
 742 }
 743
 744 int
 745 gfxfb_blt(void *BltBuffer, GFXFB_BLT_OPERATION BltOperation,
 746     uint32_t SourceX, uint32_t SourceY,
 747     uint32_t DestinationX, uint32_t DestinationY,
 748     uint32_t Width, uint32_t Height, uint32_t Delta)
 749 {
 750         int rv;
 751 #if defined(EFI)
 752         EFI_STATUS status;
 753         EFI_GRAPHICS_OUTPUT *gop = gfx_state.tg_private;
 754
 755         /*
 756          * We assume Blt() does work, if not, we will need to build
 757          * exception list case by case.
 758          */
 759         if (gop != NULL) {
 760                 switch (BltOperation) {
 761                 case GfxFbBltVideoFill:
 762                         status = gop->Blt(gop, BltBuffer, EfiBltVideoFill,
 763                             SourceX, SourceY, DestinationX, DestinationY,
 764                             Width, Height, Delta);
 765                         break;
 766
 767                 case GfxFbBltVideoToBltBuffer:
 768                         status = gop->Blt(gop, BltBuffer,
 769                             EfiBltVideoToBltBuffer,
 770                             SourceX, SourceY, DestinationX, DestinationY,
 771                             Width, Height, Delta);
 772                         break;
 773
 774                 case GfxFbBltBufferToVideo:
 775                         status = gop->Blt(gop, BltBuffer, EfiBltBufferToVideo,
 776                             SourceX, SourceY, DestinationX, DestinationY,
 777                             Width, Height, Delta);
 778                         break;
 779
 780                 case GfxFbBltVideoToVideo:
 781                         status = gop->Blt(gop, BltBuffer, EfiBltVideoToVideo,
 782                             SourceX, SourceY, DestinationX, DestinationY,
 783                             Width, Height, Delta);
 784                         break;
 785
 786                 default:
 787                         status = EFI_INVALID_PARAMETER;
 788                         break;
 789                 }
 790
 791                 switch (status) {
 792                 case EFI_SUCCESS:
 793                         rv = 0;
 794                         break;
 795
 796                 case EFI_INVALID_PARAMETER:
 797                         rv = EINVAL;
 798                         break;
 799
 800                 case EFI_DEVICE_ERROR:
 801                 default:
 802                         rv = EIO;
 803                         break;
 804                 }
 805
 806                 return (rv);
 807         }
 808 #endif
 809
 810         switch (BltOperation) {
 811         case GfxFbBltVideoFill:
 812                 rv = gfxfb_blt_fill(BltBuffer, DestinationX, DestinationY,
 813                     Width, Height);
 814                 break;
 815
 816         case GfxFbBltVideoToBltBuffer:
 817                 rv = gfxfb_blt_video_to_buffer(BltBuffer, SourceX, SourceY,
 818                     DestinationX, DestinationY, Width, Height, Delta);
 819                 break;
 820
 821         case GfxFbBltBufferToVideo:
 822                 rv = gfxfb_blt_buffer_to_video(BltBuffer, SourceX, SourceY,
 823                     DestinationX, DestinationY, Width, Height, Delta);
 824                 break;
 825
 826         case GfxFbBltVideoToVideo:
 827                 rv = gfxfb_blt_video_to_video(SourceX, SourceY,
 828                     DestinationX, DestinationY, Width, Height);
 829                 break;
 830
 831         default:
 832                 rv = EINVAL;
 833                 break;
 834         }
 835         return (rv);
 836 }
 837
 838 void
 839 gfx_bitblt_bitmap(teken_gfx_t *state, const uint8_t *glyph,
 840     const teken_attr_t *a, uint32_t alpha, bool cursor)
 841 {
 842         uint32_t width, height;
 843         uint32_t fgc, bgc, bpl, cc, o;
 844         int bpp, bit, byte;
 845         bool invert = false;
 846
 847         bpp = 4;                /* We only generate BGRA */
 848         width = state->tg_font.vf_width;
 849         height = state->tg_font.vf_height;
 850         bpl = (width + 7) / 8;  /* Bytes per source line. */
 851
 852         fgc = a->ta_fgcolor;
 853         bgc = a->ta_bgcolor;
 854         if (a->ta_format & TF_BOLD)
 855                 fgc |= TC_LIGHT;
 856         if (a->ta_format & TF_BLINK)
 857                 bgc |= TC_LIGHT;
 858
 859         fgc = gfx_fb_color_map(fgc);
 860         bgc = gfx_fb_color_map(bgc);
 861
 862         if (a->ta_format & TF_REVERSE)
 863                 invert = !invert;
 864         if (cursor)
 865                 invert = !invert;
 866         if (invert) {
 867                 uint32_t tmp;
 868
 869                 tmp = fgc;
 870                 fgc = bgc;
 871                 bgc = tmp;
 872         }
 873
 874         alpha = alpha << 24;
 875         fgc |= alpha;
 876         bgc |= alpha;
 877
 878         for (uint32_t y = 0; y < height; y++) {
 879                 for (uint32_t x = 0; x < width; x++) {
 880                         byte = y * bpl + x / 8;
 881                         bit = 0x80 >> (x % 8);
 882                         o = y * width * bpp + x * bpp;
 883                         cc = glyph[byte] & bit ? fgc : bgc;
 884
 885                         gfx_mem_wr4(state->tg_glyph,
 886                             state->tg_glyph_size, o, cc);
 887                 }
 888         }
 889 }
 890
 891 /*
 892  * Draw prepared glyph on terminal point p.
 893  */
 894 static void
 895 gfx_fb_printchar(teken_gfx_t *state, const teken_pos_t *p)
 896 {
 897         unsigned x, y, width, height;
 898
 899         width = state->tg_font.vf_width;
 900         height = state->tg_font.vf_height;
 901         x = state->tg_origin.tp_col + p->tp_col * width;
 902         y = state->tg_origin.tp_row + p->tp_row * height;
 903
 904         gfx_fb_cons_display(x, y, width, height, state->tg_glyph);
 905 }
 906
 907 /*
 908  * Store char with its attribute to buffer and put it on screen.
 909  */
 910 void
 911 gfx_fb_putchar(void *arg, const teken_pos_t *p, teken_char_t c,
 912     const teken_attr_t *a)
 913 {
 914         teken_gfx_t *state = arg;
 915         const uint8_t *glyph;
 916         int idx;
 917
 918         idx = p->tp_col + p->tp_row * state->tg_tp.tp_col;
 919         if (idx >= state->tg_tp.tp_col * state->tg_tp.tp_row)
 920                 return;
 921
 922         /* remove the cursor */
 923         if (state->tg_cursor_visible)
 924                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
 925
 926         screen_buffer[idx].c = c;
 927         screen_buffer[idx].a = *a;
 928
 929         glyph = font_lookup(&state->tg_font, c, a);
 930         gfx_bitblt_bitmap(state, glyph, a, 0xff, false);
 931         gfx_fb_printchar(state, p);
 932
 933         /* display the cursor */
 934         if (state->tg_cursor_visible) {
 935                 const teken_pos_t *c;
 936
 937                 c = teken_get_cursor(&state->tg_teken);
 938                 gfx_fb_cursor_draw(state, c, true);
 939         }
 940 }
 941
 942 void
 943 gfx_fb_fill(void *arg, const teken_rect_t *r, teken_char_t c,
 944     const teken_attr_t *a)
 945 {
 946         teken_gfx_t *state = arg;
 947         const uint8_t *glyph;
 948         teken_pos_t p;
 949         struct text_pixel *row;
 950
 951         /* remove the cursor */
 952         if (state->tg_cursor_visible)
 953                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
 954
 955         glyph = font_lookup(&state->tg_font, c, a);
 956         gfx_bitblt_bitmap(state, glyph, a, 0xff, false);
 957
 958         for (p.tp_row = r->tr_begin.tp_row; p.tp_row < r->tr_end.tp_row;
 959             p.tp_row++) {
 960                 row = &screen_buffer[p.tp_row * state->tg_tp.tp_col];
 961                 for (p.tp_col = r->tr_begin.tp_col;
 962                     p.tp_col < r->tr_end.tp_col; p.tp_col++) {
 963                         row[p.tp_col].c = c;
 964                         row[p.tp_col].a = *a;
 965                         gfx_fb_printchar(state, &p);
 966                 }
 967         }
 968
 969         /* display the cursor */
 970         if (state->tg_cursor_visible) {
 971                 const teken_pos_t *c;
 972
 973                 c = teken_get_cursor(&state->tg_teken);
 974                 gfx_fb_cursor_draw(state, c, true);
 975         }
 976 }
 977
 978 static void
 979 gfx_fb_cursor_draw(teken_gfx_t *state, const teken_pos_t *p, bool on)
 980 {
 981         const uint8_t *glyph;
 982         int idx;
 983
 984         idx = p->tp_col + p->tp_row * state->tg_tp.tp_col;
 985         if (idx >= state->tg_tp.tp_col * state->tg_tp.tp_row)
 986                 return;
 987
 988         glyph = font_lookup(&state->tg_font, screen_buffer[idx].c,
 989             &screen_buffer[idx].a);
 990         gfx_bitblt_bitmap(state, glyph, &screen_buffer[idx].a, 0xff, on);
 991         gfx_fb_printchar(state, p);
 992         state->tg_cursor = *p;
 993 }
 994
 995 void
 996 gfx_fb_cursor(void *arg, const teken_pos_t *p)
 997 {
 998         teken_gfx_t *state = arg;
 999 #if defined(EFI)
1000         EFI_TPL tpl;
1001
1002         tpl = BS->RaiseTPL(TPL_NOTIFY);
1003 #endif
1004
1005         /* Switch cursor off in old location and back on in new. */
1006         if (state->tg_cursor_visible) {
1007                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
1008                 gfx_fb_cursor_draw(state, p, true);
1009         }
1010 #if defined(EFI)
1011         BS->RestoreTPL(tpl);
1012 #endif
1013 }
1014
1015 void
1016 gfx_fb_param(void *arg, int cmd, unsigned int value)
1017 {
1018         teken_gfx_t *state = arg;
1019         const teken_pos_t *c;
1020
1021         switch (cmd) {
1022         case TP_SETLOCALCURSOR:
1023                 /*
1024                  * 0 means normal (usually block), 1 means hidden, and
1025                  * 2 means blinking (always block) for compatibility with
1026                  * syscons.  We don't support any changes except hiding,
1027                  * so must map 2 to 0.
1028                  */
1029                 value = (value == 1) ? 0 : 1;
1030                 /* FALLTHROUGH */
1031         case TP_SHOWCURSOR:
1032                 c = teken_get_cursor(&state->tg_teken);
1033                 gfx_fb_cursor_draw(state, c, true);
1034                 if (value != 0)
1035                         state->tg_cursor_visible = true;
1036                 else
1037                         state->tg_cursor_visible = false;
1038                 break;
1039         default:
1040                 /* Not yet implemented */
1041                 break;
1042         }
1043 }
1044
1045 bool
1046 is_same_pixel(struct text_pixel *px1, struct text_pixel *px2)
1047 {
1048         if (px1->c != px2->c)
1049                 return (false);
1050
1051         /* Is there image stored? */
1052         if ((px1->a.ta_format & TF_IMAGE) ||
1053             (px2->a.ta_format & TF_IMAGE))
1054                 return (false);
1055
1056         if (px1->a.ta_format != px2->a.ta_format)
1057                 return (false);
1058         if (px1->a.ta_fgcolor != px2->a.ta_fgcolor)
1059                 return (false);
1060         if (px1->a.ta_bgcolor != px2->a.ta_bgcolor)
1061                 return (false);
1062
1063         return (true);
1064 }
1065
1066 static void
1067 gfx_fb_copy_area(teken_gfx_t *state, const teken_rect_t *s,
1068     const teken_pos_t *d)
1069 {
1070         uint32_t sx, sy, dx, dy, width, height;
1071
1072         width = state->tg_font.vf_width;
1073         height = state->tg_font.vf_height;
1074
1075         sx = state->tg_origin.tp_col + s->tr_begin.tp_col * width;
1076         sy = state->tg_origin.tp_row + s->tr_begin.tp_row * height;
1077         dx = state->tg_origin.tp_col + d->tp_col * width;
1078         dy = state->tg_origin.tp_row + d->tp_row * height;
1079
1080         width *= (s->tr_end.tp_col - s->tr_begin.tp_col + 1);
1081
1082         (void) gfxfb_blt(NULL, GfxFbBltVideoToVideo, sx, sy, dx, dy,
1083                     width, height, 0);
1084 }
1085
1086 static void
1087 gfx_fb_copy_line(teken_gfx_t *state, int ncol, teken_pos_t *s, teken_pos_t *d)
1088 {
1089         teken_rect_t sr;
1090         teken_pos_t dp;
1091         unsigned soffset, doffset;
1092         bool mark = false;
1093         int x;
1094
1095         soffset = s->tp_col + s->tp_row * state->tg_tp.tp_col;
1096         doffset = d->tp_col + d->tp_row * state->tg_tp.tp_col;
1097
1098         for (x = 0; x < ncol; x++) {
1099                 if (is_same_pixel(&screen_buffer[soffset + x],
1100                     &screen_buffer[doffset + x])) {
1101                         if (mark) {
1102                                 gfx_fb_copy_area(state, &sr, &dp);
1103                                 mark = false;
1104                         }
1105                 } else {
1106                         screen_buffer[doffset + x] = screen_buffer[soffset + x];
1107                         if (mark) {
1108                                 /* update end point */
1109                                 sr.tr_end.tp_col = s->tp_col + x;;
1110                         } else {
1111                                 /* set up new rectangle */
1112                                 mark = true;
1113                                 sr.tr_begin.tp_col = s->tp_col + x;
1114                                 sr.tr_begin.tp_row = s->tp_row;
1115                                 sr.tr_end.tp_col = s->tp_col + x;
1116                                 sr.tr_end.tp_row = s->tp_row;
1117                                 dp.tp_col = d->tp_col + x;
1118                                 dp.tp_row = d->tp_row;
1119                         }
1120                 }
1121         }
1122         if (mark) {
1123                 gfx_fb_copy_area(state, &sr, &dp);
1124         }
1125 }
1126
1127 void
1128 gfx_fb_copy(void *arg, const teken_rect_t *r, const teken_pos_t *p)
1129 {
1130         teken_gfx_t *state = arg;
1131         unsigned doffset, soffset;
1132         teken_pos_t d, s;
1133         int nrow, ncol, y; /* Has to be signed - >= 0 comparison */
1134
1135         /*
1136          * Copying is a little tricky. We must make sure we do it in
1137          * correct order, to make sure we don't overwrite our own data.
1138          */
1139
1140         nrow = r->tr_end.tp_row - r->tr_begin.tp_row;
1141         ncol = r->tr_end.tp_col - r->tr_begin.tp_col;
1142
1143         if (p->tp_row + nrow > state->tg_tp.tp_row ||
1144             p->tp_col + ncol > state->tg_tp.tp_col)
1145                 return;
1146
1147         soffset = r->tr_begin.tp_col + r->tr_begin.tp_row * state->tg_tp.tp_col;
1148         doffset = p->tp_col + p->tp_row * state->tg_tp.tp_col;
1149
1150         /* remove the cursor */
1151         if (state->tg_cursor_visible)
1152                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
1153
1154         /*
1155          * Copy line by line.
1156          */
1157         if (doffset <= soffset) {
1158                 s = r->tr_begin;
1159                 d = *p;
1160                 for (y = 0; y < nrow; y++) {
1161                         s.tp_row = r->tr_begin.tp_row + y;
1162                         d.tp_row = p->tp_row + y;
1163
1164                         gfx_fb_copy_line(state, ncol, &s, &d);
1165                 }
1166         } else {
1167                 for (y = nrow - 1; y >= 0; y--) {
1168                         s.tp_row = r->tr_begin.tp_row + y;
1169                         d.tp_row = p->tp_row + y;
1170
1171                         gfx_fb_copy_line(state, ncol, &s, &d);
1172                 }
1173         }
1174
1175         /* display the cursor */
1176         if (state->tg_cursor_visible) {
1177                 const teken_pos_t *c;
1178
1179                 c = teken_get_cursor(&state->tg_teken);
1180                 gfx_fb_cursor_draw(state, c, true);
1181         }
1182 }
1183
1184 /*
1185  * Implements alpha blending for RGBA data, could use pixels for arguments,
1186  * but byte stream seems more generic.
1187  * The generic alpha blending is:
1188  * blend = alpha * fg + (1.0 - alpha) * bg.
1189  * Since our alpha is not from range [0..1], we scale appropriately.
1190  */
1191 static uint8_t
1192 alpha_blend(uint8_t fg, uint8_t bg, uint8_t alpha)
1193 {
1194         uint16_t blend, h, l;
1195
1196         /* trivial corner cases */
1197         if (alpha == 0)
1198                 return (bg);
1199         if (alpha == 0xFF)
1200                 return (fg);
1201         blend = (alpha * fg + (0xFF - alpha) * bg);
1202         /* Division by 0xFF */
1203         h = blend >> 8;
1204         l = blend & 0xFF;
1205         if (h + l >= 0xFF)
1206                 h++;
1207         return (h);
1208 }
1209
1210 /*
1211  * Implements alpha blending for RGBA data, could use pixels for arguments,
1212  * but byte stream seems more generic.
1213  * The generic alpha blending is:
1214  * blend = alpha * fg + (1.0 - alpha) * bg.
1215  * Since our alpha is not from range [0..1], we scale appropriately.
1216  */
1217 static void
1218 bitmap_cpy(void *dst, void *src, uint32_t size)
1219 {
1220 #if defined(EFI)
1221         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *ps, *pd;
1222 #else
1223         struct paletteentry *ps, *pd;
1224 #endif
1225         uint32_t i;
1226         uint8_t a;
1227
1228         ps = src;
1229         pd = dst;
1230
1231         /*
1232          * we only implement alpha blending for depth 32.
1233          */
1234         for (i = 0; i < size; i ++) {
1235                 a = ps[i].Reserved;
1236                 pd[i].Red = alpha_blend(ps[i].Red, pd[i].Red, a);
1237                 pd[i].Green = alpha_blend(ps[i].Green, pd[i].Green, a);
1238                 pd[i].Blue = alpha_blend(ps[i].Blue, pd[i].Blue, a);
1239                 pd[i].Reserved = a;
1240         }
1241 }
1242
1243 static void *
1244 allocate_glyphbuffer(uint32_t width, uint32_t height)
1245 {
1246         size_t size;
1247
1248         size = sizeof (*GlyphBuffer) * width * height;
1249         if (size != GlyphBufferSize) {
1250                 free(GlyphBuffer);
1251                 GlyphBuffer = malloc(size);
1252                 if (GlyphBuffer == NULL)
1253                         return (NULL);
1254                 GlyphBufferSize = size;
1255         }
1256         return (GlyphBuffer);
1257 }
1258
1259 void
1260 gfx_fb_cons_display(uint32_t x, uint32_t y, uint32_t width, uint32_t height,
1261     void *data)
1262 {
1263 #if defined(EFI)
1264         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *buf;
1265 #else
1266         struct paletteentry *buf;
1267 #endif
1268         size_t size;
1269
1270         size = width * height * sizeof(*buf);
1271
1272         /*
1273          * Common data to display is glyph, use preallocated
1274          * glyph buffer.
1275          */
1276         if (gfx_state.tg_glyph_size != GlyphBufferSize)
1277                 (void) allocate_glyphbuffer(width, height);
1278
1279         if (size == GlyphBufferSize)
1280                 buf = GlyphBuffer;
1281         else
1282                 buf = malloc(size);
1283         if (buf == NULL)
1284                 return;
1285
1286         if (gfxfb_blt(buf, GfxFbBltVideoToBltBuffer, x, y, 0, 0,
1287             width, height, 0) == 0) {
1288                 bitmap_cpy(buf, data, width * height);
1289                 (void) gfxfb_blt(buf, GfxFbBltBufferToVideo, 0, 0, x, y,
1290                     width, height, 0);
1291         }
1292         if (buf != GlyphBuffer)
1293                 free(buf);
1294 }
1295
1296 /*
1297  * Public graphics primitives.
1298  */
1299
1300 static int
1301 isqrt(int num)
1302 {
1303         int res = 0;
1304         int bit = 1 << 30;
1305
1306         /* "bit" starts at the highest power of four <= the argument. */
1307         while (bit > num)
1308                 bit >>= 2;
1309
1310         while (bit != 0) {
1311                 if (num >= res + bit) {
1312                         num -= res + bit;
1313                         res = (res >> 1) + bit;
1314                 } else {
1315                         res >>= 1;
1316                 }
1317                 bit >>= 2;
1318         }
1319         return (res);
1320 }
1321
1322 /* set pixel in framebuffer using gfx coordinates */
1323 void
1324 gfx_fb_setpixel(uint32_t x, uint32_t y)
1325 {
1326         uint32_t c;
1327         const teken_attr_t *ap;
1328
1329         if (gfx_state.tg_fb_type == FB_TEXT)
1330                 return;
1331
1332         ap = teken_get_curattr(&gfx_state.tg_teken);
1333         if (ap->ta_format & TF_REVERSE) {
1334                 c = ap->ta_bgcolor;
1335                 if (ap->ta_format & TF_BLINK)
1336                         c |= TC_LIGHT;
1337         } else {
1338                 c = ap->ta_fgcolor;
1339                 if (ap->ta_format & TF_BOLD)
1340                         c |= TC_LIGHT;
1341         }
1342
1343         c = gfx_fb_color_map(c);
1344
1345         if (x >= gfx_state.tg_fb.fb_width ||
1346             y >= gfx_state.tg_fb.fb_height)
1347                 return;
1348
1349         gfxfb_blt(&c, GfxFbBltVideoFill, 0, 0, x, y, 1, 1, 0);
1350 }
1351
1352 /*
1353  * draw rectangle in framebuffer using gfx coordinates.
1354  * The function is borrowed from vt_fb.c
1355  */
1356 void
1357 gfx_fb_drawrect(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2,
1358     uint32_t fill)
1359 {
1360         uint32_t x, y;
1361
1362         if (gfx_state.tg_fb_type == FB_TEXT)
1363                 return;
1364
1365         for (y = y1; y <= y2; y++) {
1366                 if (fill || (y == y1) || (y == y2)) {
1367                         for (x = x1; x <= x2; x++)
1368                                 gfx_fb_setpixel(x, y);
1369                 } else {
1370                         gfx_fb_setpixel(x1, y);
1371                         gfx_fb_setpixel(x2, y);
1372                 }
1373         }
1374 }
1375
1376 void
1377 gfx_fb_line(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t wd)
1378 {
1379         int dx, sx, dy, sy;
1380         int err, e2, x2, y2, ed, width;
1381
1382         if (gfx_state.tg_fb_type == FB_TEXT)
1383                 return;
1384
1385         width = wd;
1386         sx = x0 < x1? 1 : -1;
1387         sy = y0 < y1? 1 : -1;
1388         dx = x1 > x0? x1 - x0 : x0 - x1;
1389         dy = y1 > y0? y1 - y0 : y0 - y1;
1390         err = dx + dy;
1391         ed = dx + dy == 0 ? 1: isqrt(dx * dx + dy * dy);
1392
1393         for (;;) {
1394                 gfx_fb_setpixel(x0, y0);
1395                 e2 = err;
1396                 x2 = x0;
1397                 if ((e2 << 1) >= -dx) {         /* x step */
1398                         e2 += dy;
1399                         y2 = y0;
1400                         while (e2 < ed * width &&
1401                             (y1 != (uint32_t)y2 || dx > dy)) {
1402                                 y2 += sy;
1403                                 gfx_fb_setpixel(x0, y2);
1404                                 e2 += dx;
1405                         }
1406                         if (x0 == x1)
1407                                 break;
1408                         e2 = err;
1409                         err -= dy;
1410                         x0 += sx;
1411                 }
1412                 if ((e2 << 1) <= dy) {          /* y step */
1413                         e2 = dx-e2;
1414                         while (e2 < ed * width &&
1415                             (x1 != (uint32_t)x2 || dx < dy)) {
1416                                 x2 += sx;
1417                                 gfx_fb_setpixel(x2, y0);
1418                                 e2 += dy;
1419                         }
1420                         if (y0 == y1)
1421                                 break;
1422                         err += dx;
1423                         y0 += sy;
1424                 }
1425         }
1426 }
1427
1428 /*
1429  * quadratic Bézier curve limited to gradients without sign change.
1430  */
1431 void
1432 gfx_fb_bezier(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t x2,
1433     uint32_t y2, uint32_t wd)
1434 {
1435         int sx, sy, xx, yy, xy, width;
1436         int dx, dy, err, curvature;
1437         int i;
1438
1439         if (gfx_state.tg_fb_type == FB_TEXT)
1440                 return;
1441
1442         width = wd;
1443         sx = x2 - x1;
1444         sy = y2 - y1;
1445         xx = x0 - x1;
1446         yy = y0 - y1;
1447         curvature = xx*sy - yy*sx;
1448
1449         if (sx*sx + sy*sy > xx*xx+yy*yy) {
1450                 x2 = x0;
1451                 x0 = sx + x1;
1452                 y2 = y0;
1453                 y0 = sy + y1;
1454                 curvature = -curvature;
1455         }
1456         if (curvature != 0) {
1457                 xx += sx;
1458                 sx = x0 < x2? 1 : -1;
1459                 xx *= sx;
1460                 yy += sy;
1461                 sy = y0 < y2? 1 : -1;
1462                 yy *= sy;
1463                 xy = (xx*yy) << 1;
1464                 xx *= xx;
1465                 yy *= yy;
1466                 if (curvature * sx * sy < 0) {
1467                         xx = -xx;
1468                         yy = -yy;
1469                         xy = -xy;
1470                         curvature = -curvature;
1471                 }
1472                 dx = 4 * sy * curvature * (x1 - x0) + xx - xy;
1473                 dy = 4 * sx * curvature * (y0 - y1) + yy - xy;
1474                 xx += xx;
1475                 yy += yy;
1476                 err = dx + dy + xy;
1477                 do {
1478                         for (i = 0; i <= width; i++)
1479                                 gfx_fb_setpixel(x0 + i, y0);
1480                         if (x0 == x2 && y0 == y2)
1481                                 return;  /* last pixel -> curve finished */
1482                         y1 = 2 * err < dx;
1483                         if (2 * err > dy) {
1484                                 x0 += sx;
1485                                 dx -= xy;
1486                                 dy += yy;
1487                                 err += dy;
1488                         }
1489                         if (y1 != 0) {
1490                                 y0 += sy;
1491                                 dy -= xy;
1492                                 dx += xx;
1493                                 err += dx;
1494                         }
1495                 } while (dy < dx); /* gradient negates -> algorithm fails */
1496         }
1497         gfx_fb_line(x0, y0, x2, y2, width);
1498 }
1499
1500 /*
1501  * draw rectangle using terminal coordinates and current foreground color.
1502  */
1503 void
1504 gfx_term_drawrect(uint32_t ux1, uint32_t uy1, uint32_t ux2, uint32_t uy2)
1505 {
1506         int x1, y1, x2, y2;
1507         int xshift, yshift;
1508         int width, i;
1509         uint32_t vf_width, vf_height;
1510         teken_rect_t r;
1511
1512         if (gfx_state.tg_fb_type == FB_TEXT)
1513                 return;
1514
1515         vf_width = gfx_state.tg_font.vf_width;
1516         vf_height = gfx_state.tg_font.vf_height;
1517         width = vf_width / 4;                   /* line width */
1518         xshift = (vf_width - width) / 2;
1519         yshift = (vf_height - width) / 2;
1520
1521         /* Shift coordinates */
1522         if (ux1 != 0)
1523                 ux1--;
1524         if (uy1 != 0)
1525                 uy1--;
1526         ux2--;
1527         uy2--;
1528
1529         /* mark area used in terminal */
1530         r.tr_begin.tp_col = ux1;
1531         r.tr_begin.tp_row = uy1;
1532         r.tr_end.tp_col = ux2 + 1;
1533         r.tr_end.tp_row = uy2 + 1;
1534
1535         term_image_display(&gfx_state, &r);
1536
1537         /*
1538          * Draw horizontal lines width points thick, shifted from outer edge.
1539          */
1540         x1 = (ux1 + 1) * vf_width + gfx_state.tg_origin.tp_col;
1541         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1542         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1543         gfx_fb_drawrect(x1, y1, x2, y1 + width, 1);
1544         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1545         y2 += vf_height - yshift - width;
1546         gfx_fb_drawrect(x1, y2, x2, y2 + width, 1);
1547
1548         /*
1549          * Draw vertical lines width points thick, shifted from outer edge.
1550          */
1551         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1552         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1553         y1 += vf_height;
1554         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1555         gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
1556         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1557         x1 += vf_width - xshift - width;
1558         gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
1559
1560         /* Draw upper left corner. */
1561         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1562         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1563         y1 += vf_height;
1564
1565         x2 = ux1 * vf_width + gfx_state.tg_origin.tp_col;
1566         x2 += vf_width;
1567         y2 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1568         for (i = 0; i <= width; i++)
1569                 gfx_fb_bezier(x1 + i, y1, x1 + i, y2 + i, x2, y2 + i, width-i);
1570
1571         /* Draw lower left corner. */
1572         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col;
1573         x1 += vf_width;
1574         y1 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1575         y1 += vf_height - yshift;
1576         x2 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1577         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1578         for (i = 0; i <= width; i++)
1579                 gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
1580
1581         /* Draw upper right corner. */
1582         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1583         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1584         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1585         x2 += vf_width - xshift - width;
1586         y2 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1587         y2 += vf_height;
1588         for (i = 0; i <= width; i++)
1589                 gfx_fb_bezier(x1, y1 + i, x2 + i, y1 + i, x2 + i, y2, width-i);
1590
1591         /* Draw lower right corner. */
1592         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1593         y1 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1594         y1 += vf_height - yshift;
1595         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1596         x2 += vf_width - xshift - width;
1597         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1598         for (i = 0; i <= width; i++)
1599                 gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
1600 }
1601
1602 int
1603 gfx_fb_putimage(png_t *png, uint32_t ux1, uint32_t uy1, uint32_t ux2,
1604     uint32_t uy2, uint32_t flags)
1605 {
1606 #if defined(EFI)
1607         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *p;
1608 #else
1609         struct paletteentry *p;
1610 #endif
1611         uint8_t *data;
1612         uint32_t i, j, x, y, fheight, fwidth;
1613         int rs, gs, bs;
1614         uint8_t r, g, b, a;
1615         bool scale = false;
1616         bool trace = false;
1617         teken_rect_t rect;
1618
1619         trace = (flags & FL_PUTIMAGE_DEBUG) != 0;
1620
1621         if (gfx_state.tg_fb_type == FB_TEXT) {
1622                 if (trace)
1623                         printf("Framebuffer not active.\n");
1624                 return (1);
1625         }
1626
1627         if (png->color_type != PNG_TRUECOLOR_ALPHA) {
1628                 if (trace)
1629                         printf("Not truecolor image.\n");
1630                 return (1);
1631         }
1632
1633         if (ux1 > gfx_state.tg_fb.fb_width ||
1634             uy1 > gfx_state.tg_fb.fb_height) {
1635                 if (trace)
1636                         printf("Top left coordinate off screen.\n");
1637                 return (1);
1638         }
1639
1640         if (png->width > UINT16_MAX || png->height > UINT16_MAX) {
1641                 if (trace)
1642                         printf("Image too large.\n");
1643                 return (1);
1644         }
1645
1646         if (png->width < 1 || png->height < 1) {
1647                 if (trace)
1648                         printf("Image too small.\n");
1649                 return (1);
1650         }
1651
1652         /*
1653          * If 0 was passed for either ux2 or uy2, then calculate the missing
1654          * part of the bottom right coordinate.
1655          */
1656         scale = true;
1657         if (ux2 == 0 && uy2 == 0) {
1658                 /* Both 0, use the native resolution of the image */
1659                 ux2 = ux1 + png->width;
1660                 uy2 = uy1 + png->height;
1661                 scale = false;
1662         } else if (ux2 == 0) {
1663                 /* Set ux2 from uy2/uy1 to maintain aspect ratio */
1664                 ux2 = ux1 + (png->width * (uy2 - uy1)) / png->height;
1665         } else if (uy2 == 0) {
1666                 /* Set uy2 from ux2/ux1 to maintain aspect ratio */
1667                 uy2 = uy1 + (png->height * (ux2 - ux1)) / png->width;
1668         }
1669
1670         if (ux2 > gfx_state.tg_fb.fb_width ||
1671             uy2 > gfx_state.tg_fb.fb_height) {
1672                 if (trace)
1673                         printf("Bottom right coordinate off screen.\n");
1674                 return (1);
1675         }
1676
1677         fwidth = ux2 - ux1;
1678         fheight = uy2 - uy1;
1679
1680         /*
1681          * If the original image dimensions have been passed explicitly,
1682          * disable scaling.
1683          */
1684         if (fwidth == png->width && fheight == png->height)
1685                 scale = false;
1686
1687         if (ux1 == 0) {
1688                 /*
1689                  * No top left X co-ordinate (real coordinates start at 1),
1690                  * place as far right as it will fit.
1691                  */
1692                 ux2 = gfx_state.tg_fb.fb_width - gfx_state.tg_origin.tp_col;
1693                 ux1 = ux2 - fwidth;
1694         }
1695
1696         if (uy1 == 0) {
1697                 /*
1698                  * No top left Y co-ordinate (real coordinates start at 1),
1699                  * place as far down as it will fit.
1700                  */
1701                 uy2 = gfx_state.tg_fb.fb_height - gfx_state.tg_origin.tp_row;
1702                 uy1 = uy2 - fheight;
1703         }
1704
1705         if (ux1 >= ux2 || uy1 >= uy2) {
1706                 if (trace)
1707                         printf("Image dimensions reversed.\n");
1708                 return (1);
1709         }
1710
1711         if (fwidth < 2 || fheight < 2) {
1712                 if (trace)
1713                         printf("Target area too small\n");
1714                 return (1);
1715         }
1716
1717         if (trace)
1718                 printf("Image %ux%u -> %ux%u @%ux%u\n",
1719                     png->width, png->height, fwidth, fheight, ux1, uy1);
1720
1721         rect.tr_begin.tp_col = ux1 / gfx_state.tg_font.vf_width;
1722         rect.tr_begin.tp_row = uy1 / gfx_state.tg_font.vf_height;
1723         rect.tr_end.tp_col = (ux1 + fwidth) / gfx_state.tg_font.vf_width;
1724         rect.tr_end.tp_row = (uy1 + fheight) / gfx_state.tg_font.vf_height;
1725
1726         /*
1727          * mark area used in terminal
1728          */
1729         if (!(flags & FL_PUTIMAGE_NOSCROLL))
1730                 term_image_display(&gfx_state, &rect);
1731
1732         if ((flags & FL_PUTIMAGE_BORDER))
1733                 gfx_fb_drawrect(ux1, uy1, ux2, uy2, 0);
1734
1735         data = malloc(fwidth * fheight * sizeof(*p));
1736         p = (void *)data;
1737         if (data == NULL) {
1738                 if (trace)
1739                         printf("Out of memory.\n");
1740                 return (1);
1741         }
1742
1743         /*
1744          * Build image for our framebuffer.
1745          */
1746
1747         /* Helper to calculate the pixel index from the source png */
1748 #define GETPIXEL(xx, yy)        (((yy) * png->width + (xx)) * png->bpp)
1749
1750         /*
1751          * For each of the x and y directions, calculate the number of pixels
1752          * in the source image that correspond to a single pixel in the target.
1753          * Use fixed-point arithmetic with 16-bits for each of the integer and
1754          * fractional parts.
1755          */
1756         const uint32_t wcstep = ((png->width - 1) << 16) / (fwidth - 1);
1757         const uint32_t hcstep = ((png->height - 1) << 16) / (fheight - 1);
1758
1759         rs = 8 - (fls(gfx_state.tg_fb.fb_mask_red) -
1760             ffs(gfx_state.tg_fb.fb_mask_red) + 1);
1761         gs = 8 - (fls(gfx_state.tg_fb.fb_mask_green) -
1762             ffs(gfx_state.tg_fb.fb_mask_green) + 1);
1763         bs = 8 - (fls(gfx_state.tg_fb.fb_mask_blue) -
1764             ffs(gfx_state.tg_fb.fb_mask_blue) + 1);
1765
1766         uint32_t hc = 0;
1767         for (y = 0; y < fheight; y++) {
1768                 uint32_t hc2 = (hc >> 9) & 0x7f;
1769                 uint32_t hc1 = 0x80 - hc2;
1770
1771                 uint32_t offset_y = hc >> 16;
1772                 uint32_t offset_y1 = offset_y + 1;
1773
1774                 uint32_t wc = 0;
1775                 for (x = 0; x < fwidth; x++) {
1776                         uint32_t wc2 = (wc >> 9) & 0x7f;
1777                         uint32_t wc1 = 0x80 - wc2;
1778
1779                         uint32_t offset_x = wc >> 16;
1780                         uint32_t offset_x1 = offset_x + 1;
1781
1782                         /* Target pixel index */
1783                         j = y * fwidth + x;
1784
1785                         if (!scale) {
1786                                 i = GETPIXEL(x, y);
1787                                 r = png->image[i];
1788                                 g = png->image[i + 1];
1789                                 b = png->image[i + 2];
1790                                 a = png->image[i + 3];
1791                         } else {
1792                                 uint8_t pixel[4];
1793
1794                                 uint32_t p00 = GETPIXEL(offset_x, offset_y);
1795                                 uint32_t p01 = GETPIXEL(offset_x, offset_y1);
1796                                 uint32_t p10 = GETPIXEL(offset_x1, offset_y);
1797                                 uint32_t p11 = GETPIXEL(offset_x1, offset_y1);
1798
1799                                 /*
1800                                  * Given a 2x2 array of pixels in the source
1801                                  * image, combine them to produce a single
1802                                  * value for the pixel in the target image.
1803                                  * Each column of pixels is combined using
1804                                  * a weighted average where the top and bottom
1805                                  * pixels contribute hc1 and hc2 respectively.
1806                                  * The calculation for bottom pixel pB and
1807                                  * top pixel pT is:
1808                                  *   (pT * hc1 + pB * hc2) / (hc1 + hc2)
1809                                  * Once the values are determined for the two
1810                                  * columns of pixels, then the columns are
1811                                  * averaged together in the same way but using
1812                                  * wc1 and wc2 for the weightings.
1813                                  *
1814                                  * Since hc1 and hc2 are chosen so that
1815                                  * hc1 + hc2 == 128 (and same for wc1 + wc2),
1816                                  * the >> 14 below is a quick way to divide by
1817                                  * (hc1 + hc2) * (wc1 + wc2)
1818                                  */
1819                                 for (i = 0; i < 4; i++)
1820                                         pixel[i] = (
1821                                             (png->image[p00 + i] * hc1 +
1822                                             png->image[p01 + i] * hc2) * wc1 +
1823                                             (png->image[p10 + i] * hc1 +
1824                                             png->image[p11 + i] * hc2) * wc2)
1825                                             >> 14;
1826
1827                                 r = pixel[0];
1828                                 g = pixel[1];
1829                                 b = pixel[2];
1830                                 a = pixel[3];
1831                         }
1832
1833                         if (trace)
1834                                 printf("r/g/b: %x/%x/%x\n", r, g, b);
1835                         /*
1836                          * Rough colorspace reduction for 15/16 bit colors.
1837                          */
1838                         p[j].Red = r >> rs;
1839                         p[j].Green = g >> gs;
1840                         p[j].Blue = b >> bs;
1841                         p[j].Reserved = a;
1842
1843                         wc += wcstep;
1844                 }
1845                 hc += hcstep;
1846         }
1847
1848         gfx_fb_cons_display(ux1, uy1, fwidth, fheight, data);
1849         free(data);
1850         return (0);
1851 }
1852
1853 /*
1854  * Reset font flags to FONT_AUTO.
1855  */
1856 void
1857 reset_font_flags(void)
1858 {
1859         struct fontlist *fl;
1860
1861         STAILQ_FOREACH(fl, &fonts, font_next) {
1862                 fl->font_flags = FONT_AUTO;
1863         }
1864 }
1865
1866 static vt_font_bitmap_data_t *
1867 set_font(teken_unit_t *rows, teken_unit_t *cols, teken_unit_t h, teken_unit_t w)
1868 {
1869         vt_font_bitmap_data_t *font = NULL;
1870         struct fontlist *fl;
1871         unsigned height = h;
1872         unsigned width = w;
1873
1874         /*
1875          * First check for manually loaded font.
1876          */
1877         STAILQ_FOREACH(fl, &fonts, font_next) {
1878                 if (fl->font_flags == FONT_MANUAL) {
1879                         font = fl->font_data;
1880                         if (font->vfbd_font == NULL && fl->font_load != NULL &&
1881                             fl->font_name != NULL) {
1882                                 font = fl->font_load(fl->font_name);
1883                         }
1884                         if (font == NULL || font->vfbd_font == NULL)
1885                                 font = NULL;
1886                         break;
1887                 }
1888         }
1889
1890         if (font != NULL) {
1891                 *rows = height / font->vfbd_height;
1892                 *cols = width / font->vfbd_width;
1893                 return (font);
1894         }
1895
1896         /*
1897          * Find best font for these dimensions, or use default.
1898          * If height >= VT_FB_MAX_HEIGHT and width >= VT_FB_MAX_WIDTH,
1899          * do not use smaller font than our DEFAULT_FONT_DATA.
1900          */
1901         STAILQ_FOREACH(fl, &fonts, font_next) {
1902                 font = fl->font_data;
1903                 if ((*rows * font->vfbd_height <= height &&
1904                     *cols * font->vfbd_width <= width) ||
1905                     (height >= VT_FB_MAX_HEIGHT &&
1906                     width >= VT_FB_MAX_WIDTH &&
1907                     font->vfbd_height == DEFAULT_FONT_DATA.vfbd_height &&
1908                     font->vfbd_width == DEFAULT_FONT_DATA.vfbd_width)) {
1909                         if (font->vfbd_font == NULL ||
1910                             fl->font_flags == FONT_RELOAD) {
1911                                 if (fl->font_load != NULL &&
1912                                     fl->font_name != NULL) {
1913                                         font = fl->font_load(fl->font_name);
1914                                 }
1915                                 if (font == NULL)
1916                                         continue;
1917                         }
1918                         *rows = height / font->vfbd_height;
1919                         *cols = width / font->vfbd_width;
1920                         break;
1921                 }
1922                 font = NULL;
1923         }
1924
1925         if (font == NULL) {
1926                 /*
1927                  * We have fonts sorted smallest last, try it before
1928                  * falling back to builtin.
1929                  */
1930                 fl = STAILQ_LAST(&fonts, fontlist, font_next);
1931                 if (fl != NULL && fl->font_load != NULL &&
1932                     fl->font_name != NULL) {
1933                         font = fl->font_load(fl->font_name);
1934                 }
1935                 if (font == NULL)
1936                         font = &DEFAULT_FONT_DATA;
1937
1938                 *rows = height / font->vfbd_height;
1939                 *cols = width / font->vfbd_width;
1940         }
1941
1942         return (font);
1943 }
1944
1945 static void
1946 cons_clear(void)
1947 {
1948         char clear[] = { '\033', 'c' };
1949
1950         /* Reset terminal */
1951         teken_input(&gfx_state.tg_teken, clear, sizeof(clear));
1952         gfx_state.tg_functions->tf_param(&gfx_state, TP_SHOWCURSOR, 0);
1953 }
1954
1955 void
1956 setup_font(teken_gfx_t *state, teken_unit_t height, teken_unit_t width)
1957 {
1958         vt_font_bitmap_data_t *font_data;
1959         teken_pos_t *tp = &state->tg_tp;
1960         char env[8];
1961         int i;
1962
1963         /*
1964          * set_font() will select a appropriate sized font for
1965          * the number of rows and columns selected.  If we don't
1966          * have a font that will fit, then it will use the
1967          * default builtin font and adjust the rows and columns
1968          * to fit on the screen.
1969          */
1970         font_data = set_font(&tp->tp_row, &tp->tp_col, height, width);
1971
1972         if (font_data == NULL)
1973                 panic("out of memory");
1974
1975         for (i = 0; i < VFNT_MAPS; i++) {
1976                 state->tg_font.vf_map[i] =
1977                     font_data->vfbd_font->vf_map[i];
1978                 state->tg_font.vf_map_count[i] =
1979                     font_data->vfbd_font->vf_map_count[i];
1980         }
1981
1982         state->tg_font.vf_bytes = font_data->vfbd_font->vf_bytes;
1983         state->tg_font.vf_height = font_data->vfbd_font->vf_height;
1984         state->tg_font.vf_width = font_data->vfbd_font->vf_width;
1985
1986         snprintf(env, sizeof (env), "%ux%u",
1987             state->tg_font.vf_width, state->tg_font.vf_height);
1988         env_setenv("screen.font", EV_VOLATILE | EV_NOHOOK,
1989             env, font_set, env_nounset);
1990 }
1991
1992 /* Binary search for the glyph. Return 0 if not found. */
1993 static uint16_t
1994 font_bisearch(const vfnt_map_t *map, uint32_t len, teken_char_t src)
1995 {
1996         unsigned min, mid, max;
1997
1998         min = 0;
1999         max = len - 1;
2000
2001         /* Empty font map. */
2002         if (len == 0)
2003                 return (0);
2004         /* Character below minimal entry. */
2005         if (src < map[0].vfm_src)
2006                 return (0);
2007         /* Optimization: ASCII characters occur very often. */
2008         if (src <= map[0].vfm_src + map[0].vfm_len)
2009                 return (src - map[0].vfm_src + map[0].vfm_dst);
2010         /* Character above maximum entry. */
2011         if (src > map[max].vfm_src + map[max].vfm_len)
2012                 return (0);
2013
2014         /* Binary search. */
2015         while (max >= min) {
2016                 mid = (min + max) / 2;
2017                 if (src < map[mid].vfm_src)
2018                         max = mid - 1;
2019                 else if (src > map[mid].vfm_src + map[mid].vfm_len)
2020                         min = mid + 1;
2021                 else
2022                         return (src - map[mid].vfm_src + map[mid].vfm_dst);
2023         }
2024
2025         return (0);
2026 }
2027
2028 /*
2029  * Return glyph bitmap. If glyph is not found, we will return bitmap
2030  * for the first (offset 0) glyph.
2031  */
2032 uint8_t *
2033 font_lookup(const struct vt_font *vf, teken_char_t c, const teken_attr_t *a)
2034 {
2035         uint16_t dst;
2036         size_t stride;
2037
2038         /* Substitute bold with normal if not found. */
2039         if (a->ta_format & TF_BOLD) {
2040                 dst = font_bisearch(vf->vf_map[VFNT_MAP_BOLD],
2041                     vf->vf_map_count[VFNT_MAP_BOLD], c);
2042                 if (dst != 0)
2043                         goto found;
2044         }
2045         dst = font_bisearch(vf->vf_map[VFNT_MAP_NORMAL],
2046             vf->vf_map_count[VFNT_MAP_NORMAL], c);
2047
2048 found:
2049         stride = howmany(vf->vf_width, 8) * vf->vf_height;
2050         return (&vf->vf_bytes[dst * stride]);
2051 }
2052
2053 static int
2054 load_mapping(int fd, struct vt_font *fp, int n)
2055 {
2056         size_t i, size;
2057         ssize_t rv;
2058         vfnt_map_t *mp;
2059
2060         if (fp->vf_map_count[n] == 0)
2061                 return (0);
2062
2063         size = fp->vf_map_count[n] * sizeof(*mp);
2064         mp = malloc(size);
2065         if (mp == NULL)
2066                 return (ENOMEM);
2067         fp->vf_map[n] = mp;
2068
2069         rv = read(fd, mp, size);
2070         if (rv < 0 || (size_t)rv != size) {
2071                 free(fp->vf_map[n]);
2072                 fp->vf_map[n] = NULL;
2073                 return (EIO);
2074         }
2075
2076         for (i = 0; i < fp->vf_map_count[n]; i++) {
2077                 mp[i].vfm_src = be32toh(mp[i].vfm_src);
2078                 mp[i].vfm_dst = be16toh(mp[i].vfm_dst);
2079                 mp[i].vfm_len = be16toh(mp[i].vfm_len);
2080         }
2081         return (0);
2082 }
2083
2084 static int
2085 builtin_mapping(struct vt_font *fp, int n)
2086 {
2087         size_t size;
2088         struct vfnt_map *mp;
2089
2090         if (n >= VFNT_MAPS)
2091                 return (EINVAL);
2092
2093         if (fp->vf_map_count[n] == 0)
2094                 return (0);
2095
2096         size = fp->vf_map_count[n] * sizeof(*mp);
2097         mp = malloc(size);
2098         if (mp == NULL)
2099                 return (ENOMEM);
2100         fp->vf_map[n] = mp;
2101
2102         memcpy(mp, DEFAULT_FONT_DATA.vfbd_font->vf_map[n], size);
2103         return (0);
2104 }
2105
2106 /*
2107  * Load font from builtin or from file.
2108  * We do need special case for builtin because the builtin font glyphs
2109  * are compressed and we do need to uncompress them.
2110  * Having single load_font() for both cases will help us to simplify
2111  * font switch handling.
2112  */
2113 static vt_font_bitmap_data_t *
2114 load_font(char *path)
2115 {
2116         int fd, i;
2117         uint32_t glyphs;
2118         struct font_header fh;
2119         struct fontlist *fl;
2120         vt_font_bitmap_data_t *bp;
2121         struct vt_font *fp;
2122         size_t size;
2123         ssize_t rv;
2124
2125         /* Get our entry from the font list. */
2126         STAILQ_FOREACH(fl, &fonts, font_next) {
2127                 if (strcmp(fl->font_name, path) == 0)
2128                         break;
2129         }
2130         if (fl == NULL)
2131                 return (NULL);  /* Should not happen. */
2132
2133         bp = fl->font_data;
2134         if (bp->vfbd_font != NULL && fl->font_flags != FONT_RELOAD)
2135                 return (bp);
2136
2137         fd = -1;
2138         /*
2139          * Special case for builtin font.
2140          * Builtin font is the very first font we load, we do not have
2141          * previous loads to be released.
2142          */
2143         if (fl->font_flags == FONT_BUILTIN) {
2144                 if ((fp = calloc(1, sizeof(struct vt_font))) == NULL)
2145                         return (NULL);
2146
2147                 fp->vf_width = DEFAULT_FONT_DATA.vfbd_width;
2148                 fp->vf_height = DEFAULT_FONT_DATA.vfbd_height;
2149
2150                 fp->vf_bytes = malloc(DEFAULT_FONT_DATA.vfbd_uncompressed_size);
2151                 if (fp->vf_bytes == NULL) {
2152                         free(fp);
2153                         return (NULL);
2154                 }
2155
2156                 bp->vfbd_uncompressed_size =
2157                     DEFAULT_FONT_DATA.vfbd_uncompressed_size;
2158                 bp->vfbd_compressed_size =
2159                     DEFAULT_FONT_DATA.vfbd_compressed_size;
2160
2161                 if (lz4_decompress(DEFAULT_FONT_DATA.vfbd_compressed_data,
2162                     fp->vf_bytes,
2163                     DEFAULT_FONT_DATA.vfbd_compressed_size,
2164                     DEFAULT_FONT_DATA.vfbd_uncompressed_size, 0) != 0) {
2165                         free(fp->vf_bytes);
2166                         free(fp);
2167                         return (NULL);
2168                 }
2169
2170                 for (i = 0; i < VFNT_MAPS; i++) {
2171                         fp->vf_map_count[i] =
2172                             DEFAULT_FONT_DATA.vfbd_font->vf_map_count[i];
2173                         if (builtin_mapping(fp, i) != 0)
2174                                 goto free_done;
2175                 }
2176
2177                 bp->vfbd_font = fp;
2178                 return (bp);
2179         }
2180
2181         fd = open(path, O_RDONLY);
2182         if (fd < 0)
2183                 return (NULL);
2184
2185         size = sizeof(fh);
2186         rv = read(fd, &fh, size);
2187         if (rv < 0 || (size_t)rv != size) {
2188                 bp = NULL;
2189                 goto done;
2190         }
2191         if (memcmp(fh.fh_magic, FONT_HEADER_MAGIC, sizeof(fh.fh_magic)) != 0) {
2192                 bp = NULL;
2193                 goto done;
2194         }
2195         if ((fp = calloc(1, sizeof(struct vt_font))) == NULL) {
2196                 bp = NULL;
2197                 goto done;
2198         }
2199         for (i = 0; i < VFNT_MAPS; i++)
2200                 fp->vf_map_count[i] = be32toh(fh.fh_map_count[i]);
2201
2202         glyphs = be32toh(fh.fh_glyph_count);
2203         fp->vf_width = fh.fh_width;
2204         fp->vf_height = fh.fh_height;
2205
2206         size = howmany(fp->vf_width, 8) * fp->vf_height * glyphs;
2207         bp->vfbd_uncompressed_size = size;
2208         if ((fp->vf_bytes = malloc(size)) == NULL)
2209                 goto free_done;
2210
2211         rv = read(fd, fp->vf_bytes, size);
2212         if (rv < 0 || (size_t)rv != size)
2213                 goto free_done;
2214         for (i = 0; i < VFNT_MAPS; i++) {
2215                 if (load_mapping(fd, fp, i) != 0)
2216                         goto free_done;
2217         }
2218
2219         /*
2220          * Reset builtin flag now as we have full font loaded.
2221          */
2222         if (fl->font_flags == FONT_BUILTIN)
2223                 fl->font_flags = FONT_AUTO;
2224
2225         /*
2226          * Release previously loaded entries. We can do this now, as
2227          * the new font is loaded. Note, there can be no console
2228          * output till the new font is in place and teken is notified.
2229          * We do need to keep fl->font_data for glyph dimensions.
2230          */
2231         STAILQ_FOREACH(fl, &fonts, font_next) {
2232                 if (fl->font_data->vfbd_font == NULL)
2233                         continue;
2234
2235                 for (i = 0; i < VFNT_MAPS; i++)
2236                         free(fl->font_data->vfbd_font->vf_map[i]);
2237                 free(fl->font_data->vfbd_font->vf_bytes);
2238                 free(fl->font_data->vfbd_font);
2239                 fl->font_data->vfbd_font = NULL;
2240         }
2241
2242         bp->vfbd_font = fp;
2243         bp->vfbd_compressed_size = 0;
2244
2245 done:
2246         if (fd != -1)
2247                 close(fd);
2248         return (bp);
2249
2250 free_done:
2251         for (i = 0; i < VFNT_MAPS; i++)
2252                 free(fp->vf_map[i]);
2253         free(fp->vf_bytes);
2254         free(fp);
2255         bp = NULL;
2256         goto done;
2257 }
2258
2259 struct name_entry {
2260         char                    *n_name;
2261         SLIST_ENTRY(name_entry) n_entry;
2262 };
2263
2264 SLIST_HEAD(name_list, name_entry);
2265
2266 /* Read font names from index file. */
2267 static struct name_list *
2268 read_list(char *fonts)
2269 {
2270         struct name_list *nl;
2271         struct name_entry *np;
2272         char *dir, *ptr;
2273         char buf[PATH_MAX];
2274         int fd, len;
2275
2276         dir = strdup(fonts);
2277         if (dir == NULL)
2278                 return (NULL);
2279
2280         ptr = strrchr(dir, '/');
2281         *ptr = '\0';
2282
2283         fd = open(fonts, O_RDONLY);
2284         if (fd < 0)
2285                 return (NULL);
2286
2287         nl = malloc(sizeof(*nl));
2288         if (nl == NULL) {
2289                 close(fd);
2290                 return (nl);
2291         }
2292
2293         SLIST_INIT(nl);
2294         while ((len = fgetstr(buf, sizeof (buf), fd)) >= 0) {
2295                 if (*buf == '#' || *buf == '\0')
2296                         continue;
2297
2298                 if (bcmp(buf, "MENU", 4) == 0)
2299                         continue;
2300
2301                 if (bcmp(buf, "FONT", 4) == 0)
2302                         continue;
2303
2304                 ptr = strchr(buf, ':');
2305                 if (ptr == NULL)
2306                         continue;
2307                 else
2308                         *ptr = '\0';
2309
2310                 np = malloc(sizeof(*np));
2311                 if (np == NULL) {
2312                         close(fd);
2313                         return (nl);    /* return what we have */
2314                 }
2315                 if (asprintf(&np->n_name, "%s/%s", dir, buf) < 0) {
2316                         free(np);
2317                         close(fd);
2318                         return (nl);    /* return what we have */
2319                 }
2320                 SLIST_INSERT_HEAD(nl, np, n_entry);
2321         }
2322         close(fd);
2323         return (nl);
2324 }
2325
2326 /*
2327  * Read the font properties and insert new entry into the list.
2328  * The font list is built in descending order.
2329  */
2330 static bool
2331 insert_font(char *name, FONT_FLAGS flags)
2332 {
2333         struct font_header fh;
2334         struct fontlist *fp, *previous, *entry, *next;
2335         size_t size;
2336         ssize_t rv;
2337         int fd;
2338         char *font_name;
2339
2340         font_name = NULL;
2341         if (flags == FONT_BUILTIN) {
2342                 /*
2343                  * We only install builtin font once, while setting up
2344                  * initial console. Since this will happen very early,
2345                  * we assume asprintf will not fail. Once we have access to
2346                  * files, the builtin font will be replaced by font loaded
2347                  * from file.
2348                  */
2349                 if (!STAILQ_EMPTY(&fonts))
2350                         return (false);
2351
2352                 fh.fh_width = DEFAULT_FONT_DATA.vfbd_width;
2353                 fh.fh_height = DEFAULT_FONT_DATA.vfbd_height;
2354
2355                 (void) asprintf(&font_name, "%dx%d",
2356                     DEFAULT_FONT_DATA.vfbd_width,
2357                     DEFAULT_FONT_DATA.vfbd_height);
2358         } else {
2359                 fd = open(name, O_RDONLY);
2360                 if (fd < 0)
2361                         return (false);
2362                 rv = read(fd, &fh, sizeof(fh));
2363                 close(fd);
2364                 if (rv < 0 || (size_t)rv != sizeof(fh))
2365                         return (false);
2366
2367                 if (memcmp(fh.fh_magic, FONT_HEADER_MAGIC,
2368                     sizeof(fh.fh_magic)) != 0)
2369                         return (false);
2370                 font_name = strdup(name);
2371         }
2372
2373         if (font_name == NULL)
2374                 return (false);
2375
2376         /*
2377          * If we have an entry with the same glyph dimensions, replace
2378          * the file name and mark us. We only support unique dimensions.
2379          */
2380         STAILQ_FOREACH(entry, &fonts, font_next) {
2381                 if (fh.fh_width == entry->font_data->vfbd_width &&
2382                     fh.fh_height == entry->font_data->vfbd_height) {
2383                         free(entry->font_name);
2384                         entry->font_name = font_name;
2385                         entry->font_flags = FONT_RELOAD;
2386                         return (true);
2387                 }
2388         }
2389
2390         fp = calloc(sizeof(*fp), 1);
2391         if (fp == NULL) {
2392                 free(font_name);
2393                 return (false);
2394         }
2395         fp->font_data = calloc(sizeof(*fp->font_data), 1);
2396         if (fp->font_data == NULL) {
2397                 free(font_name);
2398                 free(fp);
2399                 return (false);
2400         }
2401         fp->font_name = font_name;
2402         fp->font_flags = flags;
2403         fp->font_load = load_font;
2404         fp->font_data->vfbd_width = fh.fh_width;
2405         fp->font_data->vfbd_height = fh.fh_height;
2406
2407         if (STAILQ_EMPTY(&fonts)) {
2408                 STAILQ_INSERT_HEAD(&fonts, fp, font_next);
2409                 return (true);
2410         }
2411
2412         previous = NULL;
2413         size = fp->font_data->vfbd_width * fp->font_data->vfbd_height;
2414
2415         STAILQ_FOREACH(entry, &fonts, font_next) {
2416                 vt_font_bitmap_data_t *bd;
2417
2418                 bd = entry->font_data;
2419                 /* Should fp be inserted before the entry? */
2420                 if (size > bd->vfbd_width * bd->vfbd_height) {
2421                         if (previous == NULL) {
2422                                 STAILQ_INSERT_HEAD(&fonts, fp, font_next);
2423                         } else {
2424                                 STAILQ_INSERT_AFTER(&fonts, previous, fp,
2425                                     font_next);
2426                         }
2427                         return (true);
2428                 }
2429                 next = STAILQ_NEXT(entry, font_next);
2430                 if (next == NULL ||
2431                     size > next->font_data->vfbd_width *
2432                     next->font_data->vfbd_height) {
2433                         STAILQ_INSERT_AFTER(&fonts, entry, fp, font_next);
2434                         return (true);
2435                 }
2436                 previous = entry;
2437         }
2438         return (true);
2439 }
2440
2441 static int
2442 font_set(struct env_var *ev __unused, int flags __unused, const void *value)
2443 {
2444         struct fontlist *fl;
2445         char *eptr;
2446         unsigned long x = 0, y = 0;
2447
2448         /*
2449          * Attempt to extract values from "XxY" string. In case of error,
2450          * we have unmaching glyph dimensions and will just output the
2451          * available values.
2452          */
2453         if (value != NULL) {
2454                 x = strtoul(value, &eptr, 10);
2455                 if (*eptr == 'x')
2456                         y = strtoul(eptr + 1, &eptr, 10);
2457         }
2458         STAILQ_FOREACH(fl, &fonts, font_next) {
2459                 if (fl->font_data->vfbd_width == x &&
2460                     fl->font_data->vfbd_height == y)
2461                         break;
2462         }
2463         if (fl != NULL) {
2464                 /* Reset any FONT_MANUAL flag. */
2465                 reset_font_flags();
2466
2467                 /* Mark this font manually loaded */
2468                 fl->font_flags = FONT_MANUAL;
2469                 cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2470                 return (CMD_OK);
2471         }
2472
2473         printf("Available fonts:\n");
2474         STAILQ_FOREACH(fl, &fonts, font_next) {
2475                 printf("    %dx%d\n", fl->font_data->vfbd_width,
2476                     fl->font_data->vfbd_height);
2477         }
2478         return (CMD_OK);
2479 }
2480
2481 void
2482 bios_text_font(bool use_vga_font)
2483 {
2484         if (use_vga_font)
2485                 (void) insert_font(VGA_8X16_FONT, FONT_MANUAL);
2486         else
2487                 (void) insert_font(DEFAULT_8X16_FONT, FONT_MANUAL);
2488 }
2489
2490 void
2491 autoload_font(bool bios)
2492 {
2493         struct name_list *nl;
2494         struct name_entry *np;
2495
2496         nl = read_list("/boot/fonts/INDEX.fonts");
2497         if (nl == NULL)
2498                 return;
2499
2500         while (!SLIST_EMPTY(nl)) {
2501                 np = SLIST_FIRST(nl);
2502                 SLIST_REMOVE_HEAD(nl, n_entry);
2503                 if (insert_font(np->n_name, FONT_AUTO) == false)
2504                         printf("failed to add font: %s\n", np->n_name);
2505                 free(np->n_name);
2506                 free(np);
2507         }
2508
2509         /*
2510          * If vga text mode was requested, load vga.font (8x16 bold) font.
2511          */
2512         if (bios) {
2513                 bios_text_font(true);
2514         }
2515
2516         (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2517 }
2518
2519 COMMAND_SET(load_font, "loadfont", "load console font from file", command_font);
2520
2521 static int
2522 command_font(int argc, char *argv[])
2523 {
2524         int i, c, rc;
2525         struct fontlist *fl;
2526         vt_font_bitmap_data_t *bd;
2527         bool list;
2528
2529         list = false;
2530         optind = 1;
2531         optreset = 1;
2532         rc = CMD_OK;
2533
2534         while ((c = getopt(argc, argv, "l")) != -1) {
2535                 switch (c) {
2536                 case 'l':
2537                         list = true;
2538                         break;
2539                 case '?':
2540                 default:
2541                         return (CMD_ERROR);
2542                 }
2543         }
2544
2545         argc -= optind;
2546         argv += optind;
2547
2548         if (argc > 1 || (list && argc != 0)) {
2549                 printf("Usage: loadfont [-l] | [file.fnt]\n");
2550                 return (CMD_ERROR);
2551         }
2552
2553         if (list) {
2554                 STAILQ_FOREACH(fl, &fonts, font_next) {
2555                         printf("font %s: %dx%d%s\n", fl->font_name,
2556                             fl->font_data->vfbd_width,
2557                             fl->font_data->vfbd_height,
2558                             fl->font_data->vfbd_font == NULL? "" : " loaded");
2559                 }
2560                 return (CMD_OK);
2561         }
2562
2563         /* Clear scren */
2564         cons_clear();
2565
2566         if (argc == 1) {
2567                 char *name = argv[0];
2568
2569                 if (insert_font(name, FONT_MANUAL) == false) {
2570                         printf("loadfont error: failed to load: %s\n", name);
2571                         return (CMD_ERROR);
2572                 }
2573
2574                 (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2575                 return (CMD_OK);
2576         }
2577
2578         if (argc == 0) {
2579                 /*
2580                  * Walk entire font list, release any loaded font, and set
2581                  * autoload flag. The font list does have at least the builtin
2582                  * default font.
2583                  */
2584                 STAILQ_FOREACH(fl, &fonts, font_next) {
2585                         if (fl->font_data->vfbd_font != NULL) {
2586
2587                                 bd = fl->font_data;
2588                                 /*
2589                                  * Note the setup_font() is releasing
2590                                  * font bytes.
2591                                  */
2592                                 for (i = 0; i < VFNT_MAPS; i++)
2593                                         free(bd->vfbd_font->vf_map[i]);
2594                                 free(fl->font_data->vfbd_font);
2595                                 fl->font_data->vfbd_font = NULL;
2596                                 fl->font_data->vfbd_uncompressed_size = 0;
2597                                 fl->font_flags = FONT_AUTO;
2598                         }
2599                 }
2600                 (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2601         }
2602         return (rc);
2603 }
2604
2605 bool
2606 gfx_get_edid_resolution(struct vesa_edid_info *edid, edid_res_list_t *res)
2607 {
2608         struct resolution *rp, *p;
2609
2610         /*
2611          * Walk detailed timings tables (4).
2612          */
2613         if ((edid->display.supported_features
2614             & EDID_FEATURE_PREFERRED_TIMING_MODE) != 0) {
2615                 /* Walk detailed timing descriptors (4) */
2616                 for (int i = 0; i < DET_TIMINGS; i++) {
2617                         /*
2618                          * Reserved value 0 is not used for display decriptor.
2619                          */
2620                         if (edid->detailed_timings[i].pixel_clock == 0)
2621                                 continue;
2622                         if ((rp = malloc(sizeof(*rp))) == NULL)
2623                                 continue;
2624                         rp->width = GET_EDID_INFO_WIDTH(edid, i);
2625                         rp->height = GET_EDID_INFO_HEIGHT(edid, i);
2626                         if (rp->width > 0 && rp->width <= EDID_MAX_PIXELS &&
2627                             rp->height > 0 && rp->height <= EDID_MAX_LINES)
2628                                 TAILQ_INSERT_TAIL(res, rp, next);
2629                         else
2630                                 free(rp);
2631                 }
2632         }
2633
2634         /*
2635          * Walk standard timings list (8).
2636          */
2637         for (int i = 0; i < STD_TIMINGS; i++) {
2638                 /* Is this field unused? */
2639                 if (edid->standard_timings[i] == 0x0101)
2640                         continue;
2641
2642                 if ((rp = malloc(sizeof(*rp))) == NULL)
2643                         continue;
2644
2645                 rp->width = HSIZE(edid->standard_timings[i]);
2646                 switch (RATIO(edid->standard_timings[i])) {
2647                 case RATIO1_1:
2648                         rp->height = HSIZE(edid->standard_timings[i]);
2649                         if (edid->header.version > 1 ||
2650                             edid->header.revision > 2) {
2651                                 rp->height = rp->height * 10 / 16;
2652                         }
2653                         break;
2654                 case RATIO4_3:
2655                         rp->height = HSIZE(edid->standard_timings[i]) * 3 / 4;
2656                         break;
2657                 case RATIO5_4:
2658                         rp->height = HSIZE(edid->standard_timings[i]) * 4 / 5;
2659                         break;
2660                 case RATIO16_9:
2661                         rp->height = HSIZE(edid->standard_timings[i]) * 9 / 16;
2662                         break;
2663                 }
2664
2665                 /*
2666                  * Create resolution list in decreasing order, except keep
2667                  * first entry (preferred timing mode).
2668                  */
2669                 TAILQ_FOREACH(p, res, next) {
2670                         if (p->width * p->height < rp->width * rp->height) {
2671                                 /* Keep preferred mode first */
2672                                 if (TAILQ_FIRST(res) == p)
2673                                         TAILQ_INSERT_AFTER(res, p, rp, next);
2674                                 else
2675                                         TAILQ_INSERT_BEFORE(p, rp, next);
2676                                 break;
2677                         }
2678                         if (TAILQ_NEXT(p, next) == NULL) {
2679                                 TAILQ_INSERT_TAIL(res, rp, next);
2680                                 break;
2681                         }
2682                 }
2683         }
2684         return (!TAILQ_EMPTY(res));
2685 }