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 */
15
16/*
17 * Graphics support for loader emulation.
18 * The interface in loader and here needs some more development.
19 * We can get colormap from gfx_private, but loader is currently
20 * relying on tem fg/bg colors for drawing, once the menu code
21 * will get some facelift, we would need to provide colors as menu component
22 * attributes and stop depending on tem.
23 */
24
25#include <stdio.h>
26#include <stdlib.h>
27#include <stdbool.h>
28#include <sys/types.h>
29#include <sys/stat.h>
30#include <fcntl.h>
31#include <unistd.h>
32#include <sys/mman.h>
33#include <sys/fbio.h>
34#include <string.h>
35#include "gfx_fb.h"
36
37struct framebuffer fb;
38
39#define	max(x, y)	((x) >= (y) ? (x) : (y))
40
41static void gfx_fb_cons_display(uint32_t, uint32_t,
42    uint32_t, uint32_t, uint8_t *);
43
44/* This colormap should be replaced by colormap query from kernel */
45typedef struct {
46	uint8_t red[16];
47	uint8_t green[16];
48	uint8_t blue[16];
49} text_cmap_t;
50
51text_cmap_t cmap4_to_24 = {
52/* BEGIN CSTYLED */
53/*             0    1    2    3    4    5    6    7    8    9   10   11   12   13   14   15
54              Wh+  Bk   Bl   Gr   Cy   Rd   Mg   Br   Wh   Bk+  Bl+  Gr+  Cy+  Rd+  Mg+  Yw */
55  .red   = {0xff,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x40,0x00,0x00,0x00,0xff,0xff,0xff},
56  .green = {0xff,0x00,0x00,0x80,0x80,0x00,0x00,0x80,0x80,0x40,0x00,0xff,0xff,0x00,0x00,0xff},
57  .blue  = {0xff,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x40,0xff,0x00,0xff,0x00,0xff,0x00}
58/* END CSTYLED */
59};
60
61const uint8_t solaris_color_to_pc_color[16] = {
62    15, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
63};
64
65void
66gfx_framework_init(void)
67{
68	struct fbgattr attr;
69	struct gfxfb_info *gfxfb_info;
70	char buf[10];
71
72	fb.fd = open("/dev/fb", O_RDWR);
73	if (fb.fd < 0)
74		return;
75
76	/* make sure we have GFX framebuffer */
77	if (ioctl(fb.fd, VIS_GETIDENTIFIER, &fb.ident) < 0 ||
78	    strcmp(fb.ident.name, "illumos_fb") != 0) {
79		(void) close(fb.fd);
80		fb.fd = -1;
81		return;
82	}
83
84	if (ioctl(fb.fd, FBIOGATTR, &attr) < 0) {
85		(void) close(fb.fd);
86		fb.fd = -1;
87		return;
88	}
89	gfxfb_info = (struct gfxfb_info *)attr.sattr.dev_specific;
90
91	fb.fb_height = attr.fbtype.fb_height;
92	fb.fb_width = attr.fbtype.fb_width;
93	fb.fb_depth = attr.fbtype.fb_depth;
94	fb.fb_size = attr.fbtype.fb_size;
95	fb.fb_bpp = attr.fbtype.fb_depth >> 3;
96	if (attr.fbtype.fb_depth == 15)
97		fb.fb_bpp = 2;
98	fb.fb_pitch = gfxfb_info->pitch;
99	fb.terminal_origin_x = gfxfb_info->terminal_origin_x;
100	fb.terminal_origin_y = gfxfb_info->terminal_origin_y;
101	fb.font_width = gfxfb_info->font_width;
102	fb.font_height = gfxfb_info->font_height;
103
104	fb.red_mask_size = gfxfb_info->red_mask_size;
105	fb.red_field_position = gfxfb_info->red_field_position;
106	fb.green_mask_size = gfxfb_info->green_mask_size;
107	fb.green_field_position = gfxfb_info->green_field_position;
108	fb.blue_mask_size = gfxfb_info->blue_mask_size;
109	fb.blue_field_position = gfxfb_info->blue_field_position;
110
111	fb.fb_addr = (uint8_t *)mmap(0, fb.fb_size, (PROT_READ | PROT_WRITE),
112	    MAP_SHARED, fb.fd, 0);
113
114	if (fb.fb_addr == NULL) {
115		(void) close(fb.fd);
116		fb.fd = -1;
117		return;
118	}
119	(void) snprintf(buf, sizeof (buf), "%d", fb.fb_height);
120	(void) setenv("screen-height", buf, 1);
121	(void) snprintf(buf, sizeof (buf), "%d", fb.fb_width);
122	(void) setenv("screen-width", buf, 1);
123}
124
125void
126gfx_framework_fini(void)
127{
128	if (fb.fd < 0)
129		return;
130
131	(void) munmap((caddr_t)fb.fb_addr, fb.fb_size);
132	(void) close(fb.fd);
133	fb.fd = -1;
134}
135
136static int
137isqrt(int num)
138{
139	int res = 0;
140	int bit = 1 << 30;
141
142	/* "bit" starts at the highest power of four <= the argument. */
143	while (bit > num)
144		bit >>= 2;
145
146	while (bit != 0) {
147		if (num >= res + bit) {
148			num -= res + bit;
149			res = (res >> 1) + bit;
150		} else {
151			res >>= 1;
152		}
153		bit >>= 2;
154	}
155	return (res);
156}
157
158void
159gfx_fb_setpixel(uint32_t x, uint32_t y)
160{
161	uint32_t c, offset;
162
163	if (fb.fd < 0)
164		return;
165	c = 0;		/* black */
166
167	if (x >= fb.fb_width || y >= fb.fb_height)
168		return;
169
170	offset = y * fb.fb_pitch + x * fb.fb_bpp;
171	switch (fb.fb_depth) {
172	case 8:
173		fb.fb_addr[offset] = c & 0xff;
174		break;
175	case 15:
176	case 16:
177		*(uint16_t *)(fb.fb_addr + offset) = c & 0xffff;
178		break;
179	case 24:
180		fb.fb_addr[offset] = (c >> 16) & 0xff;
181		fb.fb_addr[offset + 1] = (c >> 8) & 0xff;
182		fb.fb_addr[offset + 2] = c & 0xff;
183		break;
184	case 32:
185		*(uint32_t *)(fb.fb_addr + offset) = c;
186		break;
187	}
188}
189
190void
191gfx_fb_drawrect(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2,
192    uint32_t fill)
193{
194	int x, y;
195
196	if (fb.fd < 0)
197		return;
198
199	for (y = y1; y <= y2; y++) {
200		if (fill || (y == y1) || (y == y2)) {
201			for (x = x1; x <= x2; x++)
202				gfx_fb_setpixel(x, y);
203		} else {
204			gfx_fb_setpixel(x1, y);
205			gfx_fb_setpixel(x2, y);
206		}
207	}
208}
209
210void
211gfx_term_drawrect(uint32_t row1, uint32_t col1, uint32_t row2, uint32_t col2)
212{
213	int x1, y1, x2, y2;
214	int xshift, yshift;
215	int width, i;
216
217	if (fb.fd < 0)
218		return;
219
220	width = fb.font_width / 4;	/* line width */
221	xshift = (fb.font_width - width) / 2;
222	yshift = (fb.font_height - width) / 2;
223	/* Terminal coordinates start from (1,1) */
224	row1--;
225	col1--;
226	row2--;
227	col2--;
228
229	/*
230	 * Draw horizontal lines width points thick, shifted from outer edge.
231	 */
232	x1 = (row1 + 1) * fb.font_width + fb.terminal_origin_x;
233	y1 = col1 * fb.font_height + fb.terminal_origin_y + yshift;
234	x2 = row2 * fb.font_width + fb.terminal_origin_x;
235	gfx_fb_drawrect(x1, y1, x2, y1 + width, 1);
236	y2 = col2 * fb.font_height + fb.terminal_origin_y;
237	y2 += fb.font_height - yshift - width;
238	gfx_fb_drawrect(x1, y2, x2, y2 + width, 1);
239
240	/*
241	 * Draw vertical lines width points thick, shifted from outer edge.
242	 */
243	x1 = row1 * fb.font_width + fb.terminal_origin_x + xshift;
244	y1 = col1 * fb.font_height + fb.terminal_origin_y;
245	y1 += fb.font_height;
246	y2 = col2 * fb.font_height + fb.terminal_origin_y;
247	gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
248	x1 = row2 * fb.font_width + fb.terminal_origin_x;
249	x1 += fb.font_width - xshift - width;
250	gfx_fb_drawrect(x1, y1, x1 + width, y2, 1);
251
252	/* Draw upper left corner. */
253	x1 = row1 * fb.font_width + fb.terminal_origin_x + xshift;
254	y1 = col1 * fb.font_height + fb.terminal_origin_y;
255	y1 += fb.font_height;
256
257	x2 = row1 * fb.font_width + fb.terminal_origin_x;
258	x2 += fb.font_width;
259	y2 = col1 * fb.font_height + fb.terminal_origin_y + yshift;
260	for (i = 0; i <= width; i++)
261		gfx_fb_bezier(x1 + i, y1, x1 + i, y2 + i, x2, y2 + i, width-i);
262
263	/* Draw lower left corner. */
264	x1 = row1 * fb.font_width + fb.terminal_origin_x;
265	x1 += fb.font_width;
266	y1 = col2 * fb.font_height + fb.terminal_origin_y;
267	y1 += fb.font_height - yshift;
268	x2 = row1 * fb.font_width + fb.terminal_origin_x + xshift;
269	y2 = col2 * fb.font_height + fb.terminal_origin_y;
270	for (i = 0; i <= width; i++)
271		gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
272
273	/* Draw upper right corner. */
274	x1 = row2 * fb.font_width + fb.terminal_origin_x;
275	y1 = col1 * fb.font_height + fb.terminal_origin_y + yshift;
276	x2 = row2 * fb.font_width + fb.terminal_origin_x;
277	x2 += fb.font_width - xshift - width;
278	y2 = col1 * fb.font_height + fb.terminal_origin_y;
279	y2 += fb.font_height;
280	for (i = 0; i <= width; i++)
281		gfx_fb_bezier(x1, y1 + i, x2 + i, y1 + i, x2 + i, y2, width-i);
282
283	/* Draw lower right corner. */
284	x1 = row2 * fb.font_width + fb.terminal_origin_x;
285	y1 = col2 * fb.font_height + fb.terminal_origin_y;
286	y1 += fb.font_height - yshift;
287	x2 = row2 * fb.font_width + fb.terminal_origin_x;
288	x2 += fb.font_width - xshift - width;
289	y2 = col2 * fb.font_height + fb.terminal_origin_y;
290	for (i = 0; i <= width; i++)
291		gfx_fb_bezier(x1, y1 - i, x2 + i, y1 - i, x2 + i, y2, width-i);
292}
293
294void
295gfx_fb_line(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t width)
296{
297	int dx, sx, dy, sy;
298	int err, e2, x2, y2, ed;
299
300	if (fb.fd < 0)
301		return;
302
303	sx = x0 < x1? 1 : -1;
304	sy = y0 < y1? 1 : -1;
305	dx = abs(x1 - x0);
306	dy = abs(y1 - y0);
307	err = dx - dy;
308	ed = dx + dy == 0 ? 1 : isqrt(dx * dx + dy * dy);
309
310	if (dx != 0 && dy != 0)
311		width = (width + 1) >> 1;
312
313	for (;;) {
314		gfx_fb_setpixel(x0, y0);
315		e2 = err;
316		x2 = x0;
317		if ((e2 << 1) >= -dx) {		/* x step */
318			e2 += dy;
319			y2 = y0;
320			while (e2 < ed * width && (y1 != y2 || dx > dy)) {
321				y2 += sy;
322				gfx_fb_setpixel(x0, y2);
323				e2 += dx;
324			}
325			if (x0 == x1)
326				break;
327			e2 = err;
328			err -= dy;
329			x0 += sx;
330		}
331		if ((e2 << 1) <= dy) {		/* y step */
332			e2 = dx-e2;
333			while (e2 < ed * width && (x1 != x2 || dx < dy)) {
334				x2 += sx;
335				gfx_fb_setpixel(x2, y0);
336				e2 += dy;
337			}
338			if (y0 == y1)
339				break;
340			err += dx;
341			y0 += sy;
342		}
343	}
344}
345
346void
347gfx_fb_bezier(uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint32_t x2,
348    uint32_t y2, uint32_t wd)
349{
350	int sx, sy, xx, yy, xy, width;
351	int dx, dy, err, curvature;
352	int i;
353
354	if (fb.fd < 0)
355		return;
356
357	width = wd;
358	sx = x2 - x1;
359	sy = y2 - y1;
360	xx = x0 - x1;
361	yy = y0 - y1;
362	curvature = xx*sy - yy*sx;
363
364	if (sx * sx + sy * sy > xx * xx + yy * yy) {
365		x2 = x0;
366		x0 = sx + x1;
367		y2 = y0;
368		y0 = sy + y1;
369		curvature = -curvature;
370	}
371	if (curvature != 0) {
372		xx += sx;
373		sx = x0 < x2? 1 : -1;
374		xx *= sx;
375		yy += sy;
376		sy = y0 < y2? 1 : -1;
377		yy *= sy;
378		xy = 2 * xx * yy;
379		xx *= xx;
380		yy *= yy;
381		if (curvature * sx * sy < 0) {
382			xx = -xx;
383			yy = -yy;
384			xy = -xy;
385			curvature = -curvature;
386		}
387		dx = 4 * sy * curvature * (x1 - x0) + xx - xy;
388		dy = 4 * sx * curvature * (y0 - y1) + yy - xy;
389		xx += xx;
390		yy += yy;
391		err = dx + dy + xy;
392		do {
393			for (i = 0; i <= width; i++)
394				gfx_fb_setpixel(x0 + i, y0);
395			if (x0 == x2 && y0 == y2)
396				return;  /* last pixel -> curve finished */
397			y1 = 2 * err < dx;
398			if (2 * err > dy) {
399				x0 += sx;
400				dx -= xy;
401				dy += yy;
402				err += dy;
403			}
404			if (y1 != 0) {
405				y0 += sy;
406				dy -= xy;
407				dx += xx;
408				err += dx;
409			}
410		} while (dy < dx); /* gradient negates -> algorithm fails */
411	}
412	gfx_fb_line(x0, y0, x2, y2, width);
413}
414
415#define	FL_PUTIMAGE_BORDER	0x1
416#define	FL_PUTIMAGE_NOSCROLL	0x2
417#define	FL_PUTIMAGE_DEBUG	0x80
418
419int
420gfx_fb_putimage(png_t *png, uint32_t ux1, uint32_t uy1, uint32_t ux2,
421    uint32_t uy2, uint32_t flags)
422{
423	uint32_t i, j, x, y, fheight, fwidth, color;
424	uint8_t r, g, b, a, *p, *data;
425	bool scale = false;
426	bool trace = false;
427
428	trace = (flags & FL_PUTIMAGE_DEBUG) != 0;
429
430	if (fb.fd < 0) {
431		if (trace)
432			printf("Framebuffer not active.\n");
433		return (1);
434	}
435
436	if (png->color_type != PNG_TRUECOLOR_ALPHA) {
437		if (trace)
438			printf("Not truecolor image.\n");
439		return (1);
440	}
441
442	if (ux1 > fb.fb_width || uy1 > fb.fb_height) {
443		if (trace)
444			printf("Top left coordinate off screen.\n");
445		return (1);
446	}
447
448	if (png->width > UINT16_MAX || png->height > UINT16_MAX) {
449		if (trace)
450			printf("Image too large.\n");
451		return (1);
452	}
453
454	if (png->width < 1 || png->height < 1) {
455		if (trace)
456			printf("Image too small.\n");
457		return (1);
458	}
459
460	/*
461	 * If 0 was passed for either ux2 or uy2, then calculate the missing
462	 * part of the bottom right coordinate.
463	 */
464	scale = true;
465	if (ux2 == 0 && uy2 == 0) {
466		/* Both 0, use the native resolution of the image */
467		ux2 = ux1 + png->width;
468		uy2 = uy1 + png->height;
469		scale = false;
470	} else if (ux2 == 0) {
471		/* Set ux2 from uy2/uy1 to maintain aspect ratio */
472		ux2 = ux1 + (png->width * (uy2 - uy1)) / png->height;
473	} else if (uy2 == 0) {
474		/* Set uy2 from ux2/ux1 to maintain aspect ratio */
475		uy2 = uy1 + (png->height * (ux2 - ux1)) / png->width;
476	}
477
478	if (ux2 > fb.fb_width || uy2 > fb.fb_height) {
479		if (trace)
480			printf("Bottom right coordinate off screen.\n");
481		return (1);
482	}
483
484	fwidth = ux2 - ux1;
485	fheight = uy2 - uy1;
486
487	/*
488	 * If the original image dimensions have been passed explicitly,
489	 * disable scaling.
490	 */
491	if (fwidth == png->width && fheight == png->height)
492		scale = false;
493
494	if (ux1 == 0) {
495		/*
496		 * No top left X co-ordinate (real coordinates start at 1),
497		 * place as far right as it will fit.
498		 */
499		ux2 = fb.fb_width - fb.terminal_origin_x;
500		ux1 = ux2 - fwidth;
501	}
502
503	if (uy1 == 0) {
504		/*
505		 * No top left Y co-ordinate (real coordinates start at 1),
506		 * place as far down as it will fit.
507		 */
508		uy2 = fb.fb_height - fb.terminal_origin_y;
509		uy1 = uy2 - fheight;
510	}
511
512	if (ux1 >= ux2 || uy1 >= uy2) {
513		if (trace)
514			printf("Image dimensions reversed.\n");
515		return (1);
516	}
517
518	if (fwidth < 2 || fheight < 2) {
519		if (trace)
520			printf("Target area too small\n");
521		return (1);
522	}
523
524	if (trace)
525		printf("Image %ux%u -> %ux%u @%ux%u\n",
526		    png->width, png->height, fwidth, fheight, ux1, uy1);
527
528	if ((flags & FL_PUTIMAGE_BORDER))
529		gfx_fb_drawrect(ux1, uy1, ux2, uy2, 0);
530
531	data = malloc(fwidth * fheight * fb.fb_bpp);
532	if (data == NULL) {
533		if (trace)
534			printf("Out of memory.\n");
535		return (1);
536	}
537
538	/*
539	 * Build image for our framebuffer.
540	 */
541
542	/* Helper to calculate the pixel index from the source png */
543#define	GETPIXEL(xx, yy) (((yy) * png->width + (xx)) * png->bpp)
544
545	/*
546	 * For each of the x and y directions, calculate the number of pixels
547	 * in the source image that correspond to a single pixel in the target.
548	 * Use fixed-point arithmetic with 16-bits for each of the integer and
549	 * fractional parts.
550	 */
551	const uint32_t wcstep = ((png->width - 1) << 16) / (fwidth - 1);
552	const uint32_t hcstep = ((png->height - 1) << 16) / (fheight - 1);
553
554	uint32_t hc = 0;
555	for (y = 0; y < fheight; y++) {
556		uint32_t hc2 = (hc >> 9) & 0x7f;
557		uint32_t hc1 = 0x80 - hc2;
558
559		uint32_t offset_y = hc >> 16;
560		uint32_t offset_y1 = offset_y + 1;
561
562		uint32_t wc = 0;
563		for (x = 0; x < fwidth; x++) {
564			uint32_t wc2 = (wc >> 9) & 0x7f;
565			uint32_t wc1 = 0x80 - wc2;
566
567			uint32_t offset_x = wc >> 16;
568			uint32_t offset_x1 = offset_x + 1;
569
570			/* Target pixel index */
571			j = (y * fwidth + x) * fb.fb_bpp;
572
573			if (!scale) {
574				i = GETPIXEL(x, y);
575				r = png->image[i];
576				g = png->image[i + 1];
577				b = png->image[i + 2];
578				a = png->image[i + 3];
579			} else {
580				uint8_t pixel[4];
581
582				uint32_t p00 = GETPIXEL(offset_x, offset_y);
583				uint32_t p01 = GETPIXEL(offset_x, offset_y1);
584				uint32_t p10 = GETPIXEL(offset_x1, offset_y);
585				uint32_t p11 = GETPIXEL(offset_x1, offset_y1);
586
587				/*
588				 * Given a 2x2 array of pixels in the source
589				 * image, combine them to produce a single
590				 * value for the pixel in the target image.
591				 * Each column of pixels is combined using
592				 * a weighted average where the top and bottom
593				 * pixels contribute hc1 and hc2 respectively.
594				 * The calculation for bottom pixel pB and
595				 * top pixel pT is:
596				 *   (pT * hc1 + pB * hc2) / (hc1 + hc2)
597				 * Once the values are determined for the two
598				 * columns of pixels, then the columns are
599				 * averaged together in the same way but using
600				 * wc1 and wc2 for the weightings.
601				 *
602				 * Since hc1 and hc2 are chosen so that
603				 * hc1 + hc2 == 128 (and same for wc1 + wc2),
604				 * the >> 14 below is a quick way to divide by
605				 * (hc1 + hc2) * (wc1 + wc2)
606				 */
607				for (i = 0; i < 4; i++)
608					pixel[i] = (
609					    (png->image[p00 + i] * hc1 +
610					    png->image[p01 + i] * hc2) * wc1 +
611					    (png->image[p10 + i] * hc1 +
612					    png->image[p11 + i] * hc2) * wc2)
613					    >> 14;
614
615				r = pixel[0];
616				g = pixel[1];
617				b = pixel[2];
618				a = pixel[3];
619			}
620
621			color =
622			    r >> (8 - fb.red_mask_size)
623			    << fb.red_field_position |
624			    g >> (8 - fb.green_mask_size)
625			    << fb.green_field_position |
626			    b >> (8 - fb.blue_mask_size)
627			    << fb.blue_field_position;
628
629			switch (fb.fb_depth) {
630			case 8: {
631				uint32_t best, dist, k;
632				int diff;
633
634				color = 0;
635				best = 256 * 256 * 256;
636				for (k = 0; k < 16; k++) {
637					diff = r - cmap4_to_24.red[k];
638					dist = diff * diff;
639					diff = g - cmap4_to_24.green[k];
640					dist += diff * diff;
641					diff = b - cmap4_to_24.blue[k];
642					dist += diff * diff;
643
644					if (dist < best) {
645						color = k;
646						best = dist;
647						if (dist == 0)
648							break;
649					}
650				}
651				data[j] = solaris_color_to_pc_color[color];
652				break;
653			}
654			case 15:
655			case 16:
656				*(uint16_t *)(data+j) = color;
657				break;
658			case 24:
659				p = (uint8_t *)&color;
660				data[j] = p[0];
661				data[j+1] = p[1];
662				data[j+2] = p[2];
663				break;
664			case 32:
665				color |= a << 24;
666				*(uint32_t *)(data+j) = color;
667				break;
668			}
669			wc += wcstep;
670		}
671		hc += hcstep;
672	}
673
674	gfx_fb_cons_display(uy1, ux1, fwidth, fheight, data);
675	free(data);
676	return (0);
677}
678
679/*
680 * Implements alpha blending for RGBA data, could use pixels for arguments,
681 * but byte stream seems more generic.
682 * The generic alpha blending is:
683 * blend = alpha * fg + (1.0 - alpha) * bg.
684 * Since our alpha is not from range [0..1], we scale appropriately.
685 */
686static uint8_t
687alpha_blend(uint8_t fg, uint8_t bg, uint8_t alpha)
688{
689	uint16_t blend, h, l;
690
691	/* trivial corner cases */
692	if (alpha == 0)
693		return (bg);
694	if (alpha == 0xFF)
695		return (fg);
696	blend = (alpha * fg + (0xFF - alpha) * bg);
697	/* Division by 0xFF */
698	h = blend >> 8;
699	l = blend & 0xFF;
700	if (h + l >= 0xFF)
701		h++;
702	return (h);
703}
704
705/* Copy memory to framebuffer or to memory. */
706static void
707bitmap_cpy(uint8_t *dst, uint8_t *src, uint32_t len, int bpp)
708{
709	uint32_t i;
710	uint8_t a;
711
712	switch (bpp) {
713	case 4:
714		for (i = 0; i < len; i += bpp) {
715			a = src[i+3];
716			dst[i] = alpha_blend(src[i], dst[i], a);
717			dst[i+1] = alpha_blend(src[i+1], dst[i+1], a);
718			dst[i+2] = alpha_blend(src[i+2], dst[i+2], a);
719			dst[i+3] = a;
720		}
721		break;
722	default:
723		(void) memcpy(dst, src, len);
724		break;
725	}
726}
727
728/*
729 * gfx_fb_cons_display implements direct draw on frame buffer memory.
730 * It is needed till we have way to send bitmaps to tem, tem already has
731 * function to send data down to framebuffer.
732 */
733static void
734gfx_fb_cons_display(uint32_t row, uint32_t col,
735    uint32_t width, uint32_t height, uint8_t *data)
736{
737	uint32_t size;		/* write size per scanline */
738	uint8_t *fbp;		/* fb + calculated offset */
739	int i;
740
741	/* make sure we will not write past FB */
742	if (col >= fb.fb_width || row >= fb.fb_height ||
743	    col + width > fb.fb_width || row + height > fb.fb_height)
744		return;
745
746	size = width * fb.fb_bpp;
747	fbp = fb.fb_addr + col * fb.fb_bpp + row * fb.fb_pitch;
748
749	/* write all scanlines in rectangle */
750	for (i = 0; i < height; i++) {
751		uint8_t *dest = fbp + i * fb.fb_pitch;
752		uint8_t *src = data + i * size;
753		bitmap_cpy(dest, src, size, fb.fb_bpp);
754	}
755}
756