From 9961f9958cbc169c531dbdb7c3a8f71d4f79d0c1 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sat, 21 Jan 2012 11:53:44 -0500
Subject: [PATCH] vgabios: Add support for vesa get/set window function.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 vgasrc/bochsvga.c | 19 +++++++++++++++++++
 vgasrc/bochsvga.h |  2 ++
 vgasrc/clext.c    | 38 +++++++++++++++-----------------------
 vgasrc/clext.h    |  2 ++
 vgasrc/stdvga.c   | 12 ++++++++++++
 vgasrc/stdvga.h   |  2 ++
 vgasrc/vbe.c      | 30 +++++++++++++++++++++++++++---
 vgasrc/vgaentry.S |  5 +++++
 vgasrc/vgahw.h    | 17 +++++++++++++++++
 9 files changed, 101 insertions(+), 26 deletions(-)

diff --git a/vgasrc/bochsvga.c b/vgasrc/bochsvga.c
index 6cbbf9c..74da887 100644
--- a/vgasrc/bochsvga.c
+++ b/vgasrc/bochsvga.c
@@ -198,6 +198,25 @@ bochsvga_hires_enable(int enable)
     dispi_write(VBE_DISPI_INDEX_ENABLE, flags);
 }
 
+int
+bochsvga_get_window(struct vgamode_s *vmode_g, int window)
+{
+    if (window != 0)
+        return -1;
+    return dispi_read(VBE_DISPI_INDEX_BANK);
+}
+
+int
+bochsvga_set_window(struct vgamode_s *vmode_g, int window, int val)
+{
+    if (window != 0)
+        return -1;
+    dispi_write(VBE_DISPI_INDEX_BANK, val);
+    if (dispi_read(VBE_DISPI_INDEX_BANK) != val)
+        return -1;
+    return 0;
+}
+
 static void
 bochsvga_clear_scr(void)
 {
diff --git a/vgasrc/bochsvga.h b/vgasrc/bochsvga.h
index 1eaa91a..26a82e0 100644
--- a/vgasrc/bochsvga.h
+++ b/vgasrc/bochsvga.h
@@ -55,6 +55,8 @@ static inline void dispi_write(u16 reg, u16 val)
 int bochsvga_init(void);
 void bochsvga_list_modes(u16 seg, u16 *dest, u16 *last);
 struct vgamode_s *bochsvga_find_mode(int mode);
+int bochsvga_get_window(struct vgamode_s *vmode_g, int window);
+int bochsvga_set_window(struct vgamode_s *vmode_g, int window, int val);
 int bochsvga_set_mode(struct vgamode_s *vmode_g, int flags);
 
 #endif // bochsvga.h
diff --git a/vgasrc/clext.c b/vgasrc/clext.c
index 363ef5d..ee39c5e 100644
--- a/vgasrc/clext.c
+++ b/vgasrc/clext.c
@@ -400,6 +400,21 @@ cirrus_get_memsize(void)
     return 0x04 << x;
 }
 
+int
+clext_get_window(struct vgamode_s *vmode_g, int window)
+{
+    return stdvga_grdc_read(window + 9);
+}
+
+int
+clext_set_window(struct vgamode_s *vmode_g, int window, int val)
+{
+    if (val >= 0x100)
+        return -1;
+    stdvga_grdc_write(window + 9, val);
+    return 0;
+}
+
 static void
 cirrus_enable_16k_granularity(void)
 {
@@ -619,28 +634,6 @@ cirrus_get_start_addr(void)
             | ((b4 & 0x80) << 12));
 }
 
