efi: Fix the screen setup for Graphics Output Protocol

Earlier version of this routine was erroneously handling locating the
protocol.  The check for error conditions have been fixed. The
handling of more than one GOP handle has been fixed as well.

Added copyright info to the file cp865_8x16.h.

Fixed vesacon_set_mode() to use malloc() instead of lmalloc() and
cleaned up debug traces.

Fixed __vesacon_copy_to_screen() by directly writing to the frame
buffer.

Signed-off-by: Chandramouli Narayanan <chandramouli.narayanan@intel.com>
Signed-off-by: Matt Fleming <matt.fleming@intel.com>

5 files changed, 175 insertions, 131 deletions

diff --git a/com32/lib/sys/vesa/efi/cp865_8x16.h b/com32/lib/sys/vesa/efi/cp865_8x16.h
index 9c8a2678..358a5638 100644
--- a/com32/lib/sys/vesa/efi/cp865_8x16.h
+++ b/com32/lib/sys/vesa/efi/cp865_8x16.h
@@ -1,3 +1,33 @@
+/* ----------------------------------------------------------------------- *
+ *
+ *   Copyright 1999-2012 H. Peter Anvin - All Rights Reserved
+ *   Chandramouli Narayanan - extended for EFI support
+ *
+ *   Permission is hereby granted, free of charge, to any person
+ *   obtaining a copy of this software and associated documentation
+ *   files (the "Software"), to deal in the Software without
+ *   restriction, including without limitation the rights to use,
+ *   copy, modify, merge, publish, distribute, sublicense, and/or
+ *   sell copies of the Software, and to permit persons to whom
+ *   the Software is furnished to do so, subject to the following
+ *   conditions:
+ *
+ *   The above copyright notice and this permission notice shall
+ *   be included in all copies or substantial portions of the Software.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ *   OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *   NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ *   HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ *   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ *   OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * ----------------------------------------------------------------------- */
+#ifndef CP865_8x16
+#define CP865_8x16
+
 static const short cp865_8x16_font_magic = 0x436;
 static const unsigned cp865_8x16_font_mode = 0x0;
 static const int cp865_8x16_font_height = 0x10;
@@ -260,3 +290,4 @@ static const uint8_t cp865_8x16_font_data[] = {
 	0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
 };
+#endif /* CP865_8x16 */
diff --git a/com32/lib/sys/vesa/efi/initvesa.c b/com32/lib/sys/vesa/efi/initvesa.c
index 9bb1f28d..44278d5e 100644
--- a/com32/lib/sys/vesa/efi/initvesa.c
+++ b/com32/lib/sys/vesa/efi/initvesa.c
@@ -355,7 +355,6 @@ static int vesacon_set_mode(int x, int y)
     int err = 0;
 
     //debug("Hello, World!\r\n");
