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         unsigned x, y, width, height;
 982         const uint8_t *glyph;
 983         int idx;
 984
 985         idx = p->tp_col + p->tp_row * state->tg_tp.tp_col;
 986         if (idx >= state->tg_tp.tp_col * state->tg_tp.tp_row)
 987                 return;
 988
 989         width = state->tg_font.vf_width;
 990         height = state->tg_font.vf_height;
 991         x = state->tg_origin.tp_col + p->tp_col * width;
 992         y = state->tg_origin.tp_row + p->tp_row * height;
 993
 994         /*
 995          * Save original display content to preserve image data.
 996          */
 997         if (on) {
 998                 if (state->tg_cursor_image == NULL ||
 999                     state->tg_cursor_size != width * height * 4) {
1000                         free(state->tg_cursor_image);
1001                         state->tg_cursor_size = width * height * 4;
1002                         state->tg_cursor_image = malloc(state->tg_cursor_size);
1003                 }
1004                 if (state->tg_cursor_image != NULL) {
1005                         if (gfxfb_blt(state->tg_cursor_image,
1006                             GfxFbBltVideoToBltBuffer, x, y, 0, 0,
1007                             width, height, 0) != 0) {
1008                                 free(state->tg_cursor_image);
1009                                 state->tg_cursor_image = NULL;
1010                         }
1011                 }
1012         } else {
1013                 /*
1014                  * Restore display from tg_cursor_image.
1015                  * If there is no image, restore char from screen_buffer.
1016                  */
1017                 if (state->tg_cursor_image != NULL &&
1018                     gfxfb_blt(state->tg_cursor_image, GfxFbBltBufferToVideo,
1019                     0, 0, x, y, width, height, 0) == 0) {
1020                         state->tg_cursor = *p;
1021                         return;
1022                 }
1023         }
1024
1025         glyph = font_lookup(&state->tg_font, screen_buffer[idx].c,
1026             &screen_buffer[idx].a);
1027         gfx_bitblt_bitmap(state, glyph, &screen_buffer[idx].a, 0xff, on);
1028         gfx_fb_printchar(state, p);
1029
1030         state->tg_cursor = *p;
1031 }
1032
1033 void
1034 gfx_fb_cursor(void *arg, const teken_pos_t *p)
1035 {
1036         teken_gfx_t *state = arg;
1037 #if defined(EFI)
1038         EFI_TPL tpl;
1039
1040         tpl = BS->RaiseTPL(TPL_NOTIFY);
1041 #endif
1042
1043         /* Switch cursor off in old location and back on in new. */
1044         if (state->tg_cursor_visible) {
1045                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
1046                 gfx_fb_cursor_draw(state, p, true);
1047         }
1048 #if defined(EFI)
1049         BS->RestoreTPL(tpl);
1050 #endif
1051 }
1052
1053 void
1054 gfx_fb_param(void *arg, int cmd, unsigned int value)
1055 {
1056         teken_gfx_t *state = arg;
1057         const teken_pos_t *c;
1058
1059         switch (cmd) {
1060         case TP_SETLOCALCURSOR:
1061                 /*
1062                  * 0 means normal (usually block), 1 means hidden, and
1063                  * 2 means blinking (always block) for compatibility with
1064                  * syscons.  We don't support any changes except hiding,
1065                  * so must map 2 to 0.
1066                  */
1067                 value = (value == 1) ? 0 : 1;
1068                 /* FALLTHROUGH */
1069         case TP_SHOWCURSOR:
1070                 c = teken_get_cursor(&state->tg_teken);
1071                 gfx_fb_cursor_draw(state, c, true);
1072                 if (value != 0)
1073                         state->tg_cursor_visible = true;
1074                 else
1075                         state->tg_cursor_visible = false;
1076                 break;
1077         default:
1078                 /* Not yet implemented */
1079                 break;
1080         }
1081 }
1082
1083 bool
1084 is_same_pixel(struct text_pixel *px1, struct text_pixel *px2)
1085 {
1086         if (px1->c != px2->c)
1087                 return (false);
1088
1089         /* Is there image stored? */
1090         if ((px1->a.ta_format & TF_IMAGE) ||
1091             (px2->a.ta_format & TF_IMAGE))
1092                 return (false);
1093
1094         if (px1->a.ta_format != px2->a.ta_format)
1095                 return (false);
1096         if (px1->a.ta_fgcolor != px2->a.ta_fgcolor)
1097                 return (false);
1098         if (px1->a.ta_bgcolor != px2->a.ta_bgcolor)
1099                 return (false);
1100
1101         return (true);
1102 }
1103
1104 static void
1105 gfx_fb_copy_area(teken_gfx_t *state, const teken_rect_t *s,
1106     const teken_pos_t *d)
1107 {
1108         uint32_t sx, sy, dx, dy, width, height;
1109
1110         width = state->tg_font.vf_width;
1111         height = state->tg_font.vf_height;
1112
1113         sx = state->tg_origin.tp_col + s->tr_begin.tp_col * width;
1114         sy = state->tg_origin.tp_row + s->tr_begin.tp_row * height;
1115         dx = state->tg_origin.tp_col + d->tp_col * width;
1116         dy = state->tg_origin.tp_row + d->tp_row * height;
1117
1118         width *= (s->tr_end.tp_col - s->tr_begin.tp_col + 1);
1119
1120         (void) gfxfb_blt(NULL, GfxFbBltVideoToVideo, sx, sy, dx, dy,
1121                     width, height, 0);
1122 }
1123
1124 static void
1125 gfx_fb_copy_line(teken_gfx_t *state, int ncol, teken_pos_t *s, teken_pos_t *d)
1126 {
1127         teken_rect_t sr;
1128         teken_pos_t dp;
1129         unsigned soffset, doffset;
1130         bool mark = false;
1131         int x;
1132
1133         soffset = s->tp_col + s->tp_row * state->tg_tp.tp_col;
1134         doffset = d->tp_col + d->tp_row * state->tg_tp.tp_col;
1135
1136         for (x = 0; x < ncol; x++) {
1137                 if (is_same_pixel(&screen_buffer[soffset + x],
1138                     &screen_buffer[doffset + x])) {
1139                         if (mark) {
1140                                 gfx_fb_copy_area(state, &sr, &dp);
1141                                 mark = false;
1142                         }
1143                 } else {
1144                         screen_buffer[doffset + x] = screen_buffer[soffset + x];
1145                         if (mark) {
1146                                 /* update end point */
1147                                 sr.tr_end.tp_col = s->tp_col + x;;
1148                         } else {
1149                                 /* set up new rectangle */
1150                                 mark = true;
1151                                 sr.tr_begin.tp_col = s->tp_col + x;
1152                                 sr.tr_begin.tp_row = s->tp_row;
1153                                 sr.tr_end.tp_col = s->tp_col + x;
1154                                 sr.tr_end.tp_row = s->tp_row;
1155                                 dp.tp_col = d->tp_col + x;
1156                                 dp.tp_row = d->tp_row;
1157                         }
1158                 }
1159         }
1160         if (mark) {
1161                 gfx_fb_copy_area(state, &sr, &dp);
1162         }
1163 }
1164
1165 void
1166 gfx_fb_copy(void *arg, const teken_rect_t *r, const teken_pos_t *p)
1167 {
1168         teken_gfx_t *state = arg;
1169         unsigned doffset, soffset;
1170         teken_pos_t d, s;
1171         int nrow, ncol, y; /* Has to be signed - >= 0 comparison */
1172
1173         /*
1174          * Copying is a little tricky. We must make sure we do it in
1175          * correct order, to make sure we don't overwrite our own data.
1176          */
1177
1178         nrow = r->tr_end.tp_row - r->tr_begin.tp_row;
1179         ncol = r->tr_end.tp_col - r->tr_begin.tp_col;
1180
1181         if (p->tp_row + nrow > state->tg_tp.tp_row ||
1182             p->tp_col + ncol > state->tg_tp.tp_col)
1183                 return;
1184
1185         soffset = r->tr_begin.tp_col + r->tr_begin.tp_row * state->tg_tp.tp_col;
1186         doffset = p->tp_col + p->tp_row * state->tg_tp.tp_col;
1187
1188         /* remove the cursor */
1189         if (state->tg_cursor_visible)
1190                 gfx_fb_cursor_draw(state, &state->tg_cursor, false);
1191
1192         /*
1193          * Copy line by line.
1194          */
1195         if (doffset <= soffset) {
1196                 s = r->tr_begin;
1197                 d = *p;
1198                 for (y = 0; y < nrow; y++) {
1199                         s.tp_row = r->tr_begin.tp_row + y;
1200                         d.tp_row = p->tp_row + y;
1201
1202                         gfx_fb_copy_line(state, ncol, &s, &d);
1203                 }
1204         } else {
1205                 for (y = nrow - 1; y >= 0; y--) {
1206                         s.tp_row = r->tr_begin.tp_row + y;
1207                         d.tp_row = p->tp_row + y;
1208
1209                         gfx_fb_copy_line(state, ncol, &s, &d);
1210                 }
1211         }
1212
1213         /* display the cursor */
1214         if (state->tg_cursor_visible) {
1215                 const teken_pos_t *c;
1216
1217                 c = teken_get_cursor(&state->tg_teken);
1218                 gfx_fb_cursor_draw(state, c, true);
1219         }
1220 }
1221
1222 /*
1223  * Implements alpha blending for RGBA data, could use pixels for arguments,
1224  * but byte stream seems more generic.
1225  * The generic alpha blending is:
1226  * blend = alpha * fg + (1.0 - alpha) * bg.
1227  * Since our alpha is not from range [0..1], we scale appropriately.
1228  */
1229 static uint8_t
1230 alpha_blend(uint8_t fg, uint8_t bg, uint8_t alpha)
1231 {
1232         uint16_t blend, h, l;
1233
1234         /* trivial corner cases */
1235         if (alpha == 0)
1236                 return (bg);
1237         if (alpha == 0xFF)
1238                 return (fg);
1239         blend = (alpha * fg + (0xFF - alpha) * bg);
1240         /* Division by 0xFF */
1241         h = blend >> 8;
1242         l = blend & 0xFF;
1243         if (h + l >= 0xFF)
1244                 h++;
1245         return (h);
1246 }
1247
1248 /*
1249  * Implements alpha blending for RGBA data, could use pixels for arguments,
1250  * but byte stream seems more generic.
1251  * The generic alpha blending is:
1252  * blend = alpha * fg + (1.0 - alpha) * bg.
1253  * Since our alpha is not from range [0..1], we scale appropriately.
1254  */
1255 static void
1256 bitmap_cpy(void *dst, void *src, uint32_t size)
1257 {
1258 #if defined(EFI)
1259         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *ps, *pd;
1260 #else
1261         struct paletteentry *ps, *pd;
1262 #endif
1263         uint32_t i;
1264         uint8_t a;
1265
1266         ps = src;
1267         pd = dst;
1268
1269         /*
1270          * we only implement alpha blending for depth 32.
1271          */
1272         for (i = 0; i < size; i ++) {
1273                 a = ps[i].Reserved;
1274                 pd[i].Red = alpha_blend(ps[i].Red, pd[i].Red, a);
1275                 pd[i].Green = alpha_blend(ps[i].Green, pd[i].Green, a);
1276                 pd[i].Blue = alpha_blend(ps[i].Blue, pd[i].Blue, a);
1277                 pd[i].Reserved = a;
1278         }
1279 }
1280
1281 static void *
1282 allocate_glyphbuffer(uint32_t width, uint32_t height)
1283 {
1284         size_t size;
1285
1286         size = sizeof (*GlyphBuffer) * width * height;
1287         if (size != GlyphBufferSize) {
1288                 free(GlyphBuffer);
1289                 GlyphBuffer = malloc(size);
1290                 if (GlyphBuffer == NULL)
1291                         return (NULL);
1292                 GlyphBufferSize = size;
1293         }
1294         return (GlyphBuffer);
1295 }
1296
1297 void
1298 gfx_fb_cons_display(uint32_t x, uint32_t y, uint32_t width, uint32_t height,
1299     void *data)
1300 {
1301 #if defined(EFI)
1302         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *buf;
1303 #else
1304         struct paletteentry *buf;
1305 #endif
1306         size_t size;
1307
1308         size = width * height * sizeof(*buf);
1309
1310         /*
1311          * Common data to display is glyph, use preallocated
1312          * glyph buffer.
1313          */
1314         if (gfx_state.tg_glyph_size != GlyphBufferSize)
1315                 (void) allocate_glyphbuffer(width, height);
1316
1317         if (size == GlyphBufferSize)
1318                 buf = GlyphBuffer;
1319         else
1320                 buf = malloc(size);
1321         if (buf == NULL)
1322                 return;
1323
1324         if (gfxfb_blt(buf, GfxFbBltVideoToBltBuffer, x, y, 0, 0,
1325             width, height, 0) == 0) {
1326                 bitmap_cpy(buf, data, width * height);
1327                 (void) gfxfb_blt(buf, GfxFbBltBufferToVideo, 0, 0, x, y,
1328                     width, height, 0);
1329         }
1330         if (buf != GlyphBuffer)
1331                 free(buf);
1332 }
1333
1334 /*
1335  * Public graphics primitives.
1336  */
1337
1338 static int
1339 isqrt(int num)
1340 {
1341         int res = 0;
1342         int bit = 1 << 30;
1343
1344         /* "bit" starts at the highest power of four <= the argument. */
1345         while (bit > num)
1346                 bit >>= 2;
1347
1348         while (bit != 0) {
1349                 if (num >= res + bit) {
1350                         num -= res + bit;
1351                         res = (res >> 1) + bit;
1352                 } else {
1353                         res >>= 1;
1354                 }
1355                 bit >>= 2;
1356         }
1357         return (res);
1358 }
1359
1360 /* set pixel in framebuffer using gfx coordinates */
1361 void
1362 gfx_fb_setpixel(uint32_t x, uint32_t y)
1363 {
1364         uint32_t c;
1365         const teken_attr_t *ap;
1366
1367         if (gfx_state.tg_fb_type == FB_TEXT)
1368                 return;
1369
1370         ap = teken_get_curattr(&gfx_state.tg_teken);
1371         if (ap->ta_format & TF_REVERSE) {
1372                 c = ap->ta_bgcolor;
1373                 if (ap->ta_format & TF_BLINK)
1374                         c |= TC_LIGHT;
1375         } else {
1376                 c = ap->ta_fgcolor;
1377                 if (ap->ta_format & TF_BOLD)
1378                         c |= TC_LIGHT;
1379         }
1380
1381         c = gfx_fb_color_map(c);
1382
1383         if (x >= gfx_state.tg_fb.fb_width ||
1384             y >= gfx_state.tg_fb.fb_height)
1385                 return;
1386
1387         gfxfb_blt(&c, GfxFbBltVideoFill, 0, 0, x, y, 1, 1, 0);
1388 }
1389
1390 /*
1391  * draw rectangle in framebuffer using gfx coordinates.
1392  * The function is borrowed from vt_fb.c
1393  */
1394 void
1395 gfx_fb_drawrect(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2,
1396     uint32_t fill)
1397 {
1398         uint32_t x, y;
1399
1400         if (gfx_state.tg_fb_type == FB_TEXT)
1401                 return;
1402
1403         for (y = y1; y <= y2; y++) {
1404                 if (fill || (y == y1) || (y == y2)) {
1405                         for (x = x1; x <= x2; x++)
1406                                 gfx_fb_setpixel(x, y);
1407                 } else {
1408                         gfx_fb_setpixel(x1, y);
1409                         gfx_fb_setpixel(x2, y);
1410                 }
1411         }
1412 }
1413
1414 void
1415 gfx_fb_line(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t wd)
1416 {
1417         int dx, sx, dy, sy;
1418         int err, e2, x2, y2, ed, width;
1419
1420         if (gfx_state.tg_fb_type == FB_TEXT)
1421                 return;
1422
1423         width = wd;
1424         sx = x0 < x1? 1 : -1;
1425         sy = y0 < y1? 1 : -1;
1426         dx = x1 > x0? x1 - x0 : x0 - x1;
1427         dy = y1 > y0? y1 - y0 : y0 - y1;
1428         err = dx + dy;
1429         ed = dx + dy == 0 ? 1: isqrt(dx * dx + dy * dy);
1430
1431         for (;;) {
1432                 gfx_fb_setpixel(x0, y0);
1433                 e2 = err;
1434                 x2 = x0;
1435                 if ((e2 << 1) >= -dx) {         /* x step */
1436                         e2 += dy;
1437                         y2 = y0;
1438                         while (e2 < ed * width &&
1439                             (y1 != (uint32_t)y2 || dx > dy)) {
1440                                 y2 += sy;
1441                                 gfx_fb_setpixel(x0, y2);
1442                                 e2 += dx;
1443                         }
1444                         if (x0 == x1)
1445                                 break;
1446                         e2 = err;
1447                         err -= dy;
1448                         x0 += sx;
1449                 }
1450                 if ((e2 << 1) <= dy) {          /* y step */
1451                         e2 = dx-e2;
1452                         while (e2 < ed * width &&
1453                             (x1 != (uint32_t)x2 || dx < dy)) {
1454                                 x2 += sx;
1455                                 gfx_fb_setpixel(x2, y0);
1456                                 e2 += dy;
1457                         }
1458                         if (y0 == y1)
1459                                 break;
1460                         err += dx;
1461                         y0 += sy;
1462                 }
1463         }
1464 }
1465
1466 /*
1467  * quadratic Bézier curve limited to gradients without sign change.
1468  */
1469 void
1470 gfx_fb_bezier(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t x2,
1471     uint32_t y2, uint32_t wd)
1472 {
1473         int sx, sy, xx, yy, xy, width;
1474         int dx, dy, err, curvature;
1475         int i;
1476
1477         if (gfx_state.tg_fb_type == FB_TEXT)
1478                 return;
1479
1480         width = wd;
1481         sx = x2 - x1;
1482         sy = y2 - y1;
1483         xx = x0 - x1;
1484         yy = y0 - y1;
1485         curvature = xx*sy - yy*sx;
1486
1487         if (sx*sx + sy*sy > xx*xx+yy*yy) {
1488                 x2 = x0;
1489                 x0 = sx + x1;
1490                 y2 = y0;
1491                 y0 = sy + y1;
1492                 curvature = -curvature;
1493         }
1494         if (curvature != 0) {
1495                 xx += sx;
1496                 sx = x0 < x2? 1 : -1;
1497                 xx *= sx;
1498                 yy += sy;
1499                 sy = y0 < y2? 1 : -1;
1500                 yy *= sy;
1501                 xy = (xx*yy) << 1;
1502                 xx *= xx;
1503                 yy *= yy;
1504                 if (curvature * sx * sy < 0) {
1505                         xx = -xx;
1506                         yy = -yy;
1507                         xy = -xy;
1508                         curvature = -curvature;
1509                 }
1510                 dx = 4 * sy * curvature * (x1 - x0) + xx - xy;
1511                 dy = 4 * sx * curvature * (y0 - y1) + yy - xy;
1512                 xx += xx;
1513                 yy += yy;
1514                 err = dx + dy + xy;
1515                 do {
1516                         for (i = 0; i <= width; i++)
1517                                 gfx_fb_setpixel(x0 + i, y0);
1518                         if (x0 == x2 && y0 == y2)
1519                                 return;  /* last pixel -> curve finished */
1520                         y1 = 2 * err < dx;
1521                         if (2 * err > dy) {
1522                                 x0 += sx;
1523                                 dx -= xy;
1524                                 dy += yy;
1525                                 err += dy;
1526                         }
1527                         if (y1 != 0) {
1528                                 y0 += sy;
1529                                 dy -= xy;
1530                                 dx += xx;
1531                                 err += dx;
1532                         }
1533                 } while (dy < dx); /* gradient negates -> algorithm fails */
1534         }
1535         gfx_fb_line(x0, y0, x2, y2, width);
1536 }
1537
1538 /*
1539  * draw rectangle using terminal coordinates and current foreground color.
1540  */
1541 void
1542 gfx_term_drawrect(uint32_t ux1, uint32_t uy1, uint32_t ux2, uint32_t uy2)
1543 {
1544         int x1, y1, x2, y2;
1545         int xshift, yshift;
1546         int width, i;
1547         uint32_t vf_width, vf_height;
1548         teken_rect_t r;
1549
1550         if (gfx_state.tg_fb_type == FB_TEXT)
1551                 return;
1552
1553         vf_width = gfx_state.tg_font.vf_width;
1554         vf_height = gfx_state.tg_font.vf_height;
1555         width = vf_width / 4;                   /* line width */
1556         xshift = (vf_width - width) / 2;
1557         yshift = (vf_height - width) / 2;
1558
1559         /* Shift coordinates */
1560         if (ux1 != 0)
1561                 ux1--;
1562         if (uy1 != 0)
1563                 uy1--;
1564         ux2--;
1565         uy2--;
1566
1567         /* mark area used in terminal */
1568         r.tr_begin.tp_col = ux1;
1569         r.tr_begin.tp_row = uy1;
1570         r.tr_end.tp_col = ux2 + 1;
1571         r.tr_end.tp_row = uy2 + 1;
1572
1573         term_image_display(&gfx_state, &r);
1574
1575         /*
1576          * Draw horizontal lines width points thick, shifted from outer edge.
1577          */
1578         x1 = (ux1 + 1) * vf_width + gfx_state.tg_origin.tp_col;
1579         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1580         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1581         gfx_fb_drawrect(x1, y1, x2, y1 + width, 1);
1582         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1583         y2 += vf_height - yshift - width;
1584         gfx_fb_drawrect(x1, y2, x2, y2 + width, 1);
1585
1586         /*
1587          * Draw vertical lines width points thick, shifted from outer edge.
1588          */
1589         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1590         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1591         y1 += vf_height;
1592         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1593         gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
1594         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1595         x1 += vf_width - xshift - width;
1596         gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
1597
1598         /* Draw upper left corner. */
1599         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1600         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1601         y1 += vf_height;
1602
1603         x2 = ux1 * vf_width + gfx_state.tg_origin.tp_col;
1604         x2 += vf_width;
1605         y2 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1606         for (i = 0; i <= width; i++)
1607                 gfx_fb_bezier(x1 + i, y1, x1 + i, y2 + i, x2, y2 + i, width-i);
1608
1609         /* Draw lower left corner. */
1610         x1 = ux1 * vf_width + gfx_state.tg_origin.tp_col;
1611         x1 += vf_width;
1612         y1 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1613         y1 += vf_height - yshift;
1614         x2 = ux1 * vf_width + gfx_state.tg_origin.tp_col + xshift;
1615         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1616         for (i = 0; i <= width; i++)
1617                 gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
1618
1619         /* Draw upper right corner. */
1620         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1621         y1 = uy1 * vf_height + gfx_state.tg_origin.tp_row + yshift;
1622         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1623         x2 += vf_width - xshift - width;
1624         y2 = uy1 * vf_height + gfx_state.tg_origin.tp_row;
1625         y2 += vf_height;
1626         for (i = 0; i <= width; i++)
1627                 gfx_fb_bezier(x1, y1 + i, x2 + i, y1 + i, x2 + i, y2, width-i);
1628
1629         /* Draw lower right corner. */
1630         x1 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1631         y1 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1632         y1 += vf_height - yshift;
1633         x2 = ux2 * vf_width + gfx_state.tg_origin.tp_col;
1634         x2 += vf_width - xshift - width;
1635         y2 = uy2 * vf_height + gfx_state.tg_origin.tp_row;
1636         for (i = 0; i <= width; i++)
1637                 gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
1638 }
1639
1640 int
1641 gfx_fb_putimage(png_t *png, uint32_t ux1, uint32_t uy1, uint32_t ux2,
1642     uint32_t uy2, uint32_t flags)
1643 {
1644 #if defined(EFI)
1645         EFI_GRAPHICS_OUTPUT_BLT_PIXEL *p;
1646 #else
1647         struct paletteentry *p;
1648 #endif
1649         uint8_t *data;
1650         uint32_t i, j, x, y, fheight, fwidth;
1651         int rs, gs, bs;
1652         uint8_t r, g, b, a;
1653         bool scale = false;
1654         bool trace = false;
1655         teken_rect_t rect;
1656
1657         trace = (flags & FL_PUTIMAGE_DEBUG) != 0;
1658
1659         if (gfx_state.tg_fb_type == FB_TEXT) {
1660                 if (trace)
1661                         printf("Framebuffer not active.\n");
1662                 return (1);
1663         }
1664
1665         if (png->color_type != PNG_TRUECOLOR_ALPHA) {
1666                 if (trace)
1667                         printf("Not truecolor image.\n");
1668                 return (1);
1669         }
1670
1671         if (ux1 > gfx_state.tg_fb.fb_width ||
1672             uy1 > gfx_state.tg_fb.fb_height) {
1673                 if (trace)
1674                         printf("Top left coordinate off screen.\n");
1675                 return (1);
1676         }
1677
1678         if (png->width > UINT16_MAX || png->height > UINT16_MAX) {
1679                 if (trace)
1680                         printf("Image too large.\n");
1681                 return (1);
1682         }
1683
1684         if (png->width < 1 || png->height < 1) {
1685                 if (trace)
1686                         printf("Image too small.\n");
1687                 return (1);
1688         }
1689
1690         /*
1691          * If 0 was passed for either ux2 or uy2, then calculate the missing
1692          * part of the bottom right coordinate.
1693          */
1694         scale = true;
1695         if (ux2 == 0 && uy2 == 0) {
1696                 /* Both 0, use the native resolution of the image */
1697                 ux2 = ux1 + png->width;
1698                 uy2 = uy1 + png->height;
1699                 scale = false;
1700         } else if (ux2 == 0) {
1701                 /* Set ux2 from uy2/uy1 to maintain aspect ratio */
1702                 ux2 = ux1 + (png->width * (uy2 - uy1)) / png->height;
1703         } else if (uy2 == 0) {
1704                 /* Set uy2 from ux2/ux1 to maintain aspect ratio */
1705                 uy2 = uy1 + (png->height * (ux2 - ux1)) / png->width;
1706         }
1707
1708         if (ux2 > gfx_state.tg_fb.fb_width ||
1709             uy2 > gfx_state.tg_fb.fb_height) {
1710                 if (trace)
1711                         printf("Bottom right coordinate off screen.\n");
1712                 return (1);
1713         }
1714
1715         fwidth = ux2 - ux1;
1716         fheight = uy2 - uy1;
1717
1718         /*
1719          * If the original image dimensions have been passed explicitly,
1720          * disable scaling.
1721          */
1722         if (fwidth == png->width && fheight == png->height)
1723                 scale = false;
1724
1725         if (ux1 == 0) {
1726                 /*
1727                  * No top left X co-ordinate (real coordinates start at 1),
1728                  * place as far right as it will fit.
1729                  */
1730                 ux2 = gfx_state.tg_fb.fb_width - gfx_state.tg_origin.tp_col;
1731                 ux1 = ux2 - fwidth;
1732         }
1733
1734         if (uy1 == 0) {
1735                 /*
1736                  * No top left Y co-ordinate (real coordinates start at 1),
1737                  * place as far down as it will fit.
1738                  */
1739                 uy2 = gfx_state.tg_fb.fb_height - gfx_state.tg_origin.tp_row;
1740                 uy1 = uy2 - fheight;
1741         }
1742
1743         if (ux1 >= ux2 || uy1 >= uy2) {
1744                 if (trace)
1745                         printf("Image dimensions reversed.\n");
1746                 return (1);
1747         }
1748
1749         if (fwidth < 2 || fheight < 2) {
1750                 if (trace)
1751                         printf("Target area too small\n");
1752                 return (1);
1753         }
1754
1755         if (trace)
1756                 printf("Image %ux%u -> %ux%u @%ux%u\n",
1757                     png->width, png->height, fwidth, fheight, ux1, uy1);
1758
1759         rect.tr_begin.tp_col = ux1 / gfx_state.tg_font.vf_width;
1760         rect.tr_begin.tp_row = uy1 / gfx_state.tg_font.vf_height;
1761         rect.tr_end.tp_col = (ux1 + fwidth) / gfx_state.tg_font.vf_width;
1762         rect.tr_end.tp_row = (uy1 + fheight) / gfx_state.tg_font.vf_height;
1763
1764         /*
1765          * mark area used in terminal
1766          */
1767         if (!(flags & FL_PUTIMAGE_NOSCROLL))
1768                 term_image_display(&gfx_state, &rect);
1769
1770         if ((flags & FL_PUTIMAGE_BORDER))
1771                 gfx_fb_drawrect(ux1, uy1, ux2, uy2, 0);
1772
1773         data = malloc(fwidth * fheight * sizeof(*p));
1774         p = (void *)data;
1775         if (data == NULL) {
1776                 if (trace)
1777                         printf("Out of memory.\n");
1778                 return (1);
1779         }
1780
1781         /*
1782          * Build image for our framebuffer.
1783          */
1784
1785         /* Helper to calculate the pixel index from the source png */
1786 #define GETPIXEL(xx, yy)        (((yy) * png->width + (xx)) * png->bpp)
1787
1788         /*
1789          * For each of the x and y directions, calculate the number of pixels
1790          * in the source image that correspond to a single pixel in the target.
1791          * Use fixed-point arithmetic with 16-bits for each of the integer and
1792          * fractional parts.
1793          */
1794         const uint32_t wcstep = ((png->width - 1) << 16) / (fwidth - 1);
1795         const uint32_t hcstep = ((png->height - 1) << 16) / (fheight - 1);
1796
1797         rs = 8 - (fls(gfx_state.tg_fb.fb_mask_red) -
1798             ffs(gfx_state.tg_fb.fb_mask_red) + 1);
1799         gs = 8 - (fls(gfx_state.tg_fb.fb_mask_green) -
1800             ffs(gfx_state.tg_fb.fb_mask_green) + 1);
1801         bs = 8 - (fls(gfx_state.tg_fb.fb_mask_blue) -
1802             ffs(gfx_state.tg_fb.fb_mask_blue) + 1);
1803
1804         uint32_t hc = 0;
1805         for (y = 0; y < fheight; y++) {
1806                 uint32_t hc2 = (hc >> 9) & 0x7f;
1807                 uint32_t hc1 = 0x80 - hc2;
1808
1809                 uint32_t offset_y = hc >> 16;
1810                 uint32_t offset_y1 = offset_y + 1;
1811
1812                 uint32_t wc = 0;
1813                 for (x = 0; x < fwidth; x++) {
1814                         uint32_t wc2 = (wc >> 9) & 0x7f;
1815                         uint32_t wc1 = 0x80 - wc2;
1816
1817                         uint32_t offset_x = wc >> 16;
1818                         uint32_t offset_x1 = offset_x + 1;
1819
1820                         /* Target pixel index */
1821                         j = y * fwidth + x;
1822
1823                         if (!scale) {
1824                                 i = GETPIXEL(x, y);
1825                                 r = png->image[i];
1826                                 g = png->image[i + 1];
1827                                 b = png->image[i + 2];
1828                                 a = png->image[i + 3];
1829                         } else {
1830                                 uint8_t pixel[4];
1831
1832                                 uint32_t p00 = GETPIXEL(offset_x, offset_y);
1833                                 uint32_t p01 = GETPIXEL(offset_x, offset_y1);
1834                                 uint32_t p10 = GETPIXEL(offset_x1, offset_y);
1835                                 uint32_t p11 = GETPIXEL(offset_x1, offset_y1);
1836
1837                                 /*
1838                                  * Given a 2x2 array of pixels in the source
1839                                  * image, combine them to produce a single
1840                                  * value for the pixel in the target image.
1841                                  * Each column of pixels is combined using
1842                                  * a weighted average where the top and bottom
1843                                  * pixels contribute hc1 and hc2 respectively.
1844                                  * The calculation for bottom pixel pB and
1845                                  * top pixel pT is:
1846                                  *   (pT * hc1 + pB * hc2) / (hc1 + hc2)
1847                                  * Once the values are determined for the two
1848                                  * columns of pixels, then the columns are
1849                                  * averaged together in the same way but using
1850                                  * wc1 and wc2 for the weightings.
1851                                  *
1852                                  * Since hc1 and hc2 are chosen so that
1853                                  * hc1 + hc2 == 128 (and same for wc1 + wc2),
1854                                  * the >> 14 below is a quick way to divide by
1855                                  * (hc1 + hc2) * (wc1 + wc2)
1856                                  */
1857                                 for (i = 0; i < 4; i++)
1858                                         pixel[i] = (
1859                                             (png->image[p00 + i] * hc1 +
1860                                             png->image[p01 + i] * hc2) * wc1 +
1861                                             (png->image[p10 + i] * hc1 +
1862                                             png->image[p11 + i] * hc2) * wc2)
1863                                             >> 14;
1864
1865                                 r = pixel[0];
1866                                 g = pixel[1];
1867                                 b = pixel[2];
1868                                 a = pixel[3];
1869                         }
1870
1871                         if (trace)
1872                                 printf("r/g/b: %x/%x/%x\n", r, g, b);
1873                         /*
1874                          * Rough colorspace reduction for 15/16 bit colors.
1875                          */
1876                         p[j].Red = r >> rs;
1877                         p[j].Green = g >> gs;
1878                         p[j].Blue = b >> bs;
1879                         p[j].Reserved = a;
1880
1881                         wc += wcstep;
1882                 }
1883                 hc += hcstep;
1884         }
1885
1886         gfx_fb_cons_display(ux1, uy1, fwidth, fheight, data);
1887         free(data);
1888         return (0);
1889 }
1890
1891 /*
1892  * Reset font flags to FONT_AUTO.
1893  */
1894 void
1895 reset_font_flags(void)
1896 {
1897         struct fontlist *fl;
1898
1899         STAILQ_FOREACH(fl, &fonts, font_next) {
1900                 fl->font_flags = FONT_AUTO;
1901         }
1902 }
1903
1904 /* Return  w^2 + h^2 or 0, if the dimensions are unknown */
1905 static unsigned
1906 edid_diagonal_squared(void)
1907 {
1908         unsigned w, h;
1909
1910         if (edid_info == NULL)
1911                 return (0);
1912
1913         w = edid_info->display.max_horizontal_image_size;
1914         h = edid_info->display.max_vertical_image_size;
1915
1916         /* If either one is 0, we have aspect ratio, not size */
1917         if (w == 0 || h == 0)
1918                 return (0);
1919
1920         /*
1921          * some monitors encode the aspect ratio instead of the physical size.
1922          */
1923         if ((w == 16 && h == 9) || (w == 16 && h == 10) ||
1924             (w == 4 && h == 3) || (w == 5 && h == 4))
1925                 return (0);
1926
1927         /*
1928          * translate cm to inch, note we scale by 100 here.
1929          */
1930         w = w * 100 / 254;
1931         h = h * 100 / 254;
1932
1933         /* Return w^2 + h^2 */
1934         return (w * w + h * h);
1935 }
1936
1937 /*
1938  * calculate pixels per inch.
1939  */
1940 static unsigned
1941 gfx_get_ppi(void)
1942 {
1943         unsigned dp, di;
1944
1945         di = edid_diagonal_squared();
1946         if (di == 0)
1947                 return (0);
1948
1949         dp = gfx_state.tg_fb.fb_width *
1950             gfx_state.tg_fb.fb_width +
1951             gfx_state.tg_fb.fb_height *
1952             gfx_state.tg_fb.fb_height;
1953
1954         return (isqrt(dp / di));
1955 }
1956
1957 /*
1958  * Calculate font size from density independent pixels (dp):
1959  * ((16dp * ppi) / 160) * display_factor.
1960  * Here we are using fixed constants: 1dp == 160 ppi and
1961  * display_factor 2.
1962  *
1963  * We are rounding font size up and are searching for font which is
1964  * not smaller than calculated size value.
1965  */
1966 static vt_font_bitmap_data_t *
1967 gfx_get_font(void)
1968 {
1969         unsigned ppi, size;
1970         vt_font_bitmap_data_t *font = NULL;
1971         struct fontlist *fl, *next;
1972
1973         /* Text mode is not supported here. */
1974         if (gfx_state.tg_fb_type == FB_TEXT)
1975                 return (NULL);
1976
1977         ppi = gfx_get_ppi();
1978         if (ppi == 0)
1979                 return (NULL);
1980
1981         /*
1982          * We will search for 16dp font.
1983          * We are using scale up by 10 for roundup.
1984          */
1985         size = (16 * ppi * 10) / 160;
1986         /* Apply display factor 2.  */
1987         size = roundup(size * 2, 10) / 10;
1988
1989         STAILQ_FOREACH(fl, &fonts, font_next) {
1990                 next = STAILQ_NEXT(fl, font_next);
1991
1992                 /*
1993                  * If this is last font or, if next font is smaller,
1994                  * we have our font. Make sure, it actually is loaded.
1995                  */
1996                 if (next == NULL || next->font_data->vfbd_height < size) {
1997                         font = fl->font_data;
1998                         if (font->vfbd_font == NULL ||
1999                             fl->font_flags == FONT_RELOAD) {
2000                                 if (fl->font_load != NULL &&
2001                                     fl->font_name != NULL)
2002                                         font = fl->font_load(fl->font_name);
2003                         }
2004                         break;
2005                 }
2006         }
2007
2008         return (font);
2009 }
2010
2011 static vt_font_bitmap_data_t *
2012 set_font(teken_unit_t *rows, teken_unit_t *cols, teken_unit_t h, teken_unit_t w)
2013 {
2014         vt_font_bitmap_data_t *font = NULL;
2015         struct fontlist *fl;
2016         unsigned height = h;
2017         unsigned width = w;
2018
2019         /*
2020          * First check for manually loaded font.
2021          */
2022         STAILQ_FOREACH(fl, &fonts, font_next) {
2023                 if (fl->font_flags == FONT_MANUAL) {
2024                         font = fl->font_data;
2025                         if (font->vfbd_font == NULL && fl->font_load != NULL &&
2026                             fl->font_name != NULL) {
2027                                 font = fl->font_load(fl->font_name);
2028                         }
2029                         if (font == NULL || font->vfbd_font == NULL)
2030                                 font = NULL;
2031                         break;
2032                 }
2033         }
2034
2035         if (font == NULL)
2036                 font = gfx_get_font();
2037
2038         if (font != NULL) {
2039                 *rows = height / font->vfbd_height;
2040                 *cols = width / font->vfbd_width;
2041                 return (font);
2042         }
2043
2044         /*
2045          * Find best font for these dimensions, or use default.
2046          * If height >= VT_FB_MAX_HEIGHT and width >= VT_FB_MAX_WIDTH,
2047          * do not use smaller font than our DEFAULT_FONT_DATA.
2048          */
2049         STAILQ_FOREACH(fl, &fonts, font_next) {
2050                 font = fl->font_data;
2051                 if ((*rows * font->vfbd_height <= height &&
2052                     *cols * font->vfbd_width <= width) ||
2053                     (height >= VT_FB_MAX_HEIGHT &&
2054                     width >= VT_FB_MAX_WIDTH &&
2055                     font->vfbd_height == DEFAULT_FONT_DATA.vfbd_height &&
2056                     font->vfbd_width == DEFAULT_FONT_DATA.vfbd_width)) {
2057                         if (font->vfbd_font == NULL ||
2058                             fl->font_flags == FONT_RELOAD) {
2059                                 if (fl->font_load != NULL &&
2060                                     fl->font_name != NULL) {
2061                                         font = fl->font_load(fl->font_name);
2062                                 }
2063                                 if (font == NULL)
2064                                         continue;
2065                         }
2066                         *rows = height / font->vfbd_height;
2067                         *cols = width / font->vfbd_width;
2068                         break;
2069                 }
2070                 font = NULL;
2071         }
2072
2073         if (font == NULL) {
2074                 /*
2075                  * We have fonts sorted smallest last, try it before
2076                  * falling back to builtin.
2077                  */
2078                 fl = STAILQ_LAST(&fonts, fontlist, font_next);
2079                 if (fl != NULL && fl->font_load != NULL &&
2080                     fl->font_name != NULL) {
2081                         font = fl->font_load(fl->font_name);
2082                 }
2083                 if (font == NULL)
2084                         font = &DEFAULT_FONT_DATA;
2085
2086                 *rows = height / font->vfbd_height;
2087                 *cols = width / font->vfbd_width;
2088         }
2089
2090         return (font);
2091 }
2092
2093 static void
2094 cons_clear(void)
2095 {
2096         char clear[] = { '\033', 'c' };
2097
2098         /* Reset terminal */
2099         teken_input(&gfx_state.tg_teken, clear, sizeof(clear));
2100         gfx_state.tg_functions->tf_param(&gfx_state, TP_SHOWCURSOR, 0);
2101 }
2102
2103 void
2104 setup_font(teken_gfx_t *state, teken_unit_t height, teken_unit_t width)
2105 {
2106         vt_font_bitmap_data_t *font_data;
2107         teken_pos_t *tp = &state->tg_tp;
2108         char env[8];
2109         int i;
2110
2111         /*
2112          * set_font() will select a appropriate sized font for
2113          * the number of rows and columns selected.  If we don't
2114          * have a font that will fit, then it will use the
2115          * default builtin font and adjust the rows and columns
2116          * to fit on the screen.
2117          */
2118         font_data = set_font(&tp->tp_row, &tp->tp_col, height, width);
2119
2120         if (font_data == NULL)
2121                 panic("out of memory");
2122
2123         for (i = 0; i < VFNT_MAPS; i++) {
2124                 state->tg_font.vf_map[i] =
2125                     font_data->vfbd_font->vf_map[i];
2126                 state->tg_font.vf_map_count[i] =
2127                     font_data->vfbd_font->vf_map_count[i];
2128         }
2129
2130         state->tg_font.vf_bytes = font_data->vfbd_font->vf_bytes;
2131         state->tg_font.vf_height = font_data->vfbd_font->vf_height;
2132         state->tg_font.vf_width = font_data->vfbd_font->vf_width;
2133
2134         snprintf(env, sizeof (env), "%ux%u",
2135             state->tg_font.vf_width, state->tg_font.vf_height);
2136         env_setenv("screen.font", EV_VOLATILE | EV_NOHOOK,
2137             env, font_set, env_nounset);
2138 }
2139
2140 /* Binary search for the glyph. Return 0 if not found. */
2141 static uint16_t
2142 font_bisearch(const vfnt_map_t *map, uint32_t len, teken_char_t src)
2143 {
2144         unsigned min, mid, max;
2145
2146         min = 0;
2147         max = len - 1;
2148
2149         /* Empty font map. */
2150         if (len == 0)
2151                 return (0);
2152         /* Character below minimal entry. */
2153         if (src < map[0].vfm_src)
2154                 return (0);
2155         /* Optimization: ASCII characters occur very often. */
2156         if (src <= map[0].vfm_src + map[0].vfm_len)
2157                 return (src - map[0].vfm_src + map[0].vfm_dst);
2158         /* Character above maximum entry. */
2159         if (src > map[max].vfm_src + map[max].vfm_len)
2160                 return (0);
2161
2162         /* Binary search. */
2163         while (max >= min) {
2164                 mid = (min + max) / 2;
2165                 if (src < map[mid].vfm_src)
2166                         max = mid - 1;
2167                 else if (src > map[mid].vfm_src + map[mid].vfm_len)
2168                         min = mid + 1;
2169                 else
2170                         return (src - map[mid].vfm_src + map[mid].vfm_dst);
2171         }
2172
2173         return (0);
2174 }
2175
2176 /*
2177  * Return glyph bitmap. If glyph is not found, we will return bitmap
2178  * for the first (offset 0) glyph.
2179  */
2180 uint8_t *
2181 font_lookup(const struct vt_font *vf, teken_char_t c, const teken_attr_t *a)
2182 {
2183         uint16_t dst;
2184         size_t stride;
2185
2186         /* Substitute bold with normal if not found. */
2187         if (a->ta_format & TF_BOLD) {
2188                 dst = font_bisearch(vf->vf_map[VFNT_MAP_BOLD],
2189                     vf->vf_map_count[VFNT_MAP_BOLD], c);
2190                 if (dst != 0)
2191                         goto found;
2192         }
2193         dst = font_bisearch(vf->vf_map[VFNT_MAP_NORMAL],
2194             vf->vf_map_count[VFNT_MAP_NORMAL], c);
2195
2196 found:
2197         stride = howmany(vf->vf_width, 8) * vf->vf_height;
2198         return (&vf->vf_bytes[dst * stride]);
2199 }
2200
2201 static int
2202 load_mapping(int fd, struct vt_font *fp, int n)
2203 {
2204         size_t i, size;
2205         ssize_t rv;
2206         vfnt_map_t *mp;
2207
2208         if (fp->vf_map_count[n] == 0)
2209                 return (0);
2210
2211         size = fp->vf_map_count[n] * sizeof(*mp);
2212         mp = malloc(size);
2213         if (mp == NULL)
2214                 return (ENOMEM);
2215         fp->vf_map[n] = mp;
2216
2217         rv = read(fd, mp, size);
2218         if (rv < 0 || (size_t)rv != size) {
2219                 free(fp->vf_map[n]);
2220                 fp->vf_map[n] = NULL;
2221                 return (EIO);
2222         }
2223
2224         for (i = 0; i < fp->vf_map_count[n]; i++) {
2225                 mp[i].vfm_src = be32toh(mp[i].vfm_src);
2226                 mp[i].vfm_dst = be16toh(mp[i].vfm_dst);
2227                 mp[i].vfm_len = be16toh(mp[i].vfm_len);
2228         }
2229         return (0);
2230 }
2231
2232 static int
2233 builtin_mapping(struct vt_font *fp, int n)
2234 {
2235         size_t size;
2236         struct vfnt_map *mp;
2237
2238         if (n >= VFNT_MAPS)
2239                 return (EINVAL);
2240
2241         if (fp->vf_map_count[n] == 0)
2242                 return (0);
2243
2244         size = fp->vf_map_count[n] * sizeof(*mp);
2245         mp = malloc(size);
2246         if (mp == NULL)
2247                 return (ENOMEM);
2248         fp->vf_map[n] = mp;
2249
2250         memcpy(mp, DEFAULT_FONT_DATA.vfbd_font->vf_map[n], size);
2251         return (0);
2252 }
2253
2254 /*
2255  * Load font from builtin or from file.
2256  * We do need special case for builtin because the builtin font glyphs
2257  * are compressed and we do need to uncompress them.
2258  * Having single load_font() for both cases will help us to simplify
2259  * font switch handling.
2260  */
2261 static vt_font_bitmap_data_t *
2262 load_font(char *path)
2263 {
2264         int fd, i;
2265         uint32_t glyphs;
2266         struct font_header fh;
2267         struct fontlist *fl;
2268         vt_font_bitmap_data_t *bp;
2269         struct vt_font *fp;
2270         size_t size;
2271         ssize_t rv;
2272
2273         /* Get our entry from the font list. */
2274         STAILQ_FOREACH(fl, &fonts, font_next) {
2275                 if (strcmp(fl->font_name, path) == 0)
2276                         break;
2277         }
2278         if (fl == NULL)
2279                 return (NULL);  /* Should not happen. */
2280
2281         bp = fl->font_data;
2282         if (bp->vfbd_font != NULL && fl->font_flags != FONT_RELOAD)
2283                 return (bp);
2284
2285         fd = -1;
2286         /*
2287          * Special case for builtin font.
2288          * Builtin font is the very first font we load, we do not have
2289          * previous loads to be released.
2290          */
2291         if (fl->font_flags == FONT_BUILTIN) {
2292                 if ((fp = calloc(1, sizeof(struct vt_font))) == NULL)
2293                         return (NULL);
2294
2295                 fp->vf_width = DEFAULT_FONT_DATA.vfbd_width;
2296                 fp->vf_height = DEFAULT_FONT_DATA.vfbd_height;
2297
2298                 fp->vf_bytes = malloc(DEFAULT_FONT_DATA.vfbd_uncompressed_size);
2299                 if (fp->vf_bytes == NULL) {
2300                         free(fp);
2301                         return (NULL);
2302                 }
2303
2304                 bp->vfbd_uncompressed_size =
2305                     DEFAULT_FONT_DATA.vfbd_uncompressed_size;
2306                 bp->vfbd_compressed_size =
2307                     DEFAULT_FONT_DATA.vfbd_compressed_size;
2308
2309                 if (lz4_decompress(DEFAULT_FONT_DATA.vfbd_compressed_data,
2310                     fp->vf_bytes,
2311                     DEFAULT_FONT_DATA.vfbd_compressed_size,
2312                     DEFAULT_FONT_DATA.vfbd_uncompressed_size, 0) != 0) {
2313                         free(fp->vf_bytes);
2314                         free(fp);
2315                         return (NULL);
2316                 }
2317
2318                 for (i = 0; i < VFNT_MAPS; i++) {
2319                         fp->vf_map_count[i] =
2320                             DEFAULT_FONT_DATA.vfbd_font->vf_map_count[i];
2321                         if (builtin_mapping(fp, i) != 0)
2322                                 goto free_done;
2323                 }
2324
2325                 bp->vfbd_font = fp;
2326                 return (bp);
2327         }
2328
2329         fd = open(path, O_RDONLY);
2330         if (fd < 0)
2331                 return (NULL);
2332
2333         size = sizeof(fh);
2334         rv = read(fd, &fh, size);
2335         if (rv < 0 || (size_t)rv != size) {
2336                 bp = NULL;
2337                 goto done;
2338         }
2339         if (memcmp(fh.fh_magic, FONT_HEADER_MAGIC, sizeof(fh.fh_magic)) != 0) {
2340                 bp = NULL;
2341                 goto done;
2342         }
2343         if ((fp = calloc(1, sizeof(struct vt_font))) == NULL) {
2344                 bp = NULL;
2345                 goto done;
2346         }
2347         for (i = 0; i < VFNT_MAPS; i++)
2348                 fp->vf_map_count[i] = be32toh(fh.fh_map_count[i]);
2349
2350         glyphs = be32toh(fh.fh_glyph_count);
2351         fp->vf_width = fh.fh_width;
2352         fp->vf_height = fh.fh_height;
2353
2354         size = howmany(fp->vf_width, 8) * fp->vf_height * glyphs;
2355         bp->vfbd_uncompressed_size = size;
2356         if ((fp->vf_bytes = malloc(size)) == NULL)
2357                 goto free_done;
2358
2359         rv = read(fd, fp->vf_bytes, size);
2360         if (rv < 0 || (size_t)rv != size)
2361                 goto free_done;
2362         for (i = 0; i < VFNT_MAPS; i++) {
2363                 if (load_mapping(fd, fp, i) != 0)
2364                         goto free_done;
2365         }
2366
2367         /*
2368          * Reset builtin flag now as we have full font loaded.
2369          */
2370         if (fl->font_flags == FONT_BUILTIN)
2371                 fl->font_flags = FONT_AUTO;
2372
2373         /*
2374          * Release previously loaded entries. We can do this now, as
2375          * the new font is loaded. Note, there can be no console
2376          * output till the new font is in place and teken is notified.
2377          * We do need to keep fl->font_data for glyph dimensions.
2378          */
2379         STAILQ_FOREACH(fl, &fonts, font_next) {
2380                 if (fl->font_data->vfbd_font == NULL)
2381                         continue;
2382
2383                 for (i = 0; i < VFNT_MAPS; i++)
2384                         free(fl->font_data->vfbd_font->vf_map[i]);
2385                 free(fl->font_data->vfbd_font->vf_bytes);
2386                 free(fl->font_data->vfbd_font);
2387                 fl->font_data->vfbd_font = NULL;
2388         }
2389
2390         bp->vfbd_font = fp;
2391         bp->vfbd_compressed_size = 0;
2392
2393 done:
2394         if (fd != -1)
2395                 close(fd);
2396         return (bp);
2397
2398 free_done:
2399         for (i = 0; i < VFNT_MAPS; i++)
2400                 free(fp->vf_map[i]);
2401         free(fp->vf_bytes);
2402         free(fp);
2403         bp = NULL;
2404         goto done;
2405 }
2406
2407 struct name_entry {
2408         char                    *n_name;
2409         SLIST_ENTRY(name_entry) n_entry;
2410 };
2411
2412 SLIST_HEAD(name_list, name_entry);
2413
2414 /* Read font names from index file. */
2415 static struct name_list *
2416 read_list(char *fonts)
2417 {
2418         struct name_list *nl;
2419         struct name_entry *np;
2420         char *dir, *ptr;
2421         char buf[PATH_MAX];
2422         int fd, len;
2423
2424         dir = strdup(fonts);
2425         if (dir == NULL)
2426                 return (NULL);
2427
2428         ptr = strrchr(dir, '/');
2429         *ptr = '\0';
2430
2431         fd = open(fonts, O_RDONLY);
2432         if (fd < 0)
2433                 return (NULL);
2434
2435         nl = malloc(sizeof(*nl));
2436         if (nl == NULL) {
2437                 close(fd);
2438                 return (nl);
2439         }
2440
2441         SLIST_INIT(nl);
2442         while ((len = fgetstr(buf, sizeof (buf), fd)) >= 0) {
2443                 if (*buf == '#' || *buf == '\0')
2444                         continue;
2445
2446                 if (bcmp(buf, "MENU", 4) == 0)
2447                         continue;
2448
2449                 if (bcmp(buf, "FONT", 4) == 0)
2450                         continue;
2451
2452                 ptr = strchr(buf, ':');
2453                 if (ptr == NULL)
2454                         continue;
2455                 else
2456                         *ptr = '\0';
2457
2458                 np = malloc(sizeof(*np));
2459                 if (np == NULL) {
2460                         close(fd);
2461                         return (nl);    /* return what we have */
2462                 }
2463                 if (asprintf(&np->n_name, "%s/%s", dir, buf) < 0) {
2464                         free(np);
2465                         close(fd);
2466                         return (nl);    /* return what we have */
2467                 }
2468                 SLIST_INSERT_HEAD(nl, np, n_entry);
2469         }
2470         close(fd);
2471         return (nl);
2472 }
2473
2474 /*
2475  * Read the font properties and insert new entry into the list.
2476  * The font list is built in descending order.
2477  */
2478 static bool
2479 insert_font(char *name, FONT_FLAGS flags)
2480 {
2481         struct font_header fh;
2482         struct fontlist *fp, *previous, *entry, *next;
2483         size_t size;
2484         ssize_t rv;
2485         int fd;
2486         char *font_name;
2487
2488         font_name = NULL;
2489         if (flags == FONT_BUILTIN) {
2490                 /*
2491                  * We only install builtin font once, while setting up
2492                  * initial console. Since this will happen very early,
2493                  * we assume asprintf will not fail. Once we have access to
2494                  * files, the builtin font will be replaced by font loaded
2495                  * from file.
2496                  */
2497                 if (!STAILQ_EMPTY(&fonts))
2498                         return (false);
2499
2500                 fh.fh_width = DEFAULT_FONT_DATA.vfbd_width;
2501                 fh.fh_height = DEFAULT_FONT_DATA.vfbd_height;
2502
2503                 (void) asprintf(&font_name, "%dx%d",
2504                     DEFAULT_FONT_DATA.vfbd_width,
2505                     DEFAULT_FONT_DATA.vfbd_height);
2506         } else {
2507                 fd = open(name, O_RDONLY);
2508                 if (fd < 0)
2509                         return (false);
2510                 rv = read(fd, &fh, sizeof(fh));
2511                 close(fd);
2512                 if (rv < 0 || (size_t)rv != sizeof(fh))
2513                         return (false);
2514
2515                 if (memcmp(fh.fh_magic, FONT_HEADER_MAGIC,
2516                     sizeof(fh.fh_magic)) != 0)
2517                         return (false);
2518                 font_name = strdup(name);
2519         }
2520
2521         if (font_name == NULL)
2522                 return (false);
2523
2524         /*
2525          * If we have an entry with the same glyph dimensions, replace
2526          * the file name and mark us. We only support unique dimensions.
2527          */
2528         STAILQ_FOREACH(entry, &fonts, font_next) {
2529                 if (fh.fh_width == entry->font_data->vfbd_width &&
2530                     fh.fh_height == entry->font_data->vfbd_height) {
2531                         free(entry->font_name);
2532                         entry->font_name = font_name;
2533                         entry->font_flags = FONT_RELOAD;
2534                         return (true);
2535                 }
2536         }
2537
2538         fp = calloc(sizeof(*fp), 1);
2539         if (fp == NULL) {
2540                 free(font_name);
2541                 return (false);
2542         }
2543         fp->font_data = calloc(sizeof(*fp->font_data), 1);
2544         if (fp->font_data == NULL) {
2545                 free(font_name);
2546                 free(fp);
2547                 return (false);
2548         }
2549         fp->font_name = font_name;
2550         fp->font_flags = flags;
2551         fp->font_load = load_font;
2552         fp->font_data->vfbd_width = fh.fh_width;
2553         fp->font_data->vfbd_height = fh.fh_height;
2554
2555         if (STAILQ_EMPTY(&fonts)) {
2556                 STAILQ_INSERT_HEAD(&fonts, fp, font_next);
2557                 return (true);
2558         }
2559
2560         previous = NULL;
2561         size = fp->font_data->vfbd_width * fp->font_data->vfbd_height;
2562
2563         STAILQ_FOREACH(entry, &fonts, font_next) {
2564                 vt_font_bitmap_data_t *bd;
2565
2566                 bd = entry->font_data;
2567                 /* Should fp be inserted before the entry? */
2568                 if (size > bd->vfbd_width * bd->vfbd_height) {
2569                         if (previous == NULL) {
2570                                 STAILQ_INSERT_HEAD(&fonts, fp, font_next);
2571                         } else {
2572                                 STAILQ_INSERT_AFTER(&fonts, previous, fp,
2573                                     font_next);
2574                         }
2575                         return (true);
2576                 }
2577                 next = STAILQ_NEXT(entry, font_next);
2578                 if (next == NULL ||
2579                     size > next->font_data->vfbd_width *
2580                     next->font_data->vfbd_height) {
2581                         STAILQ_INSERT_AFTER(&fonts, entry, fp, font_next);
2582                         return (true);
2583                 }
2584                 previous = entry;
2585         }
2586         return (true);
2587 }
2588
2589 static int
2590 font_set(struct env_var *ev __unused, int flags __unused, const void *value)
2591 {
2592         struct fontlist *fl;
2593         char *eptr;
2594         unsigned long x = 0, y = 0;
2595
2596         /*
2597          * Attempt to extract values from "XxY" string. In case of error,
2598          * we have unmaching glyph dimensions and will just output the
2599          * available values.
2600          */
2601         if (value != NULL) {
2602                 x = strtoul(value, &eptr, 10);
2603                 if (*eptr == 'x')
2604                         y = strtoul(eptr + 1, &eptr, 10);
2605         }
2606         STAILQ_FOREACH(fl, &fonts, font_next) {
2607                 if (fl->font_data->vfbd_width == x &&
2608                     fl->font_data->vfbd_height == y)
2609                         break;
2610         }
2611         if (fl != NULL) {
2612                 /* Reset any FONT_MANUAL flag. */
2613                 reset_font_flags();
2614
2615                 /* Mark this font manually loaded */
2616                 fl->font_flags = FONT_MANUAL;
2617                 cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2618                 return (CMD_OK);
2619         }
2620
2621         printf("Available fonts:\n");
2622         STAILQ_FOREACH(fl, &fonts, font_next) {
2623                 printf("    %dx%d\n", fl->font_data->vfbd_width,
2624                     fl->font_data->vfbd_height);
2625         }
2626         return (CMD_OK);
2627 }
2628
2629 void
2630 bios_text_font(bool use_vga_font)
2631 {
2632         if (use_vga_font)
2633                 (void) insert_font(VGA_8X16_FONT, FONT_MANUAL);
2634         else
2635                 (void) insert_font(DEFAULT_8X16_FONT, FONT_MANUAL);
2636 }
2637
2638 void
2639 autoload_font(bool bios)
2640 {
2641         struct name_list *nl;
2642         struct name_entry *np;
2643
2644         nl = read_list("/boot/fonts/INDEX.fonts");
2645         if (nl == NULL)
2646                 return;
2647
2648         while (!SLIST_EMPTY(nl)) {
2649                 np = SLIST_FIRST(nl);
2650                 SLIST_REMOVE_HEAD(nl, n_entry);
2651                 if (insert_font(np->n_name, FONT_AUTO) == false)
2652                         printf("failed to add font: %s\n", np->n_name);
2653                 free(np->n_name);
2654                 free(np);
2655         }
2656
2657         /*
2658          * If vga text mode was requested, load vga.font (8x16 bold) font.
2659          */
2660         if (bios) {
2661                 bios_text_font(true);
2662         }
2663
2664         (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2665 }
2666
2667 COMMAND_SET(load_font, "loadfont", "load console font from file", command_font);
2668
2669 static int
2670 command_font(int argc, char *argv[])
2671 {
2672         int i, c, rc;
2673         struct fontlist *fl;
2674         vt_font_bitmap_data_t *bd;
2675         bool list;
2676
2677         list = false;
2678         optind = 1;
2679         optreset = 1;
2680         rc = CMD_OK;
2681
2682         while ((c = getopt(argc, argv, "l")) != -1) {
2683                 switch (c) {
2684                 case 'l':
2685                         list = true;
2686                         break;
2687                 case '?':
2688                 default:
2689                         return (CMD_ERROR);
2690                 }
2691         }
2692
2693         argc -= optind;
2694         argv += optind;
2695
2696         if (argc > 1 || (list && argc != 0)) {
2697                 printf("Usage: loadfont [-l] | [file.fnt]\n");
2698                 return (CMD_ERROR);
2699         }
2700
2701         if (list) {
2702                 STAILQ_FOREACH(fl, &fonts, font_next) {
2703                         printf("font %s: %dx%d%s\n", fl->font_name,
2704                             fl->font_data->vfbd_width,
2705                             fl->font_data->vfbd_height,
2706                             fl->font_data->vfbd_font == NULL? "" : " loaded");
2707                 }
2708                 return (CMD_OK);
2709         }
2710
2711         /* Clear scren */
2712         cons_clear();
2713
2714         if (argc == 1) {
2715                 char *name = argv[0];
2716
2717                 if (insert_font(name, FONT_MANUAL) == false) {
2718                         printf("loadfont error: failed to load: %s\n", name);
2719                         return (CMD_ERROR);
2720                 }
2721
2722                 (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2723                 return (CMD_OK);
2724         }
2725
2726         if (argc == 0) {
2727                 /*
2728                  * Walk entire font list, release any loaded font, and set
2729                  * autoload flag. The font list does have at least the builtin
2730                  * default font.
2731                  */
2732                 STAILQ_FOREACH(fl, &fonts, font_next) {
2733                         if (fl->font_data->vfbd_font != NULL) {
2734
2735                                 bd = fl->font_data;
2736                                 /*
2737                                  * Note the setup_font() is releasing
2738                                  * font bytes.
2739                                  */
2740                                 for (i = 0; i < VFNT_MAPS; i++)
2741                                         free(bd->vfbd_font->vf_map[i]);
2742                                 free(fl->font_data->vfbd_font);
2743                                 fl->font_data->vfbd_font = NULL;
2744                                 fl->font_data->vfbd_uncompressed_size = 0;
2745                                 fl->font_flags = FONT_AUTO;
2746                         }
2747                 }
2748                 (void) cons_update_mode(gfx_state.tg_fb_type != FB_TEXT);
2749         }
2750         return (rc);
2751 }
2752
2753 bool
2754 gfx_get_edid_resolution(struct vesa_edid_info *edid, edid_res_list_t *res)
2755 {
2756         struct resolution *rp, *p;
2757
2758         /*
2759          * Walk detailed timings tables (4).
2760          */
2761         if ((edid->display.supported_features
2762             & EDID_FEATURE_PREFERRED_TIMING_MODE) != 0) {
2763                 /* Walk detailed timing descriptors (4) */
2764                 for (int i = 0; i < DET_TIMINGS; i++) {
2765                         /*
2766                          * Reserved value 0 is not used for display decriptor.
2767                          */
2768                         if (edid->detailed_timings[i].pixel_clock == 0)
2769                                 continue;
2770                         if ((rp = malloc(sizeof(*rp))) == NULL)
2771                                 continue;
2772                         rp->width = GET_EDID_INFO_WIDTH(edid, i);
2773                         rp->height = GET_EDID_INFO_HEIGHT(edid, i);
2774                         if (rp->width > 0 && rp->width <= EDID_MAX_PIXELS &&
2775                             rp->height > 0 && rp->height <= EDID_MAX_LINES)
2776                                 TAILQ_INSERT_TAIL(res, rp, next);
2777                         else
2778                                 free(rp);
2779                 }
2780         }
2781
2782         /*
2783          * Walk standard timings list (8).
2784          */
2785         for (int i = 0; i < STD_TIMINGS; i++) {
2786                 /* Is this field unused? */
2787                 if (edid->standard_timings[i] == 0x0101)
2788                         continue;
2789
2790                 if ((rp = malloc(sizeof(*rp))) == NULL)
2791                         continue;
2792
2793                 rp->width = HSIZE(edid->standard_timings[i]);
2794                 switch (RATIO(edid->standard_timings[i])) {
2795                 case RATIO1_1:
2796                         rp->height = HSIZE(edid->standard_timings[i]);
2797                         if (edid->header.version > 1 ||
2798                             edid->header.revision > 2) {
2799                                 rp->height = rp->height * 10 / 16;
2800                         }
2801                         break;
2802                 case RATIO4_3:
2803                         rp->height = HSIZE(edid->standard_timings[i]) * 3 / 4;
2804                         break;
2805                 case RATIO5_4:
2806                         rp->height = HSIZE(edid->standard_timings[i]) * 4 / 5;
2807                         break;
2808                 case RATIO16_9:
2809                         rp->height = HSIZE(edid->standard_timings[i]) * 9 / 16;
2810                         break;
2811                 }
2812
2813                 /*
2814                  * Create resolution list in decreasing order, except keep
2815                  * first entry (preferred timing mode).
2816                  */
2817                 TAILQ_FOREACH(p, res, next) {
2818                         if (p->width * p->height < rp->width * rp->height) {
2819                                 /* Keep preferred mode first */
2820                                 if (TAILQ_FIRST(res) == p)
2821                                         TAILQ_INSERT_AFTER(res, p, rp, next);
2822                                 else
2823                                         TAILQ_INSERT_BEFORE(p, rp, next);
2824                                 break;
2825                         }
2826                         if (TAILQ_NEXT(p, next) == NULL) {
2827                                 TAILQ_INSERT_TAIL(res, rp, next);
2828                                 break;
2829                         }
2830                 }
2831         }
2832         return (!TAILQ_EMPTY(res));
2833 }