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 0a36afc..957bde2 100644
--- a/vgasrc/bochsvga.c
+++ b/vgasrc/bochsvga.c
@@ -201,6 +201,71 @@
     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 78a9e0a..1c98203 100644
--- a/vgasrc/bochsvga.h
+++ b/vgasrc/bochsvga.h
@@ -62,6 +62,9 @@
 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 0470d80..7d1a604 100644
--- a/vgasrc/clext.c
+++ b/vgasrc/clext.c
@@ -368,6 +368,30 @@
     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 3d83507..78dba01 100644
--- a/vgasrc/clext.h
+++ b/vgasrc/clext.h
@@ -11,6 +11,9 @@
 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 0ee69b4..505cb61 100644
--- a/vgasrc/vbe.c
+++ b/vgasrc/vbe.c
@@ -201,8 +201,36 @@
 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 ea78c45..7b6c50a 100644
--- a/vgasrc/vgabios.c
+++ b/vgasrc/vgabios.c
@@ -307,7 +307,7 @@
 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 @@
 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 @@
     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 76ecb88..d32e1b0 100644
--- a/vgasrc/vgabios.h
+++ b/vgasrc/vgabios.h
@@ -20,7 +20,7 @@
 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 585ae3d..044cd32 100644
--- a/vgasrc/vgahw.h
+++ b/vgasrc/vgahw.h
@@ -105,4 +105,28 @@
     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