Add support for paged (banked) VESA video mode

Add support for paged ("banked", non-linear-framebuffer) VESA video
modes.  Apparently some manufacturers haven't caught on that
non-linear graphics modes have been obsolete for 15 years or so
already.

6 files changed, 162 insertions, 14 deletions

diff --git a/com32/lib/Makefile b/com32/lib/Makefile
index 6540f9fe..cd0cebd9 100644
--- a/com32/lib/Makefile
+++ b/com32/lib/Makefile
@@ -43,7 +43,7 @@ LIBOBJS = \
 	\
 	sys/vesacon_write.o sys/vesaserial_write.o			\
 	sys/vesa/initvesa.o sys/vesa/drawtxt.o	sys/vesa/background.o	\
-	sys/vesa/alphatbl.o						\
+	sys/vesa/alphatbl.o sys/vesa/screencpy.o			\
 	\
 	pci/cfgtype.o pci/scan.o					\
 	pci/readb.o pci/readw.o pci/readl.o pci/readbios.o		\
diff --git a/com32/lib/sys/vesa/background.c b/com32/lib/sys/vesa/background.c
index ad537175..d7a0d019 100644
--- a/com32/lib/sys/vesa/background.c
+++ b/com32/lib/sys/vesa/background.c
@@ -63,7 +63,7 @@ static void draw_background_line(int line, int start, int npixels)
   while (npixels--)
     lbp = format_pixel(lbp, *bgptr++, pixel_format);
 
-  memcpy(fbptr, line_buf, lbp-line_buf);
+  __vesacon_copy_to_screen(fbptr, line_buf, lbp-line_buf);
 }
 
 /* This draws the border, then redraws the text area */
diff --git a/com32/lib/sys/vesa/drawtxt.c b/com32/lib/sys/vesa/drawtxt.c
index 5760c759..7a2347cc 100644
--- a/com32/lib/sys/vesa/drawtxt.c
+++ b/com32/lib/sys/vesa/drawtxt.c
@@ -195,7 +195,8 @@ static void vesacon_update_characters(int row, int col, int nrows, int ncols)
     /* Copy to frame buffer */
     /* Note that the dword_count is rounded down, not up.  That's because
        the row_buffer includes a spillover pixel. */
-    copy_dword(fbrowptr, row_buffer, (rowbufptr-row_buffer) >> 2);
+    __vesacon_copy_to_screen(fbrowptr, row_buffer,
+			     (rowbufptr-row_buffer) & ~3);
 
     bgrowptr += VIDEO_X_SIZE;
     fbrowptr += __vesa_info.mi.logical_scan;
diff --git a/com32/lib/sys/vesa/initvesa.c b/com32/lib/sys/vesa/initvesa.c
index fa6236fe..d8fb85d3 100644
--- a/com32/lib/sys/vesa/initvesa.c
+++ b/com32/lib/sys/vesa/initvesa.c
@@ -89,6 +89,27 @@ static void unpack_font(uint8_t *dst, uint8_t *src, int height)
   }
 }
 