-    printf("Hello, world, entering EFI graphics mode set operation (x=%d, y=%d)\n", x,y);//debug
     /* At this point, we assume that gnu-efi library is initialized */
     st = LibLocateProtocol(&GraphicsOutputProtocolGuid, (VOID **) &GraphicsOutput);
     if (EFI_ERROR(st)) {
@@ -366,24 +365,15 @@ static int vesacon_set_mode(int x, int y)
     /* We use the VESA info structure to store relevant GOP info as much as possible */
     gop_mode = GraphicsOutput->Mode;
 
+    dprintf("mode %d version %d pixlfmt %d hres=%d vres=%d\n", mode_num, 
+			mode_info->Version, mode_info->PixelFormat,
+			mode_info->HorizontalResolution, mode_info->VerticalResolution);
+    
     /* simply pick the best mode that suits the caller's resolution */
     for (mode_num = 0; mode_num < gop_mode->MaxMode; mode_num++) {
 	st = uefi_call_wrapper(GraphicsOutput->QueryMode, 4, GraphicsOutput, mode_num, &sz_info, &mode_info);
 	debug("mode_num = %d query_status %d\n", mode_num, st);
 	if (st == EFI_SUCCESS && sz_info >= sizeof(EFI_GRAPHICS_OUTPUT_MODE_INFORMATION)) {
-		/*
-		Print(L"mode %d ver %d (Hres=%d, Vres=%d) PixlFmt %d ",
-				mode_num,
-				info->Version, info->HorizontalResolution,
-				info->VerticalResolution,
-				info->PixelFormat);
-		if (info->PixelFormat == PixelBitMask)
-			Print(L"PixelBitMask RMask 0x%x GMask 0x%x BMask 0x%x ", 
-				info->PixelInformation.RedMask,
-				info->PixelInformation.GreenMask,
-				info->PixelInformation.BlueMask);
-		Print(L"ScanPerLine %d\n", info->PixelsPerScanLine);
-		*/
 
 		/* For now, simply pick the best mode that suits caller's resolution (x,y)
 		 * FIXME: Consider any additional criteria for matching mode
@@ -403,7 +393,7 @@ static int vesacon_set_mode(int x, int y)
     }
 
     /* Allocate space in the bounce buffer for these structures */
-    vi = lzalloc(sizeof *vi);
+    vi = malloc(sizeof(*vi));
     if (!vi) {
 	err = 10;		/* Out of memory */
 	goto exit;
@@ -423,6 +413,7 @@ static int vesacon_set_mode(int x, int y)
 
     switch (mode_info->PixelFormat) {
     case PixelRedGreenBlueReserved8BitPerColor:
+	dprintf("RGB8bit ");
 	mi->mode_attr = 0x0080;		/* supports physical frame buffer */
 	mi->bpp = sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL) * 8;
 	mi->rpos = 0;
@@ -432,8 +423,11 @@ static int vesacon_set_mode(int x, int y)
 	mi->resv_size = 8;
 	mi->logical_scan = mi->lfb_line_size = (mode_info->PixelsPerScanLine * mi->bpp) / 8;
 	bestpxf = PXF_BGRA32;
+	dprintf("bpp %d pixperScanLine %d logical_scan %d bytesperPix %d\n", mi->bpp, mode_info->PixelsPerScanLine, 
+		mi->logical_scan, (mi->bpp + 7)>>3);
 	break;
     case PixelBlueGreenRedReserved8BitPerColor:
+	dprintf("BGR8bit ");
 	mi->mode_attr = 0x0080;		/* supports physical frame buffer */
 	mi->bpp = sizeof(EFI_GRAPHICS_OUTPUT_BLT_PIXEL) * 8;
 	mi->bpos = 0;
@@ -443,9 +437,16 @@ static int vesacon_set_mode(int x, int y)
 	mi->resv_size = 8;
 	mi->logical_scan = mi->lfb_line_size = (mode_info->PixelsPerScanLine * mi->bpp) / 8;
 	bestpxf = PXF_BGRA32;
+	dprintf("bpp %d pixperScanLine %d logical_scan %d bytesperPix %d\n", mi->bpp, mode_info->PixelsPerScanLine, 
+		mi->logical_scan, (mi->bpp + 7)>>3);
 	break;
     case PixelBitMask:
 	mi->mode_attr = 0x0080;		/* supports physical frame buffer */
+	dprintf("RedMask 0x%x GrnMask 0x%x BluMask 0x%x RsvMask 0x%x\n",
+		mode_info->PixelInformation.RedMask,
+		mode_info->PixelInformation.GreenMask,
+		mode_info->PixelInformation.BlueMask,
+		mode_info->PixelInformation.ReservedMask);
 	find_pixmask_bits(mode_info->PixelInformation.RedMask,
                           &mi->rpos, &mi->lfb_rsize);
 	find_pixmask_bits(mode_info->PixelInformation.GreenMask,
@@ -453,11 +454,28 @@ static int vesacon_set_mode(int x, int y)
 	find_pixmask_bits(mode_info->PixelInformation.BlueMask,
                           &mi->bpos, &mi->lfb_bsize);
 	find_pixmask_bits(mode_info->PixelInformation.ReservedMask,
-                          &mi->resv_pos, &mi->resv_size);
+                          &mi->resv_pos, &mi->lfb_resv_size);
 	mi->bpp = mi->lfb_rsize + mi->lfb_gsize +
                                   mi->lfb_bsize + mi->lfb_resv_size;
 	mi->logical_scan = mi->lfb_line_size = (mode_info->PixelsPerScanLine * mi->bpp) / 8;
-	bestpxf = PXF_BGRA32; /* FIXME: correct? */
+	dprintf("RPos %d Rsize %d GPos %d Gsize %d\n", mi->rpos, mi->lfb_rsize, mi->gpos, mi->lfb_gsize);
+	dprintf("BPos %d Bsize %d RsvP %d RsvSz %d\n", mi->bpos, mi->lfb_bsize, mi->resv_pos, mi->lfb_resv_size);
+	dprintf("bpp %d logical_scan %d bytesperPix %d\n", mi->bpp, mi->logical_scan, (mi->bpp + 7)>>3);
+	switch (mi->bpp) {
+	case 32:
+		bestpxf = PXF_BGRA32;
+		break;
+	case 24:
+		bestpxf = PXF_BGR24;
+		break;
+	case 16:
+		bestpxf = PXF_LE_RGB16_565;
+		break;
+	default:
+		dprintf("Unable to handle bits per pixel %d, bailing out\n", mi->bpp);
+		err = 4;
+		goto exit;
+	}
 	break;
     case PixelBltOnly:
 	/* FIXME: unsupported */
@@ -501,6 +519,7 @@ static int vesacon_set_mode(int x, int y)
     st = uefi_call_wrapper(GraphicsOutput->SetMode, 2, GraphicsOutput, bestmode);
     if (EFI_ERROR(st)) {
 	err = 9;		/* Failed to set mode */
+	dprintf("Failed to set mode %d\n", bestmode);
 	goto exit;
     }	
 
@@ -540,7 +559,7 @@ static int vesacon_set_mode(int x, int y)
 
 exit:
     if (vi)
-	lfree(vi);
+	free(vi);
 
     return err;
 }
@@ -587,11 +606,9 @@ int __vesacon_init(int x, int y)
     int rv;
 
     /* We need the FPU for graphics, at least libpng et al will need it... */
-printf("vesacon_init: enter\n");
     if (x86_init_fpu())
 	return 10;
 
-printf("vesacon_init: set mode\n");
     rv = vesacon_set_mode(x, y);
     /* FIXME: Accessing Video BIOS from EFI will probably not work */
 #ifndef SYSLINUX_EFI
diff --git a/com32/lib/sys/vesa/efi/screencpy.c b/com32/lib/sys/vesa/efi/screencpy.c
index 345cdf44..aa650cc3 100644
--- a/com32/lib/sys/vesa/efi/screencpy.c
+++ b/com32/lib/sys/vesa/efi/screencpy.c
@@ -151,19 +151,10 @@ void __vesacon_copy_to_screen(size_t dst, const uint32_t * src, size_t npixels)
 
     s = (const char *)__vesacon_format_pixels(rowbuf, src, npixels);
 
-    while (bytes) {
-	win_off = dst & omask;
-	win_pos = dst & ~omask;
-
-	if (__unlikely(win_pos != wi.win_pos))
-	    set_window_pos(win_pos);
-
-	l = min(bytes, win_size - win_off);
-	memcpy(win_base + win_off, s, l);
-
-	bytes -= l;
-	s += l;
-	dst += l;
-    }
+    /* For EFI, we simply take the offset from the framebuffer and write to it
+     * FIXME: any gotchas?
+     */
+    win_off = dst;
+    memcpy(win_base + win_off, s, bytes);
 }
 #endif /* SYSLINUX_EFI */
diff --git a/efi/console.c b/efi/console.c
index 1e475b17..09081467 100644
--- a/efi/console.c
+++ b/efi/console.c
@@ -46,19 +46,26 @@ void setup_screen(struct screen_info *si)
 {
 	EFI_HANDLE *handles = NULL;
 	EFI_STATUS status;
+	EFI_GRAPHICS_OUTPUT_PROTOCOL *gop, *found;
+	EFI_GRAPHICS_PIXEL_FORMAT pixel_fmt;
+	EFI_PIXEL_BITMASK pixel_info;
+	uint32_t pixel_scanline;
 	UINTN nr_handles;
 	UINTN size;
 	uint16_t lfb_width, lfb_height;
 	uint32_t lfb_base, lfb_size;
 	int i;
-
+	void **gop_handle = NULL;
+
+	size = 0;
+	status = uefi_call_wrapper(BS->LocateHandle, 5, ByProtocol, &GraphicsOutputProtocol,
+				NULL, &size, gop_handle);
+	/* LibLocateHandle handle already returns the number of handles.
+	 * There is no need to divide by sizeof(EFI_HANDLE)
+	 */
 	status = LibLocateHandle(ByProtocol, &GraphicsOutputProtocol,
 				 NULL, &nr_handles, &handles);
 	if (status == EFI_BUFFER_TOO_SMALL) {
-		EFI_GRAPHICS_OUTPUT_PROTOCOL *gop, *found;
-		EFI_GRAPHICS_PIXEL_FORMAT pixel_fmt;
-		EFI_PIXEL_BITMASK pixel_info;
-		uint32_t pixel_scanline;
 
 		handles = AllocatePool(nr_handles);
 		if (!handles)
@@ -66,34 +73,22 @@ void setup_screen(struct screen_info *si)
 
 		status = LibLocateHandle(ByProtocol, &GraphicsOutputProtocol,
 					 NULL, &nr_handles, &handles);
-		if (status != EFI_SUCCESS)
-			goto out;
-
-		found = NULL;
-		for (i = 0; i < (nr_handles / sizeof(EFI_HANDLE)); i++) {
-			EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *info;
-			EFI_PCI_IO *pciio = NULL;
-			EFI_HANDLE *h = handles[i];
-
-			status = open_protocol(h, &GraphicsOutputProtocol,
-					       (void **)&gop,
-					       image_handle, NULL,
-					       EFI_OPEN_PROTOCOL_GET_PROTOCOL);
-			if (status != EFI_SUCCESS)
-				continue;
-
-			status = open_protocol(h, &PciIoProtocol,
-					       (void **)&pciio,
-					       image_handle, NULL,
-					       EFI_OPEN_PROTOCOL_GET_PROTOCOL);
-			status = gop_query_mode(gop, &size, &info);
-			if (status != EFI_SUCCESS)
-				continue;
-
-			if (!pciio && found)
-				continue;
-			found = gop;
+	}
+	if (status != EFI_SUCCESS)
+		goto out;
+
+	found = NULL;
+	for (i = 0; i < nr_handles; i++) {
+		EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *info;
+		EFI_PCI_IO *pciio = NULL;
+		EFI_HANDLE *h = handles[i];
 
+		status = uefi_call_wrapper(BS->HandleProtocol, 3, h, &GraphicsOutputProtocol, &gop);
+		if (status != EFI_SUCCESS)
+			continue;
+		uefi_call_wrapper(BS->HandleProtocol, 3, h, &PciIoProtocol, &pciio);
+		status = gop_query_mode(gop, &size, &info);
+		if (status == EFI_SUCCESS && (!found || pciio)) {
 			lfb_width = info->HorizontalResolution;
 			lfb_height = info->VerticalResolution;
 			lfb_base = gop->Mode->FrameBufferBase;
@@ -101,75 +96,84 @@ void setup_screen(struct screen_info *si)
 			pixel_fmt = info->PixelFormat;
 			pixel_info = info->PixelInformation;
 			pixel_scanline = info->PixelsPerScanLine;
-
 			if (pciio)
 				break;
-		}
-
-		if (!found)
-			goto out;
-
-		si->orig_video_isVGA = 0x70; /* EFI framebuffer */
-
-		si->lfb_base = lfb_base;
-		si->lfb_size = lfb_size;
-		si->lfb_width = lfb_width;
-		si->lfb_height = lfb_height;
-		si->pages = 1;
-
-		switch (pixel_fmt) {
-		case PixelRedGreenBlueReserved8BitPerColor:
-			si->lfb_depth = 32;
-			si->lfb_linelength = pixel_scanline * 4;
-			si->red_size = 8;
-			si->red_pos = 0;
-			si->green_size = 8;
-			si->green_pos = 8;
-			si->blue_size = 8;
-			si->blue_pos = 16;
-			si->rsvd_size = 8;
-			si->rsvd_pos = 24;
-			break;
-		case PixelBlueGreenRedReserved8BitPerColor:
-			si->lfb_depth = 32;
-			si->lfb_linelength = pixel_scanline * 4;
-			si->red_size = 8;
-			si->red_pos = 16;
-			si->green_size = 8;
-			si->green_pos = 8;
-			si->blue_size = 8;
-			si->blue_pos = 0;
-			si->rsvd_size = 8;
-			si->rsvd_pos = 24;
-			break;
-		case PixelBitMask:
-			bit_mask(pixel_info.RedMask, &si->red_pos,
-				 &si->red_size);
-			bit_mask(pixel_info.GreenMask, &si->green_pos,
-				 &si->green_size);
-			bit_mask(pixel_info.BlueMask, &si->blue_pos,
-				 &si->blue_size);
-			bit_mask(pixel_info.ReservedMask, &si->rsvd_pos,
-				 &si->rsvd_size);
-			si->lfb_depth = si->red_size + si->green_size +
-				si->blue_size + si->rsvd_size;
-			si->lfb_linelength = (pixel_scanline * si->lfb_depth) / 8;
-			break;
-		default:
-			si->lfb_depth = 4;;
-			si->lfb_linelength = si->lfb_width / 2;
-			si->red_size = 0;
-			si->red_pos = 0;
-			si->green_size = 0;
-			si->green_pos = 0;
-			si->blue_size = 0;
-			si->blue_pos = 0;
-			si->rsvd_size = 0;
-			si->rsvd_pos = 0;
-			break;
+			found = gop;
 		}
 	}
 
+	if (!found)
+		goto out;
+
+	dprintf("setup_screen: set up screen parameters for EFI GOP\n");
+	si->orig_video_isVGA = 0x70; /* EFI framebuffer */
+
+	si->lfb_base = lfb_base;
+	si->lfb_size = lfb_size;
+	si->lfb_width = lfb_width;
+	si->lfb_height = lfb_height;
+	si->pages = 1;
+
+	dprintf("setup_screen: lfb_base 0x%x lfb_size %d lfb_width %d lfb_height %d\n", lfb_base, lfb_size, lfb_width, lfb_height);
+	switch (pixel_fmt) {
+	case PixelRedGreenBlueReserved8BitPerColor:
+		si->lfb_depth = 32;
+		si->lfb_linelength = pixel_scanline * 4;
+		si->red_size = 8;
+		si->red_pos = 0;
+		si->green_size = 8;
+		si->green_pos = 8;
+		si->blue_size = 8;
+		si->blue_pos = 16;
+		si->rsvd_size = 8;
+		si->rsvd_pos = 24;
+		break;
+	case PixelBlueGreenRedReserved8BitPerColor:
+		si->lfb_depth = 32;
+		si->lfb_linelength = pixel_scanline * 4;
+		si->red_size = 8;
+		si->red_pos = 16;
+		si->green_size = 8;
+		si->green_pos = 8;
+		si->blue_size = 8;
+		si->blue_pos = 0;
+		si->rsvd_size = 8;
+		si->rsvd_pos = 24;
+		break;
+	case PixelBitMask:
+		bit_mask(pixel_info.RedMask, &si->red_pos,
+			 &si->red_size);
+		bit_mask(pixel_info.GreenMask, &si->green_pos,
+			 &si->green_size);
+		bit_mask(pixel_info.BlueMask, &si->blue_pos,
+			 &si->blue_size);
+		bit_mask(pixel_info.ReservedMask, &si->rsvd_pos,
+			 &si->rsvd_size);
+		si->lfb_depth = si->red_size + si->green_size +
+			si->blue_size + si->rsvd_size;
+		si->lfb_linelength = (pixel_scanline * si->lfb_depth) / 8;
+		break;
+	default:
+		si->lfb_depth = 4;;
+		si->lfb_linelength = si->lfb_width / 2;
+		si->red_size = 0;
+		si->red_pos = 0;
+		si->green_size = 0;
+		si->green_pos = 0;
+		si->blue_size = 0;
+		si->blue_pos = 0;
+		si->rsvd_size = 0;
+		si->rsvd_pos = 0;
+		break;
+	}
+	dprintf("setup_screen: depth %d line %d rpos %d rsize %d gpos %d gsize %d bpos %d bsize %d rsvpos %d rsvsize %d\n",
+		si->lfb_depth, si->lfb_linelength,
+		si->red_pos, si->red_size,
+		si->green_pos, si->green_size,
+		si->blue_pos, si->blue_size,
+		si->blue_pos, si->blue_size,
+		si->rsvd_pos, si->rsvd_size);
+	
 out:
-	FreePool(handles);
+	if (handles) FreePool(handles);
 }
diff --git a/efi/efi.h b/efi/efi.h
index ed3fe8a3..8ffe5332 100644
--- a/efi/efi.h
+++ b/efi/efi.h
@@ -9,5 +9,6 @@
 #include <efistdarg.h>
 
 extern EFI_HANDLE image_handle;
+void setup_screen(struct screen_info *); /* fix build error */
 
 #endif /* _SYSLINUX_EFI_H */