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