+static int __constfunc is_power_of_2(unsigned int x)
+{
+  return x && !(x & (x-1));
+}
+
+static int vesacon_paged_mode_ok(const struct vesa_mode_info *mi)
+{
+  int i;
+
+  for (i = 0; i < 2; i++) {
+    if ((mi->win_attr[i] & 0x05) == 0x05 &&
+	mi->win_seg[i] &&
+	is_power_of_2(mi->win_size) &&
+	is_power_of_2(mi->win_grain) &&
+	mi->win_grain <= mi->win_size)
+      return 1;			/* We can deal with this... */
+  }
+
+  return 0;			/* Nope... */
+}
+
 static int vesacon_set_mode(void)
 {
   com32sys_t rm;
@@ -117,13 +138,8 @@ static int vesacon_set_mode(void)
     return 1;			/* Function call failed */
   if ( gi->signature != VESA_MAGIC )
     return 2;			/* No magic */
-#if 1
-  /* Linear frame buffer is a VBE 2.0 feature.  In theory this
-     test is redundant given that we check the bitmasks. */
-  if ( gi->version < 0x0200 ) {
-    return 3;			/* VESA 2.0 not supported */
-  }
-#endif
+  if ( gi->version < 0x0102 )
+    return 3;			/* VESA 1.2+ required */
 
   /* Copy general info */
   memcpy(&__vesa_info.gi, gi, sizeof *gi);
@@ -134,6 +150,8 @@ static int vesacon_set_mode(void)
   bestpxf  = PXF_NONE;
 
   while ((mode = *mode_ptr++) != 0xFFFF) {
+    mode &= 0x1FF;		/* The rest are attributes of sorts */
+
     debug("Found mode: 0x%04x\r\n", mode);
 
     memset(mi, 0, sizeof *mi);
@@ -154,21 +172,29 @@ static int vesacon_set_mode(void)
     /* Must be an LFB color graphics mode supported by the hardware.
 
       The bits tested are:
-       7 - linear frame buffer available
        4 - graphics mode
        3 - color mode
        1 - mode information available (mandatory in VBE 1.2+)
        0 - mode supported by hardware
     */
-    if ( (mi->mode_attr & 0x009b) != 0x009b )
+    if ( (mi->mode_attr & 0x001b) != 0x001b )
       continue;
 
     /* Must be 640x480 */
     if ( mi->h_res != VIDEO_X_SIZE || mi->v_res != VIDEO_Y_SIZE )
       continue;
 
+    /* We don't support multibank (interlaced memory) modes */
+    if ( mi->banks > 1 )
+      continue;
+
+    /* Must be either a flat-framebuffer mode, or be an acceptable
+       paged mode */
+    if ( !(mi->mode_attr & 0x0080) && !vesacon_paged_mode_ok(mi) )
+      continue;
+
     /* Must either be a packed-pixel mode or a direct color mode
-       (depending on VESA version ) */
+       (depending on VESA version ); must be a supported pixel format */
     pxf = PXF_NONE;		/* Not usable */
 
     if (mi->bpp == 32 &&
@@ -226,11 +252,15 @@ static int vesacon_set_mode(void)
 
   /* Now set video mode */
   rm.eax.w[0] = 0x4F02;		/* Set SVGA video mode */
-  rm.ebx.w[0] = mode | 0x4000;	/* Clear video RAM, use linear fb */
+  if (mi->mode_attr & 0x0080)
+    mode |= 0x4000;		/* Request linear framebuffer if supported */
+  rm.ebx.w[0] = mode;
   __intcall(0x10, &rm, &rm);
   if ( rm.eax.w[0] != 0x004F )
     return 9;			/* Failed to set mode */
 
+  __vesacon_init_copy_to_screen();
+
   /* Tell syslinux we changed video mode */
   rm.eax.w[0] = 0x0017;		/* Report video mode change */
   /* In theory this should be:
diff --git a/com32/lib/sys/vesa/screencpy.c b/com32/lib/sys/vesa/screencpy.c
new file mode 100644
index 00000000..a8be4306
--- /dev/null
+++ b/com32/lib/sys/vesa/screencpy.c
@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------- *
+ *   
+ *   Copyright 2008 H. Peter Anvin - All Rights Reserved
+ *
+ *   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.
+ *
+ * ----------------------------------------------------------------------- */
+
+#include <inttypes.h>
+#include <minmax.h>
+#include <klibc/compiler.h>
+#include <string.h>
+#include <com32.h>
+#include "vesa.h"
+#include "video.h"
+
+static struct win_info {
+  char *win_base;
+  size_t win_pos;
+  size_t win_size;
+  int win_gshift;
+  int win_num;
+} wi;
+
+static void *
+vesacon_copy_to_paged_screen(void *dst, const void *src, size_t len);
+
+static inline int __constfunc ilog2(unsigned int x)
+{
+  asm("bsrl %1,%0" : "=r" (x) : "rm" (x));
+  return x;
+}
+
+void __vesacon_init_copy_to_screen(void)
+{
+  struct vesa_mode_info * const mi = &__vesa_info.mi;
+  int winn;
+
+  if (mi->mode_attr & 0x0080) {
+    __vesacon_copy_to_screen = memcpy; /* Really easy... */
+  } else {
+    __vesacon_copy_to_screen = vesacon_copy_to_paged_screen;
+
+    mi->lfb_ptr = 0;		/* Zero-base this */
+    wi.win_pos = -1;		/* Undefined position */
+    
+    /* We have already tested that *one* of these is usable */
+    if ((mi->win_attr[0] & 0x05) == 0x05 && mi->win_seg[0])
+      winn = 0;
+    else
+      winn = 1;
+    
+    wi.win_num    = winn;
+    wi.win_base   = (char *)(mi->win_seg[winn] << 4);
+    wi.win_size   = mi->win_size << 10;
+    wi.win_gshift = ilog2(mi->win_grain) + 10;
+  }
+}
+
+static void *
+vesacon_copy_to_paged_screen(void *dst, const void *src, size_t len)
+{
+  size_t win_pos, win_off;
+  size_t win_size = wi.win_size;
+  size_t omask = win_size - 1;
+  char *win_base = wi.win_base;
+  size_t l;
+  size_t d = (size_t)dst;
+  const char *s = src;
+  com32sys_t ireg;
+
+  memset(&ireg, 0, sizeof ireg);
+  ireg.eax.w[0] = 0x4F05;	/* VBE Window Control */
+  /* BH = 0 -> Set memory window */
+  ireg.ebx.b[0] = wi.win_num;
+
+  while (len) {
+    win_off = d & omask;
+    win_pos = d & ~omask;
+
+    if (win_pos != wi.win_pos) {
+      ireg.edx.w[0] = win_pos >> wi.win_gshift;
+      __intcall(0x10, &ireg, NULL);
+      wi.win_pos = win_pos;
+    }
+
+    l = min(len, win_size - win_off);
+    memcpy(win_base + win_off, s, l);
+
+    len -= l;
+    s += l;
+    d += l;
+  }
+
+  return dst;
+}
diff --git a/com32/lib/sys/vesa/video.h b/com32/lib/sys/vesa/video.h
index b37642f6..f9b3e5da 100644
--- a/com32/lib/sys/vesa/video.h
+++ b/com32/lib/sys/vesa/video.h
@@ -80,5 +80,7 @@ void __vesacon_write_char(int, int, uint8_t, attr_t);
 void __vesacon_redraw_text(void);
 void __vesacon_doit(void);
 void __vesacon_set_cursor(int, int, int);
+void * (*__vesacon_copy_to_screen)(void *, const void *, size_t);
+void __vesacon_init_copy_to_screen(void);
 
 #endif /* LIB_SYS_VESA_VIDEO_H */