vgabios: Simplify save/restore mechanism.

Reorganize the save/restore functions to eliminate some boilerplate
code.

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

10 files changed, 97 insertions, 194 deletions

diff --git a/vgasrc/bochsvga.c b/vgasrc/bochsvga.c
index 6da9d5da..eae20ae1 100644
--- a/vgasrc/bochsvga.c
+++ b/vgasrc/bochsvga.c
@@ -245,30 +245,9 @@ 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 (GET_GLOBAL(dispi_found) && (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)
+static int
+bochsvga_save_state(u16 seg, u16 *info)
 {
-    int ret = stdvga_save_state(seg, data, states);
-    if (ret < 0)
-        return ret;
-
-    if (!GET_GLOBAL(dispi_found))
-        return 0;
-    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++;
@@ -284,19 +263,9 @@ bochsvga_save_state(u16 seg, void *data, int states)
     return 0;
 }
 
-int
-bochsvga_restore_state(u16 seg, void *data, int states)
+static int
+bochsvga_restore_state(u16 seg, u16 *info)
 {
-    int ret = stdvga_restore_state(seg, data, states);
-    if (ret < 0)
-        return ret;
-
-    if (!GET_GLOBAL(dispi_found))
-        return 0;
-    if (!(states & 8))
-        return 0;
-
-    u16 *info = (data + stdvga_size_state(states));
     u16 en = GET_FARVAR(seg, *info);
     info++;
     if (!(en & VBE_DISPI_ENABLED)) {
@@ -314,6 +283,21 @@ bochsvga_restore_state(u16 seg, void *data, int states)
     return 0;
 }
 
+int
+bochsvga_save_restore(int cmd, u16 seg, void *data)
+{
+    int ret = stdvga_save_restore(cmd, seg, data);
+    if (ret < 0 || !(cmd & SR_REGISTERS) || !GET_GLOBAL(dispi_found))
+        return ret;
+
+    u16 *info = (data + ret);
+    if (cmd & SR_SAVE)
+        bochsvga_save_state(seg, info);
+    if (cmd & SR_RESTORE)
+        bochsvga_restore_state(seg, info);
+    return ret + (VBE_DISPI_INDEX_Y_OFFSET-VBE_DISPI_INDEX_XRES+1)*sizeof(u16);
+}
+
 
 /****************************************************************
  * Mode setting
diff --git a/vgasrc/bochsvga.h b/vgasrc/bochsvga.h
index 78b27c80..ae5f75db 100644
--- a/vgasrc/bochsvga.h
+++ b/vgasrc/bochsvga.h
@@ -50,9 +50,7 @@ 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_save_restore(int cmd, u16 seg, void *data);
 int bochsvga_set_mode(struct vgamode_s *vmode_g, int flags);
 int bochsvga_setup(void);
 
diff --git a/vgasrc/clext.c b/vgasrc/clext.c
index f7751a2f..fc5b42f6 100644
--- a/vgasrc/clext.c
+++ b/vgasrc/clext.c
@@ -369,27 +369,11 @@ clext_set_displaystart(struct vgamode_s *vmode_g, int val)
 }
 
 int
-clext_size_state(int states)
+clext_save_restore(int cmd, u16 seg, void *data)
 {
-    if (states & 8)
+    if (cmd & SR_REGISTERS)
         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);
+    return stdvga_save_restore(cmd, seg, data);
 }
 
 
diff --git a/vgasrc/clext.h b/vgasrc/clext.h
index efc98b9c..cf47a5bf 100644
--- a/vgasrc/clext.h
+++ b/vgasrc/clext.h
@@ -11,9 +11,7 @@ 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_save_restore(int cmd, u16 seg, void *data);
 int clext_set_mode(struct vgamode_s *vmode_g, int flags);
 struct bregs;
 void clext_1012(struct bregs *regs);
diff --git a/vgasrc/stdvga.c b/vgasrc/stdvga.c
index c94ec060..70cceed1 100644
--- a/vgasrc/stdvga.c
+++ b/vgasrc/stdvga.c
@@ -434,48 +434,25 @@ stdvga_restore_dac_state(u16 seg, struct saveDACcolors *info)
 }
 
 int
-stdvga_size_state(int states)
-{
-    int size = 0;
-    if (states & 1)
-        size += sizeof(struct saveVideoHardware);
-    if (states & 2)
-        size += sizeof(struct saveBDAstate);
-    if (states & 4)
-        size += sizeof(struct saveDACcolors);
-    return size;
-}
-
-int
-stdvga_save_state(u16 seg, void *data, int states)
-{
-    if (states & 1) {
-        stdvga_save_hw_state(seg, data);
-        data += sizeof(struct saveVideoHardware);
+stdvga_save_restore(int cmd, u16 seg, void *data)
+{
+    void *pos = data;
+    if (cmd & SR_HARDWARE) {
+        if (cmd & SR_SAVE)
+            stdvga_save_hw_state(seg, pos);
+        if (cmd & SR_RESTORE)
+            stdvga_restore_hw_state(seg, pos);
+        pos += sizeof(struct saveVideoHardware);
     }
-    if (states & 2) {
-        save_bda_state(seg, data);
-        data += sizeof(struct saveBDAstate);
+    pos += bda_save_restore(cmd, seg, pos);
+    if (cmd & SR_DAC) {
+        if (cmd & SR_SAVE)
+            stdvga_save_dac_state(seg, pos);
+        if (cmd & SR_RESTORE)
+            stdvga_restore_dac_state(seg, pos);
+        pos += sizeof(struct saveDACcolors);
     }
-    if (states & 4)
-        stdvga_save_dac_state(seg, data);
-    return 0;
-}
-
-int
-stdvga_restore_state(u16 seg, void *data, int states)
-{
-    if (states & 1) {
-        stdvga_restore_hw_state(seg, data);
-        data += sizeof(struct saveVideoHardware);
-    }
-    if (states & 2) {
-        restore_bda_state(seg, data);
-        data += sizeof(struct saveBDAstate);
-    }
-    if (states & 4)
-        stdvga_restore_dac_state(seg, data);
-    return 0;
+    return pos - data;
 }
 
 
diff --git a/vgasrc/stdvga.h b/vgasrc/stdvga.h
index 3685abab..df09bab8 100644
--- a/vgasrc/stdvga.h
+++ b/vgasrc/stdvga.h
@@ -104,9 +104,7 @@ int stdvga_get_displaystart(struct vgamode_s *vmode_g);
 int stdvga_set_displaystart(struct vgamode_s *vmode_g, int val);
 int stdvga_get_dacformat(struct vgamode_s *vmode_g);
 int stdvga_set_dacformat(struct vgamode_s *vmode_g, int val);
-int stdvga_size_state(int states);
-int stdvga_save_state(u16 seg, void *data, int states);
-int stdvga_restore_state(u16 seg, void *data, int states);
+int stdvga_save_restore(int cmd, u16 seg, void *data);
 void stdvga_enable_video_addressing(u8 disable);
 int stdvga_setup(void);
 
diff --git a/vgasrc/vbe.c b/vgasrc/vbe.c
index f7e2203a..12bd9812 100644
--- a/vgasrc/vbe.c
+++ b/vgasrc/vbe.c
@@ -228,29 +228,14 @@ vbe_104f04(struct bregs *regs)
     u16 seg = regs->es;
     void *data = (void*)(regs->bx+0);
     u16 states = regs->cx;
-    if (states & ~0x0f)
+    u8 cmd = regs->dl;
+    if (states & ~0x0f || cmd > 2)
         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:
+    int ret = vgahw_save_restore(states | (cmd<<8), seg, data);
+    if (ret < 0)
         goto fail;
-    }
+    if (cmd == 0)
+        regs->bx = ret / 64;
     regs->ax = 0x004f;
     return;
 fail:
diff --git a/vgasrc/vgabios.c b/vgasrc/vgabios.c
index 47bfe2cb..b6d9dc23 100644
--- a/vgasrc/vgabios.c
+++ b/vgasrc/vgabios.c
@@ -236,28 +236,39 @@ write_teletype(struct cursorpos *pcp, struct carattr ca)
  * Save and restore bda state
  ****************************************************************/
 
-void
-save_bda_state(u16 seg, struct saveBDAstate *info)
-{
-    memcpy_far(seg, info->bda_0x49, SEG_BDA, (void*)0x49
-               , sizeof(info->bda_0x49));
-    memcpy_far(seg, info->bda_0x84, SEG_BDA, (void*)0x84
-               , sizeof(info->bda_0x84));
-    SET_FARVAR(seg, info->vbe_mode, GET_BDA(vbe_mode));
-    SET_FARVAR(seg, info->font0, GET_IVT(0x1f));
-    SET_FARVAR(seg, info->font1, GET_IVT(0x43));
-}
+struct saveBDAstate {
+    u8 bda_0x49[28];
+    u8 bda_0x84[6];
+    u16 vbe_mode;
+    struct segoff_s font0;
+    struct segoff_s font1;
+};
 
-void
-restore_bda_state(u16 seg, struct saveBDAstate *info)
+int
+bda_save_restore(int cmd, u16 seg, void *data)
 {
-    memcpy_far(SEG_BDA, (void*)0x49, seg, info->bda_0x49
-               , sizeof(info->bda_0x49));
-    memcpy_far(SEG_BDA, (void*)0x84, seg, info->bda_0x84
-               , sizeof(info->bda_0x84));
-    SET_BDA(vbe_mode, GET_FARVAR(seg, info->vbe_mode));
-    SET_IVT(0x1f, GET_FARVAR(seg, info->font0));
-    SET_IVT(0x43, GET_FARVAR(seg, info->font1));
+    if (!(cmd & SR_BDA))
+        return 0;
+    struct saveBDAstate *info = data;
+    if (cmd & SR_SAVE) {
+        memcpy_far(seg, info->bda_0x49, SEG_BDA, (void*)0x49
+                   , sizeof(info->bda_0x49));
+        memcpy_far(seg, info->bda_0x84, SEG_BDA, (void*)0x84
+                   , sizeof(info->bda_0x84));
+        SET_FARVAR(seg, info->vbe_mode, GET_BDA(vbe_mode));
+        SET_FARVAR(seg, info->font0, GET_IVT(0x1f));
+        SET_FARVAR(seg, info->font1, GET_IVT(0x43));
+    }
+    if (cmd & SR_RESTORE) {
+        memcpy_far(SEG_BDA, (void*)0x49, seg, info->bda_0x49
+                   , sizeof(info->bda_0x49));
+        memcpy_far(SEG_BDA, (void*)0x84, seg, info->bda_0x84
+                   , sizeof(info->bda_0x84));
+        SET_BDA(vbe_mode, GET_FARVAR(seg, info->vbe_mode));
+        SET_IVT(0x1f, GET_FARVAR(seg, info->font0));
+        SET_IVT(0x43, GET_FARVAR(seg, info->font1));
+    }
+    return sizeof(*info);
 }
 
 
@@ -1120,29 +1131,14 @@ handle_101c(struct bregs *regs)
     u16 seg = regs->es;
     void *data = (void*)(regs->bx+0);
     u16 states = regs->cx;
-    if (states & ~0x07)
+    u8 cmd = regs->al;
+    if (states & ~0x07 || cmd > 2)
         goto fail;
-    int ret;
-    switch (regs->al) {
-    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:
+    int ret = vgahw_save_restore(states | (cmd<<8), seg, data);
+    if (ret < 0)
         goto fail;
-    }
+    if (cmd == 0)
+        regs->bx = ret / 64;
     regs->al = 0x1c;
 fail:
     return;
diff --git a/vgasrc/vgabios.h b/vgasrc/vgabios.h
index f58e4988..69495600 100644
--- a/vgasrc/vgabios.h
+++ b/vgasrc/vgabios.h
@@ -19,13 +19,13 @@ struct VideoParam_s {
 
 extern struct VideoParam_s video_param_table[29];
 
-struct saveBDAstate {
-    u8 bda_0x49[28];
-    u8 bda_0x84[6];
-    u16 vbe_mode;
-    struct segoff_s font0;
-    struct segoff_s font1;
-};
+// Save/Restore flags
+#define SR_HARDWARE   0x0001
+#define SR_BDA        0x0002
+#define SR_DAC        0x0004
+#define SR_REGISTERS  0x0008
+#define SR_SAVE       0x0100
+#define SR_RESTORE    0x0200
 
 // Mode flags
 #define MF_LEGACY     0x0001
@@ -82,8 +82,7 @@ struct cursorpos {
 };
 int vga_bpp(struct vgamode_s *vmode_g);
 u16 calc_page_size(u8 memmodel, u16 width, u16 height);
-void save_bda_state(u16 seg, struct saveBDAstate *info);
-void restore_bda_state(u16 seg, struct saveBDAstate *info);
+int bda_save_restore(int cmd, u16 seg, void *data);
 struct vgamode_s *get_current_mode(void);
 int vga_set_mode(int mode, int flags);
 
diff --git a/vgasrc/vgahw.h b/vgasrc/vgahw.h
index f69a5ac5..3e843571 100644
--- a/vgasrc/vgahw.h
+++ b/vgasrc/vgahw.h
@@ -105,28 +105,12 @@ 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) {
+static inline int vgahw_save_restore(int cmd, u16 seg, void *data) {
     if (CONFIG_VGA_CIRRUS)
-        return clext_size_state(states);
+        return clext_save_restore(cmd, seg, data);
     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);
+        return bochsvga_save_restore(cmd, seg, data);
+    return stdvga_save_restore(cmd, seg, data);
 }
 
 #endif // vgahw.h