vgabios: Implement VBE save/restore state function (func 04).

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

diff --git a/vgasrc/bochsvga.c b/vgasrc/bochsvga.c
index 0a36afc5f3a11389807bf93b0f2a5d00ceff16d4..957bde2929615b9f443edc9934b8b114f8b04be4 100644 (file)
--- a/vgasrc/bochsvga.c
+++ b/vgasrc/bochsvga.c
@@ -201,6 +201,71 @@ bochsvga_set_dacformat(struct vgamode_s *vmode_g, int val)
     return 0;
 }
 
+int
+bochsvga_size_state(int states)
+{
+    int size = stdvga_size_state(states);
+    if (size < 0)
+        return size;
+    if (states & 8)
+        size += (VBE_DISPI_INDEX_Y_OFFSET-VBE_DISPI_INDEX_XRES+1)*sizeof(u16);
+    return size;
+}
+
+int
+bochsvga_save_state(u16 seg, void *data, int states)
+{
+    int ret = stdvga_save_state(seg, data, states);
+    if (ret < 0)
+        return ret;
+
+    if (!(states & 8))
+        return 0;
+
+    u16 *info = (data + stdvga_size_state(states));
+    u16 en = dispi_read(VBE_DISPI_INDEX_ENABLE);
+    SET_FARVAR(seg, *info, en);
+    info++;
+    if (!(en & VBE_DISPI_ENABLED))
+        return 0;
+    int i;
+    for (i = VBE_DISPI_INDEX_XRES; i <= VBE_DISPI_INDEX_Y_OFFSET; i++)
+        if (i != VBE_DISPI_INDEX_ENABLE) {
+            u16 v = dispi_read(i);
+            SET_FARVAR(seg, *info, v);
+            info++;
+        }
+    return 0;
+}
+
+int
+bochsvga_restore_state(u16 seg, void *data, int states)
+{
+    int ret = stdvga_restore_state(seg, data, states);
+    if (ret < 0)
+        return ret;
+
+    if (!(states & 8))
+        return 0;
+
+    u16 *info = (data + stdvga_size_state(states));
+    u16 en = GET_FARVAR(seg, *info);
+    info++;
+    if (!(en & VBE_DISPI_ENABLED)) {
+        dispi_write(VBE_DISPI_INDEX_ENABLE, en);
+        return 0;
+    }
+    int i;
+    for (i = VBE_DISPI_INDEX_XRES; i <= VBE_DISPI_INDEX_Y_OFFSET; i++)
+        if (i == VBE_DISPI_INDEX_ENABLE) {
+            dispi_write(i, en);
+        } else {
+            dispi_write(i, GET_FARVAR(seg, *info));
+            info++;
+        }
+    return 0;
+}
+
 
 /****************************************************************
  * Mode setting

diff --git a/vgasrc/bochsvga.h b/vgasrc/bochsvga.h
index 78a9e0a535ebae9cfc7c69df84923e6b053cfaee..1c982034b30f31044924ca9a4f40a67c8ecfb0f6 100644 (file)
--- a/vgasrc/bochsvga.h
+++ b/vgasrc/bochsvga.h
@@ -62,6 +62,9 @@ int bochsvga_get_displaystart(struct vgamode_s *vmode_g);
 int bochsvga_set_displaystart(struct vgamode_s *vmode_g, int val);
 int bochsvga_get_dacformat(struct vgamode_s *vmode_g);
 int bochsvga_set_dacformat(struct vgamode_s *vmode_g, int val);
+int bochsvga_size_state(int states);
+int bochsvga_save_state(u16 seg, void *data, int states);
+int bochsvga_restore_state(u16 seg, void *data, int states);
 int bochsvga_set_mode(struct vgamode_s *vmode_g, int flags);
 int bochsvga_init(void);
 

diff --git a/vgasrc/clext.c b/vgasrc/clext.c
index 0470d80a548329d3ad2df1a8b691309971efd7ad..7d1a604d45fc117acd132ff7460eb54a43414d8d 100644 (file)
--- a/vgasrc/clext.c
+++ b/vgasrc/clext.c
@@ -368,6 +368,30 @@ clext_set_displaystart(struct vgamode_s *vmode_g, int val)
     return 0;
 }
 
+int
+clext_size_state(int states)
+{
+    if (states & 8)
+        return -1;
+    return stdvga_size_state(states);
+}
+
+int
+clext_save_state(u16 seg, void *data, int states)
+{
+    if (states & 8)
+        return -1;
+    return stdvga_save_state(seg, data, states);
+}
+
+int
+clext_restore_state(u16 seg, void *data, int states)
+{
+    if (states & 8)
+        return -1;
+    return stdvga_restore_state(seg, data, states);
+}
+
 
 /****************************************************************
  * Mode setting

diff --git a/vgasrc/clext.h b/vgasrc/clext.h
index 3d8350787ff35f2e01a1226eb984a146b92b2e32..78dba01d7e2d77f123bb18c9d553b6b043756b43 100644 (file)
--- a/vgasrc/clext.h
+++ b/vgasrc/clext.h
@@ -11,6 +11,9 @@ int clext_get_linelength(struct vgamode_s *vmode_g);
 int clext_set_linelength(struct vgamode_s *vmode_g, int val);
 int clext_get_displaystart(struct vgamode_s *vmode_g);
 int clext_set_displaystart(struct vgamode_s *vmode_g, int val);
+int clext_size_state(int states);
+int clext_save_state(u16 seg, void *data, int states);
+int clext_restore_state(u16 seg, void *data, int states);
 int clext_set_mode(struct vgamode_s *vmode_g, int flags);
 struct bregs;
 void clext_1012(struct bregs *regs);

diff --git a/vgasrc/vbe.c b/vgasrc/vbe.c
index 0ee69b4ad8257df408ff47b38e3ca45307719b3f..505cb61cbea902c352b195ec2e9258e041c93ecc 100644 (file)
--- a/vgasrc/vbe.c
+++ b/vgasrc/vbe.c
@@ -201,8 +201,36 @@ vbe_104f03(struct bregs *regs)
 static void
 vbe_104f04(struct bregs *regs)
 {
-    debug_stub(regs);
-    regs->ax = 0x0100;
+    u16 seg = regs->es;
+    void *data = (void*)(regs->bx+0);
+    u16 states = regs->cx;
+    if (states & ~0x0f)
+        goto fail;
+    int ret;
+    switch (regs->dl) {
+    case 0x00:
+        ret = vgahw_size_state(states);
+        if (ret < 0)
+            goto fail;
+        regs->bx = ret / 64;
+        break;
+    case 0x01:
+        ret = vgahw_save_state(seg, data, states);
+        if (ret)
+            goto fail;
+        break;
+    case 0x02:
+        ret = vgahw_restore_state(seg, data, states);
+        if (ret)
+            goto fail;
+        break;
+    default:
+        goto fail;
+    }
+    regs->ax = 0x004f;
+    return;
+fail:
+    regs->ax = 0x014f;
 }
 
 void VISIBLE16

diff --git a/vgasrc/vgabios.c b/vgasrc/vgabios.c
index ea78c45caa7c296bc3c78858c1a83952ab7bc9bd..7b6c50af95051fbf513cffd4af2a2d34c5d31997 100644 (file)
--- a/vgasrc/vgabios.c
+++ b/vgasrc/vgabios.c
@@ -307,7 +307,7 @@ write_string(struct cursorpos *pcp, u8 attr, u16 count, u16 seg, u8 *offset_far)
 void
 save_bda_state(u16 seg, struct saveBDAstate *info)
 {
-    SET_FARVAR(seg, info->video_mode, GET_BDA(video_mode));
+    SET_FARVAR(seg, info->video_mode, GET_BDA(vbe_mode));
     SET_FARVAR(seg, info->video_cols, GET_BDA(video_cols));
     SET_FARVAR(seg, info->video_pagesize, GET_BDA(video_pagesize));
     SET_FARVAR(seg, info->crtc_address, GET_BDA(crtc_address));
@@ -331,8 +331,11 @@ void
 restore_bda_state(u16 seg, struct saveBDAstate *info)
 {
     u16 mode = GET_FARVAR(seg, info->video_mode);
-    SET_BDA(video_mode, mode);
     SET_BDA(vbe_mode, mode);
+    if (mode < 0x100)
+        SET_BDA(video_mode, mode);
+    else
+        SET_BDA(video_mode, 0xff);
     SET_BDA(video_cols, GET_FARVAR(seg, info->video_cols));
     SET_BDA(video_pagesize, GET_FARVAR(seg, info->video_pagesize));
     SET_BDA(crtc_address, GET_FARVAR(seg, info->crtc_address));
@@ -1140,18 +1143,18 @@ handle_101c(struct bregs *regs)
     int ret;
     switch (regs->al) {
     case 0x00:
-        ret = stdvga_size_state(states);
+        ret = vgahw_size_state(states);
         if (ret < 0)
             goto fail;
         regs->bx = ret / 64;
         break;
     case 0x01:
-        ret = stdvga_save_state(seg, data, states);
+        ret = vgahw_save_state(seg, data, states);
         if (ret)
             goto fail;
         break;
     case 0x02:
-        ret = stdvga_restore_state(seg, data, states);
+        ret = vgahw_restore_state(seg, data, states);
         if (ret)
             goto fail;
         break;

diff --git a/vgasrc/vgabios.h b/vgasrc/vgabios.h
index 76ecb88a95f8dc116cdfd9fc87873aa4ec014a7d..d32e1b04c6166a6350200c8efa90dc2647b245ae 100644 (file)
--- a/vgasrc/vgabios.h
+++ b/vgasrc/vgabios.h
@@ -20,7 +20,7 @@ struct VideoParam_s {
 extern struct VideoParam_s video_param_table[29];
 
 struct saveBDAstate {
-    u8 video_mode;
+    u16 video_mode;
     u16 video_cols;
     u16 video_pagesize;
     u16 crtc_address;

diff --git a/vgasrc/vgahw.h b/vgasrc/vgahw.h
index 585ae3dada6ace516d4f03637f3163eb09f3c81d..044cd32ca281184f834e0e5922c96e05bf98b665 100644 (file)
--- a/vgasrc/vgahw.h
+++ b/vgasrc/vgahw.h
@@ -105,4 +105,28 @@ static inline int vgahw_set_dacformat(struct vgamode_s *vmode_g, int val) {
     return stdvga_set_dacformat(vmode_g, val);
 }
 
+static inline int vgahw_size_state(int states) {
+    if (CONFIG_VGA_CIRRUS)
+        return clext_size_state(states);
+    if (CONFIG_VGA_BOCHS)
+        return bochsvga_size_state(states);
+    return stdvga_size_state(states);
+}
+
+static inline int vgahw_save_state(u16 seg, void *data, int states) {
+    if (CONFIG_VGA_CIRRUS)
+        return clext_save_state(seg, data, states);
+    if (CONFIG_VGA_BOCHS)
+        return bochsvga_save_state(seg, data, states);
+    return stdvga_save_state(seg, data, states);
+}
+
+static inline int vgahw_restore_state(u16 seg, void *data, int states) {
+    if (CONFIG_VGA_CIRRUS)
+        return clext_restore_state(seg, data, states);
+    if (CONFIG_VGA_BOCHS)
+        return bochsvga_restore_state(seg, data, states);
+    return stdvga_restore_state(seg, data, states);
+}
+
 #endif // vgahw.h