vgabios: Move save/restore state code from vgabios.c to stdvga.c.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
diff --git a/vgasrc/stdvga.c b/vgasrc/stdvga.c
index 1b552db..ae6c0be 100644
--- a/vgasrc/stdvga.c
+++ b/vgasrc/stdvga.c
@@ -111,25 +111,6 @@
  ****************************************************************/
 
 void
-stdvga_save_dac_state(u16 seg, struct saveDACcolors *info)
-{
-    /* XXX: check this */
-    SET_FARVAR(seg, info->rwmode, inb(VGAREG_DAC_STATE));
-    SET_FARVAR(seg, info->peladdr, inb(VGAREG_DAC_WRITE_ADDRESS));
-    SET_FARVAR(seg, info->pelmask, stdvga_pelmask_read());
-    stdvga_dac_read(seg, info->dac, 0, 256);
-    SET_FARVAR(seg, info->color_select, 0);
-}
-
-void
-stdvga_restore_dac_state(u16 seg, struct saveDACcolors *info)
-{
-    stdvga_pelmask_write(GET_FARVAR(seg, info->pelmask));
-    stdvga_dac_write(seg, info->dac, 0, 256);
-    outb(GET_FARVAR(seg, info->peladdr), VGAREG_DAC_WRITE_ADDRESS);
-}
-
-void
 stdvga_perform_gray_scale_summing(u16 start, u16 count)
 {
     stdvga_attrindex_write(0x00);
@@ -343,8 +324,23 @@
  * Save/Restore state
  ****************************************************************/
 
-void
-stdvga_save_state(u16 seg, struct saveVideoHardware *info)
+struct saveVideoHardware {
+    u8 sequ_index;
+    u8 crtc_index;
+    u8 grdc_index;
+    u8 actl_index;
+    u8 feature;
+    u8 sequ_regs[4];
+    u8 sequ0;
+    u8 crtc_regs[25];
+    u8 actl_regs[20];
+    u8 grdc_regs[9];
+    u16 crtc_addr;
+    u8 plane_latch[4];
+};
+
+static void
+stdvga_save_hw_state(u16 seg, struct saveVideoHardware *info)
 {
     u16 crtc_addr = stdvga_get_crtc();
     SET_FARVAR(seg, info->sequ_index, inb(VGAREG_SEQU_ADDRESS));
@@ -374,8 +370,8 @@
         SET_FARVAR(seg, info->plane_latch[i], 0);
 }
 
-void
-stdvga_restore_state(u16 seg, struct saveVideoHardware *info)
+static void
+stdvga_restore_hw_state(u16 seg, struct saveVideoHardware *info)
 {
     int i;
     for (i=0; i<4; i++)
@@ -409,6 +405,78 @@
     outb(GET_FARVAR(seg, info->feature), crtc_addr - 0x4 + 0xa);
 }
 
+struct saveDACcolors {
+    u8 rwmode;
+    u8 peladdr;
+    u8 pelmask;
+    u8 dac[768];
+    u8 color_select;
+};
+
+static void
+stdvga_save_dac_state(u16 seg, struct saveDACcolors *info)
+{
+    /* XXX: check this */
+    SET_FARVAR(seg, info->rwmode, inb(VGAREG_DAC_STATE));
+    SET_FARVAR(seg, info->peladdr, inb(VGAREG_DAC_WRITE_ADDRESS));
+    SET_FARVAR(seg, info->pelmask, stdvga_pelmask_read());
+    stdvga_dac_read(seg, info->dac, 0, 256);
+    SET_FARVAR(seg, info->color_select, 0);
+}
+
+static void
+stdvga_restore_dac_state(u16 seg, struct saveDACcolors *info)
+{
+    stdvga_pelmask_write(GET_FARVAR(seg, info->pelmask));
+    stdvga_dac_write(seg, info->dac, 0, 256);
+    outb(GET_FARVAR(seg, info->peladdr), VGAREG_DAC_WRITE_ADDRESS);
+}
+
+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);
+    }
+    if (states & 2) {
+        save_bda_state(seg, data);
+        data += sizeof(struct saveBDAstate);
+    }
+    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;
+}
+
 
 /****************************************************************
  * Misc
diff --git a/vgasrc/stdvga.h b/vgasrc/stdvga.h
index 000a097..c9a9ba1 100644
--- a/vgasrc/stdvga.h
+++ b/vgasrc/stdvga.h
@@ -44,29 +44,6 @@
 #define SEG_CTEXT 0xB800
 #define SEG_MTEXT 0xB000
 
-struct saveVideoHardware {
-    u8 sequ_index;
-    u8 crtc_index;
-    u8 grdc_index;
-    u8 actl_index;
-    u8 feature;
-    u8 sequ_regs[4];
-    u8 sequ0;
-    u8 crtc_regs[25];
-    u8 actl_regs[20];
-    u8 grdc_regs[9];
-    u16 crtc_addr;
-    u8 plane_latch[4];
-};
-
-struct saveDACcolors {
-    u8 rwmode;
-    u8 peladdr;
-    u8 pelmask;
-    u8 dac[768];
-    u8 color_select;
-};
-
 // stdvgamodes.c
 struct vgamode_s *stdvga_find_mode(int mode);
 void stdvga_list_modes(u16 seg, u16 *dest, u16 *last);
@@ -107,8 +84,6 @@
 void stdvga_toggle_intensity(u8 flag);
 void stdvga_select_video_dac_color_page(u8 flag, u8 data);
 void stdvga_read_video_dac_state(u8 *pmode, u8 *curpage);
-void stdvga_save_dac_state(u16 seg, struct saveDACcolors *info);
-void stdvga_restore_dac_state(u16 seg, struct saveDACcolors *info);
 void stdvga_perform_gray_scale_summing(u16 start, u16 count);
 void stdvga_set_text_block_specifier(u8 spec);
 void stdvga_planar4_plane(int plane);
@@ -128,8 +103,9 @@
 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);
-void stdvga_save_state(u16 seg, struct saveVideoHardware *info);
-void stdvga_restore_state(u16 seg, struct saveVideoHardware *info);
+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);
 void stdvga_enable_video_addressing(u8 disable);
 int stdvga_init(void);
 
diff --git a/vgasrc/vgabios.c b/vgasrc/vgabios.c
index ec251fe..ea78c45 100644
--- a/vgasrc/vgabios.c
+++ b/vgasrc/vgabios.c
@@ -304,7 +304,7 @@
  * Save and restore bda state
  ****************************************************************/
 
