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