-static void
-cirrus_vesa_05h(struct bregs *regs)
-{
-    if (regs->bl > 1)
-        goto fail;
-    if (regs->bh == 0) {
-        // set mempage
-        if (regs->dx >= 0x100)
-            goto fail;
-        stdvga_grdc_write(regs->bl + 9, regs->dx);
-    } else if (regs->bh == 1) {
-        // get mempage
-        regs->dx = stdvga_grdc_read(regs->bl + 9);
-    } else
-        goto fail;
-
-    regs->ax = 0x004f;
-    return;
-fail:
-    regs->ax = 0x014f;
-}
-
 static void
 cirrus_vesa_06h(struct bregs *regs)
 {
@@ -714,7 +707,6 @@ void
 cirrus_vesa(struct bregs *regs)
 {
     switch (regs->al) {
-    case 0x05: cirrus_vesa_05h(regs); break;
     case 0x06: cirrus_vesa_06h(regs); break;
     case 0x07: cirrus_vesa_07h(regs); break;
     case 0x10: cirrus_vesa_10h(regs); break;
diff --git a/vgasrc/clext.h b/vgasrc/clext.h
index 718b756..b300cf4 100644
--- a/vgasrc/clext.h
+++ b/vgasrc/clext.h
@@ -4,6 +4,8 @@
 #include "types.h" // u16
 
 struct vgamode_s *clext_find_mode(int mode);
+int clext_get_window(struct vgamode_s *vmode_g, int window);
+int clext_set_window(struct vgamode_s *vmode_g, int window, int val);
 int clext_set_mode(struct vgamode_s *vmode_g, int flags);
 void clext_list_modes(u16 seg, u16 *dest, u16 *last);
 int clext_init(void);
diff --git a/vgasrc/stdvga.c b/vgasrc/stdvga.c
index b7def32..9c0cba9 100644
--- a/vgasrc/stdvga.c
+++ b/vgasrc/stdvga.c
@@ -270,6 +270,18 @@ stdvga_get_vde(void)
     return vde;
 }
 
+int
+stdvga_get_window(struct vgamode_s *vmode_g, int window)
+{
+    return -1;
+}
+
+int
+stdvga_set_window(struct vgamode_s *vmode_g, int window, int val)
+{
+    return -1;
+}
+
 
 /****************************************************************
  * Save/Restore/Set state
diff --git a/vgasrc/stdvga.h b/vgasrc/stdvga.h
index cc93f0d..05f2bf5 100644
--- a/vgasrc/stdvga.h
+++ b/vgasrc/stdvga.h
@@ -134,6 +134,8 @@ void stdvga_set_active_page(u16 address);
 void stdvga_set_cursor_pos(u16 address);
 void stdvga_set_scan_lines(u8 lines);
 u16 stdvga_get_vde(void);
+int stdvga_get_window(struct vgamode_s *vmode_g, int window);
+int stdvga_set_window(struct vgamode_s *vmode_g, int window, int val);
 void stdvga_save_state(u16 seg, struct saveVideoHardware *info);
 void stdvga_restore_state(u16 seg, struct saveVideoHardware *info);
 int stdvga_set_mode(struct vgamode_s *vmode_g, int flags);
diff --git a/vgasrc/vbe.c b/vgasrc/vbe.c
index b6ffb97..7e84794 100644
--- a/vgasrc/vbe.c
+++ b/vgasrc/vbe.c
@@ -102,7 +102,9 @@ vbe_104f01(struct bregs *regs)
     SET_FARVAR(seg, info->win_size, 64); /* Bank size 64K */
     SET_FARVAR(seg, info->winA_seg, GET_GLOBAL(vmode_g->sstart));
     SET_FARVAR(seg, info->winB_seg, 0x0);
-    SET_FARVAR(seg, info->win_func_ptr.segoff, 0x0);
+    extern void entry_104f05(void);
+    SET_FARVAR(seg, info->win_func_ptr
+               , SEGOFF(get_global_seg(), (u32)entry_104f05));
     int width = GET_GLOBAL(vmode_g->width);
     int height = GET_GLOBAL(vmode_g->height);
     int linesize = width * DIV_ROUND_UP(depth, 8);
@@ -207,10 +209,32 @@ vbe_104f04(struct bregs *regs)
     regs->ax = 0x0100;
 }
 
-static void
+void VISIBLE16
 vbe_104f05(struct bregs *regs)
 {
-    debug_stub(regs);
+    if (regs->bh > 1 || regs->bl > 1)
+        goto fail;
+    if (GET_BDA(vbe_mode) & MF_LINEARFB) {
+        regs->ah = VBE_RETURN_STATUS_INVALID;
+        return;
+    }
+    struct vgamode_s *vmode_g = get_current_mode();
+    if (! vmode_g)
+        goto fail;
+    if (regs->bh) {
+        int ret = vgahw_get_window(vmode_g, regs->bl);
+        if (ret < 0)
+            goto fail;
+        regs->dx = ret;
+        regs->ax = 0x004f;
+        return;
+    }
+    int ret = vgahw_set_window(vmode_g, regs->bl, regs->dx);
+    if (ret)
+        goto fail;
+    regs->ax = 0x004f;
+    return;
+fail:
     regs->ax = 0x0100;
 }
 
diff --git a/vgasrc/vgaentry.S b/vgasrc/vgaentry.S
index 112857b..785d91f 100644
--- a/vgasrc/vgaentry.S
+++ b/vgasrc/vgaentry.S
@@ -50,6 +50,11 @@ _rom_header_signature:
  * Entry points
  ****************************************************************/
 
+        DECLFUNC entry_104f05
+entry_104f05:
+        ENTRY_ARG vbe_104f05
+        lretw
+
         DECLFUNC _optionrom_entry
 _optionrom_entry:
         ENTRY_ARG vga_post
diff --git a/vgasrc/vgahw.h b/vgasrc/vgahw.h
index bb5112a..ca8798c 100644
--- a/vgasrc/vgahw.h
+++ b/vgasrc/vgahw.h
@@ -44,4 +44,21 @@ static inline int vgahw_init(void) {
     return stdvga_init();
 }
 
+static inline int vgahw_get_window(struct vgamode_s *vmode_g, int window) {
+    if (CONFIG_VGA_CIRRUS)
+        return clext_get_window(vmode_g, window);
+    if (CONFIG_VGA_BOCHS)
+        return bochsvga_get_window(vmode_g, window);
+    return stdvga_get_window(vmode_g, window);
+}
+
+static inline int vgahw_set_window(struct vgamode_s *vmode_g, int window
+                                   , int val) {
+    if (CONFIG_VGA_CIRRUS)
+        return clext_set_window(vmode_g, window, val);
+    if (CONFIG_VGA_BOCHS)
+        return bochsvga_set_window(vmode_g, window, val);
+    return stdvga_set_window(vmode_g, window, val);
+}
+
 #endif // vgahw.h
-- 
2.25.1