stand/common/gfx_fb.c

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