+
+static uint32_t calc_score(int x, int y, int bpp)
+{
+ uint32_t a = y >> 5;
+ uint32_t b = x >> 5;
+
+ if(a > 0xfff) a = 0xfff;
+ if(b > 0xfff) b = 0xfff;
+
+ return (a << 20) | (b << 8) | (bpp & 0xff);
+}
+
+int find_best_video_mode(void)
+{
+ int i, nmodes, w, h;
+ int score, best_score = 0, best_mode = -1;
+ unsigned long misize;
+ EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *mi;
+
+ nmodes = gop->Mode->MaxMode;
+ for(i=0; i<nmodes; i++) {
+ uefi_call_wrapper(gop->QueryMode, 4, gop, i, &misize, &mi);
+ w = mi->HorizontalResolution;
+ h = mi->VerticalResolution;
+
+ score = calc_score(w, h, mode_bpp(mi));
+ if(score > best_score) {
+ best_score = score;
+ best_mode = i;
+ }
+ }
+ return best_mode;
+}
+
+int find_video_mode(int xres, int yres, int bpp)
+{
+ int i, nmodes, w, h;
+ unsigned long misize;
+ EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *mi;
+
+ nmodes = gop->Mode->MaxMode;
+ for(i=0; i<nmodes; i++) {
+ uefi_call_wrapper(gop->QueryMode, 4, gop, i, &misize, &mi);
+ w = mi->HorizontalResolution;
+ h = mi->VerticalResolution;
+ if(w == xres && h == yres && mode_bpp(mi) >= bpp) {
+ return i;
+ }
+ }
+ return -1;
+}
+
+int set_video_mode(int mode)
+{
+ if(uefi_call_wrapper(gop->SetMode, 2, gop, mode) != 0) {
+ return -1;
+ }
+ return 0;
+}
+
+int cur_video_mode(void)
+{
+ return gop->Mode->Mode;
+}
+
+void draw_box(int x, int y, int w, int h, int r, int g, int b)
+{
+ int i, j;
+ uint32_t pixcol;
+ uint32_t *pptr = fb + y * scanwidth + x;
+
+ pixcol = (((uint32_t)r << rshift) & rmask) |
+ (((uint32_t)g << gshift) & gmask) |
+ (((uint32_t)b << bshift) & bmask);
+
+ for(i=0; i<h; i++) {
+ for(j=0; j<w; j++) {
+ *pptr++ = pixcol;
+ }
+ pptr += scanwidth - w;
+ }
+}
+
+int mask_to_shift(unsigned int mask)
+{
+ int res = 0;
+ while((mask & 1) == 0) {
+ mask >>= 1;
+ res++;
+ }
+ return res;
+}
+
+int mask_to_bpp(unsigned int mask)
+{
+ int res = 0;
+
+ while(mask) {
+ mask >>= 1;
+ res++;
+ }
+ return res;
+}
+
+int mode_bpp(EFI_GRAPHICS_OUTPUT_MODE_INFORMATION *minf)
+{
+ switch(minf->PixelFormat) {
+ case PixelRedGreenBlueReserved8BitPerColor:
+ case PixelBlueGreenRedReserved8BitPerColor:
+ return 24;
+
+ case PixelBitMask:
+ return mask_to_bpp(minf->PixelInformation.RedMask | minf->PixelInformation.GreenMask |
+ minf->PixelInformation.BlueMask);
+
+ default:
+ break;
+ }
+ return 0;
+}