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