-static void
+void
 save_bda_state(u16 seg, struct saveBDAstate *info)
 {
     SET_FARVAR(seg, info->video_mode, GET_BDA(video_mode));
@@ -317,7 +317,7 @@
     SET_FARVAR(seg, info->video_switches, GET_BDA(video_switches));
     SET_FARVAR(seg, info->modeset_ctl, GET_BDA(modeset_ctl));
     SET_FARVAR(seg, info->cursor_type, GET_BDA(cursor_type));
-    u16 i;
+    int i;
     for (i=0; i<8; i++)
         SET_FARVAR(seg, info->cursor_pos[i], GET_BDA(cursor_pos[i]));
     SET_FARVAR(seg, info->video_pagestart, GET_BDA(video_pagestart));
@@ -327,7 +327,7 @@
     SET_FARVAR(seg, info->font1, GET_IVT(0x43));
 }
 
-static void
+void
 restore_bda_state(u16 seg, struct saveBDAstate *info)
 {
     u16 mode = GET_FARVAR(seg, info->video_mode);
@@ -342,7 +342,7 @@
     SET_BDA(video_switches, GET_FARVAR(seg, info->video_switches));
     SET_BDA(modeset_ctl, GET_FARVAR(seg, info->modeset_ctl));
     SET_BDA(cursor_type, GET_FARVAR(seg, info->cursor_type));
-    u16 i;
+    int i;
     for (i = 0; i < 8; i++)
         SET_BDA(cursor_pos[i], GET_FARVAR(seg, info->cursor_pos[i]));
     SET_BDA(video_pagestart, GET_FARVAR(seg, info->video_pagestart));
@@ -1130,73 +1130,37 @@
 
 
 static void
-handle_101c00(struct bregs *regs)
-{
-    u16 flags = regs->cx;
-    u16 size = 0;
-    if (flags & 1)
-        size += sizeof(struct saveVideoHardware);
-    if (flags & 2)
-        size += sizeof(struct saveBDAstate);
-    if (flags & 4)
-        size += sizeof(struct saveDACcolors);
-    regs->bx = size;
-    regs->al = 0x1c;
-}
-
-static void
-handle_101c01(struct bregs *regs)
-{
-    u16 flags = regs->cx;
-    u16 seg = regs->es;
-    void *data = (void*)(regs->bx+0);
-    if (flags & 1) {
-        stdvga_save_state(seg, data);
-        data += sizeof(struct saveVideoHardware);
-    }
-    if (flags & 2) {
-        save_bda_state(seg, data);
-        data += sizeof(struct saveBDAstate);
-    }
-    if (flags & 4)
-        stdvga_save_dac_state(seg, data);
-    regs->al = 0x1c;
-}
-
-static void
-handle_101c02(struct bregs *regs)
-{
-    u16 flags = regs->cx;
-    u16 seg = regs->es;
-    void *data = (void*)(regs->bx+0);
-    if (flags & 1) {
-        stdvga_restore_state(seg, data);
-        data += sizeof(struct saveVideoHardware);
-    }
-    if (flags & 2) {
-        restore_bda_state(seg, data);
-        data += sizeof(struct saveBDAstate);
-    }
-    if (flags & 4)
-        stdvga_restore_dac_state(seg, data);
-    regs->al = 0x1c;
-}
-
-static void
-handle_101cXX(struct bregs *regs)
-{
-    debug_stub(regs);
-}
-
-static void
 handle_101c(struct bregs *regs)
 {
+    u16 seg = regs->es;
+    void *data = (void*)(regs->bx+0);
+    u16 states = regs->cx;
+    if (states & ~0x07)
+        goto fail;
+    int ret;
     switch (regs->al) {
-    case 0x00: handle_101c00(regs); break;
-    case 0x01: handle_101c01(regs); break;
-    case 0x02: handle_101c02(regs); break;
-    default:   handle_101cXX(regs); break;
+    case 0x00:
+        ret = stdvga_size_state(states);
+        if (ret < 0)
+            goto fail;
+        regs->bx = ret / 64;
+        break;
+    case 0x01:
+        ret = stdvga_save_state(seg, data, states);
+        if (ret)
+            goto fail;
+        break;
+    case 0x02:
+        ret = stdvga_restore_state(seg, data, states);
+        if (ret)
+            goto fail;
+        break;
+    default:
+        goto fail;
     }
+    regs->al = 0x1c;
+fail:
+    return;
 }
 
 static void
diff --git a/vgasrc/vgabios.h b/vgasrc/vgabios.h
index 0aff684..76ecb88 100644
--- a/vgasrc/vgabios.h
+++ b/vgasrc/vgabios.h
@@ -85,6 +85,8 @@
 };
 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);
 struct vgamode_s *get_current_mode(void);
 int vga_set_mode(int mode, int flags);