VGA: Define structs for save/restore state calls.

Define C structs for the state info.
Move hw specific code to vgahw.c.
Also, make sure to set 0x1c in regs->al on state save/restore calls.

3 files changed, 285 insertions, 289 deletions

diff --git a/vgasrc/vga.c b/vgasrc/vga.c
index 70e05b5..b1ba01b 100644
--- a/vgasrc/vga.c
+++ b/vgasrc/vga.c
@@ -7,7 +7,6 @@
 
 
 // TODO:
-//  * define structs for save/restore state
 //  * review correctness of converted asm by comparing with RBIL
 //  * refactor redundant code into sub-functions
 //  * See if there is a method to the in/out stuff that can be encapsulated.
@@ -640,288 +639,52 @@ biosfn_set_display_code(struct bregs *regs)
     regs->al = 0x1a;
 }
 
-// -------------------------------------------------------------------
 static void
-biosfn_read_state_info(u16 BX, u16 ES, u16 DI)
-{
-    // Address of static functionality table
-    SET_FARVAR(ES, *(u16*)(DI + 0x00), (u32)static_functionality);
-    SET_FARVAR(ES, *(u16*)(DI + 0x02), get_global_seg());
-
-    // Hard coded copy from BIOS area. Should it be cleaner ?
-    memcpy_far(ES, (void*)(DI + 0x04), SEG_BDA, (void*)0x49, 30);
-    memcpy_far(ES, (void*)(DI + 0x22), SEG_BDA, (void*)0x84, 3);
-
-    SET_FARVAR(ES, *(u8*)(DI + 0x25), GET_BDA(dcc_index));
-    SET_FARVAR(ES, *(u8*)(DI + 0x26), 0);
-    SET_FARVAR(ES, *(u8*)(DI + 0x27), 16);
-    SET_FARVAR(ES, *(u8*)(DI + 0x28), 0);
-    SET_FARVAR(ES, *(u8*)(DI + 0x29), 8);
-    SET_FARVAR(ES, *(u8*)(DI + 0x2a), 2);
-    SET_FARVAR(ES, *(u8*)(DI + 0x2b), 0);
-    SET_FARVAR(ES, *(u8*)(DI + 0x2c), 0);
-    SET_FARVAR(ES, *(u8*)(DI + 0x31), 3);
-    SET_FARVAR(ES, *(u8*)(DI + 0x32), 0);
-
-    memset_far(ES, (void*)(DI + 0x33), 0, 13);
-}
-
-// -------------------------------------------------------------------
-// -------------------------------------------------------------------
-static u16
-biosfn_read_video_state_size(u16 CX)
-{
-    u16 size = 0;
-    if (CX & 1)
-        size += 0x46;
-    if (CX & 2)
-        size += (5 + 8 + 5) * 2 + 6;
-    if (CX & 4)
-        size += 3 + 256 * 3 + 1;
-    return size;
-}
-
-static u16
-biosfn_save_video_state(u16 CX, u16 ES, u16 BX)
-{
-    u16 crtc_addr = GET_BDA(crtc_address);
-    if (CX & 1) {
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_ADDRESS));
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr));
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_ADDRESS));
-        BX++;
-        inb(VGAREG_ACTL_RESET);
-        u16 ar_index = inb(VGAREG_ACTL_ADDRESS);
-        SET_FARVAR(ES, *(u8*)(BX+0), ar_index);
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_READ_FEATURE_CTL));
-        BX++;
-
-        u16 i;
-        for (i = 1; i <= 4; i++) {
-            outb(i, VGAREG_SEQU_ADDRESS);
-            SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
-            BX++;
-        }
-        outb(0, VGAREG_SEQU_ADDRESS);
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
-        BX++;
-
-        for (i = 0; i <= 0x18; i++) {
-            outb(i, crtc_addr);
-            SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr + 1));
-            BX++;
-        }
-
-        for (i = 0; i <= 0x13; i++) {
-            inb(VGAREG_ACTL_RESET);
-            outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
-            SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_ACTL_READ_DATA));
-            BX++;
-        }
-        inb(VGAREG_ACTL_RESET);
-
-        for (i = 0; i <= 8; i++) {
-            outb(i, VGAREG_GRDC_ADDRESS);
-            SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_DATA));
-            BX++;
-        }
-
-        SET_FARVAR(ES, *(u16*)(BX+0), crtc_addr);
-        BX += 2;
-
-        /* XXX: read plane latches */
-        SET_FARVAR(ES, *(u8*)(BX+0), 0);
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), 0);
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), 0);
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), 0);
-        BX++;
-    }
-    if (CX & 2) {
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_mode));
-        BX++;
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_cols));
-        BX += 2;
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagesize));
-        BX += 2;
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(crtc_address));
-        BX += 2;
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_rows));
-        BX++;
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(char_height));
-        BX += 2;
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_ctl));
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_switches));
-        BX++;
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(modeset_ctl));
-        BX++;
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_type));
-        BX += 2;
-        u16 i;
-        for (i = 0; i < 8; i++) {
-            SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_pos[i]));
-            BX += 2;
-        }
-        SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagestart));
-        BX += 2;
-        SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_page));
-        BX++;
-        /* current font */
-        SET_FARVAR(ES, *(u32*)(BX+0), GET_IVT(0x1f).segoff);
-        BX += 4;
-        SET_FARVAR(ES, *(u32*)(BX+0), GET_IVT(0x43).segoff);
-        BX += 4;
-    }
-    if (CX & 4) {
-        /* XXX: check this */
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_STATE));
-        BX++;                   /* read/write mode dac */
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_WRITE_ADDRESS));
-        BX++;                   /* pix address */
-        SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_PEL_MASK));
-        BX++;
-        // Set the whole dac always, from 0
-        outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
-        u16 i;
-        for (i = 0; i < 256 * 3; i++) {
-            SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_DATA));
-            BX++;
-        }
-        SET_FARVAR(ES, *(u8*)(BX+0), 0);
-        BX++;                   /* color select register */
-    }
-    return BX;
-}
-
-static u16
-biosfn_restore_video_state(u16 CX, u16 ES, u16 BX)
+biosfn_save_bda_state(u16 seg, struct saveBDAstate *info)
 {
-    if (CX & 1) {
-        // Reset Attribute Ctl flip-flop
-        inb(VGAREG_ACTL_RESET);
-
-        u16 crtc_addr = GET_FARVAR(ES, *(u16*)(BX + 0x40));
-        u16 addr1 = BX;
-        BX += 5;
-
-        u16 i;
-        for (i = 1; i <= 4; i++) {
-            outb(i, VGAREG_SEQU_ADDRESS);
-            outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
-            BX++;
-        }
-        outb(0, VGAREG_SEQU_ADDRESS);
-        outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
-        BX++;
-
-        // Disable CRTC write protection
-        outw(0x0011, crtc_addr);
-        // Set CRTC regs
-        for (i = 0; i <= 0x18; i++) {
-            if (i != 0x11) {
-                outb(i, crtc_addr);
-                outb(GET_FARVAR(ES, *(u8*)(BX+0)), crtc_addr + 1);
-            }
-            BX++;
-        }
-        // select crtc base address
-        u16 v = inb(VGAREG_READ_MISC_OUTPUT) & ~0x01;
-        if (crtc_addr == VGAREG_VGA_CRTC_ADDRESS)
-            v |= 0x01;
-        outb(v, VGAREG_WRITE_MISC_OUTPUT);
-
-        // enable write protection if needed
-        outb(0x11, crtc_addr);
-        outb(GET_FARVAR(ES, *(u8*)(BX - 0x18 + 0x11)), crtc_addr + 1);
-
-        // Set Attribute Ctl
-        u16 ar_index = GET_FARVAR(ES, *(u8*)(addr1 + 0x03));
-        inb(VGAREG_ACTL_RESET);
-        for (i = 0; i <= 0x13; i++) {
-            outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
-            outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_ACTL_WRITE_DATA);
-            BX++;
-        }
-        outb(ar_index, VGAREG_ACTL_ADDRESS);
-        inb(VGAREG_ACTL_RESET);
-
-        for (i = 0; i <= 8; i++) {
-            outb(i, VGAREG_GRDC_ADDRESS);
-            outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_GRDC_DATA);
-            BX++;
-        }
-        BX += 2;                /* crtc_addr */
-        BX += 4;                /* plane latches */
-
-        outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_SEQU_ADDRESS);
-        addr1++;
-        outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr);
-        addr1++;
-        outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_GRDC_ADDRESS);
-        addr1++;
-        addr1++;
-        outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr - 0x4 + 0xa);
-        addr1++;
-    }
-    if (CX & 2) {
-        SET_BDA(video_mode, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        SET_BDA(video_cols, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        SET_BDA(video_pagesize, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        SET_BDA(crtc_address, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        SET_BDA(video_rows, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        SET_BDA(char_height, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        SET_BDA(video_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        SET_BDA(video_switches, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        SET_BDA(modeset_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        SET_BDA(cursor_type, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        u16 i;
-        for (i = 0; i < 8; i++) {
-            SET_BDA(cursor_pos[i], GET_FARVAR(ES, *(u16*)(BX+0)));
-            BX += 2;
-        }
-        SET_BDA(video_pagestart, GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 2;
-        SET_BDA(video_page, GET_FARVAR(ES, *(u8*)(BX+0)));
-        BX++;
-        /* current font */
-        SET_IVT(0x1f, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 4;
-        SET_IVT(0x43, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
-        BX += 4;
-    }
-    if (CX & 4) {
-        BX++;
-        u16 v = GET_FARVAR(ES, *(u8*)(BX+0));
-        BX++;
-        outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_PEL_MASK);
-        BX++;
-        // Set the whole dac always, from 0
-        outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
-        u16 i;
-        for (i = 0; i < 256 * 3; i++) {
-            outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_DAC_DATA);
-            BX++;
-        }
-        BX++;
-        outb(v, VGAREG_DAC_WRITE_ADDRESS);
-    }
-    return BX;
+    SET_FARVAR(seg, info->video_mode, GET_BDA(video_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));
+    SET_FARVAR(seg, info->video_rows, GET_BDA(video_rows));
+    SET_FARVAR(seg, info->char_height, GET_BDA(char_height));
+    SET_FARVAR(seg, info->video_ctl, GET_BDA(video_ctl));
+    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;
+    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));
+    SET_FARVAR(seg, info->video_page, GET_BDA(video_page));
+    /* current font */
+    SET_FARVAR(seg, *(u32*)&info->font0_off, GET_IVT(0x1f).segoff);
+    SET_FARVAR(seg, *(u32*)&info->font1_off, GET_IVT(0x43).segoff);
+}
+
+static void
+biosfn_restore_bda_state(u16 seg, struct saveBDAstate *info)
+{
+    SET_BDA(video_mode, GET_FARVAR(seg, info->video_mode));
+    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));
+    SET_BDA(video_rows, GET_FARVAR(seg, info->video_rows));
+    SET_BDA(char_height, GET_FARVAR(seg, info->char_height));
+    SET_BDA(video_ctl, GET_FARVAR(seg, info->video_ctl));
+    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;
+    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));
+    SET_BDA(video_page, GET_FARVAR(seg, info->video_page));
+    /* current font */
+    SET_IVT(0x1f, GET_FARVAR(seg, info->font0_seg)
+            , GET_FARVAR(seg, info->font0_off));
+    SET_IVT(0x43, GET_FARVAR(seg, info->font1_seg)
+            , GET_FARVAR(seg, info->font1_off));
 }
 
 
@@ -1430,11 +1193,46 @@ handle_101a(struct bregs *regs)
 }
 
 
+struct funcInfo {
+    u16 static_functionality_off;
+    u16 static_functionality_seg;
+    u8 bda_0x49[30];
+    u8 bda_0x84[3];
+    u8 dcc_index;
+    u8 dcc_alt;
+    u16 colors;
+    u8 pages;
+    u8 scan_lines;
+    u8 primary_char;
+    u8 secondar_char;
+    u8 misc;
+    u8 non_vga_mode;
+    u8 reserved_2f[2];
+    u8 video_mem;
+    u8 save_flags;
+    u8 disp_info;
+    u8 reserved_34[12];
+};
+
 static void
 handle_101b(struct bregs *regs)
 {
-    // XXX - inline
-    biosfn_read_state_info(regs->bx, regs->es, regs->di);
+    u16 seg = regs->es;
+    struct funcInfo *info = (void*)(regs->di+0);
+    memset_far(seg, info, 0, sizeof(*info));
+    // Address of static functionality table
+    SET_FARVAR(seg, info->static_functionality_off, (u32)static_functionality);
+    SET_FARVAR(seg, info->static_functionality_seg, get_global_seg());
+
+    // Hard coded copy from BIOS area. Should it be cleaner ?
+    memcpy_far(seg, info->bda_0x49, SEG_BDA, (void*)0x49, 30);
+    memcpy_far(seg, info->bda_0x84, SEG_BDA, (void*)0x84, 3);
+
+    SET_FARVAR(seg, info->dcc_index, GET_BDA(dcc_index));
+    SET_FARVAR(seg, info->colors, 16);
+    SET_FARVAR(seg, info->pages, 8);
+    SET_FARVAR(seg, info->scan_lines, 2);
+    SET_FARVAR(seg, info->video_mem, 3);
     regs->al = 0x1B;
 }
 
@@ -1442,22 +1240,54 @@ handle_101b(struct bregs *regs)
 static void
 handle_101c00(struct bregs *regs)
 {
-    // XXX - inline
-    regs->bx = biosfn_read_video_state_size(regs->cx);
+    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)
 {
-    // XXX - inline
-    biosfn_save_video_state(regs->cx, regs->es, regs->bx);
+    u16 flags = regs->cx;
+    u16 seg = regs->es;
+    void *data = (void*)(regs->bx+0);
+    if (flags & 1) {
+        vgahw_save_state(seg, data);
+        data += sizeof(struct saveVideoHardware);
+    }
+    if (flags & 2) {
+        biosfn_save_bda_state(seg, data);
+        data += sizeof(struct saveBDAstate);
+    }
+    if (flags & 4)
+        vgahw_save_dac_state(seg, data);
+    regs->al = 0x1c;
 }
 
 static void
 handle_101c02(struct bregs *regs)
 {
-    // XXX - inline
-    biosfn_restore_video_state(regs->cx, regs->es, regs->bx);
+    u16 flags = regs->cx;
+    u16 seg = regs->es;
+    void *data = (void*)(regs->bx+0);
+    if (flags & 1) {
+        vgahw_restore_state(seg, data);
+        data += sizeof(struct saveVideoHardware);
+    }
+    if (flags & 2) {
+        biosfn_restore_bda_state(seg, data);
+        data += sizeof(struct saveBDAstate);
+    }
+    if (flags & 4)
+        vgahw_restore_dac_state(seg, data);
+    regs->al = 0x1c;
 }
 
 static void
diff --git a/vgasrc/vgaio.c b/vgasrc/vgaio.c
index 6f530a8..6c1dbbd 100644
--- a/vgasrc/vgaio.c
+++ b/vgasrc/vgaio.c
@@ -242,6 +242,25 @@ vgahw_get_pel_mask()
     return inb(VGAREG_PEL_MASK);
 }
 
+void
+vgahw_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, inb(VGAREG_PEL_MASK));
+    vgahw_get_dac_regs(seg, info->dac, 0, 256);
+    SET_FARVAR(seg, info->color_select, 0);
+}
+
+void
+vgahw_restore_dac_state(u16 seg, struct saveDACcolors *info)
+{
+    outb(GET_FARVAR(seg, info->pelmask), VGAREG_PEL_MASK);
+    vgahw_set_dac_regs(seg, info->dac, 0, 256);
+    outb(GET_FARVAR(seg, info->peladdr), VGAREG_DAC_WRITE_ADDRESS);
+}
+
 
 /****************************************************************
  * Memory control
@@ -364,3 +383,102 @@ vgahw_init()
     outb(0x04, VGAREG_SEQU_ADDRESS);
     outb(0x02, VGAREG_SEQU_DATA);
 }
+
+void
+vgahw_save_state(u16 seg, struct saveVideoHardware *info)
+{
+    u16 crtc_addr = GET_BDA(crtc_address);
+    SET_FARVAR(seg, info->sequ_index, inb(VGAREG_SEQU_ADDRESS));
+    SET_FARVAR(seg, info->crtc_index, inb(crtc_addr));
+    SET_FARVAR(seg, info->grdc_index, inb(VGAREG_GRDC_ADDRESS));
+    inb(VGAREG_ACTL_RESET);
+    u16 ar_index = inb(VGAREG_ACTL_ADDRESS);
+    SET_FARVAR(seg, info->actl_index, ar_index);
+    SET_FARVAR(seg, info->feature, inb(VGAREG_READ_FEATURE_CTL));
+
+    u16 i;
+    for (i=0; i<4; i++) {
+        outb(i+1, VGAREG_SEQU_ADDRESS);
+        SET_FARVAR(seg, info->sequ_regs[i], inb(VGAREG_SEQU_DATA));
+    }
+    outb(0, VGAREG_SEQU_ADDRESS);
+    SET_FARVAR(seg, info->sequ0, inb(VGAREG_SEQU_DATA));
+
+    for (i=0; i<25; i++) {
+        outb(i, crtc_addr);
+        SET_FARVAR(seg, info->crtc_regs[i], inb(crtc_addr + 1));
+    }
+
+    for (i=0; i<20; i++) {
+        inb(VGAREG_ACTL_RESET);
+        outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
+        SET_FARVAR(seg, info->actl_regs[i], inb(VGAREG_ACTL_READ_DATA));
+    }
+    inb(VGAREG_ACTL_RESET);
+
+    for (i=0; i<9; i++) {
+        outb(i, VGAREG_GRDC_ADDRESS);
+        SET_FARVAR(seg, info->grdc_regs[i], inb(VGAREG_GRDC_DATA));
+    }
+
+    SET_FARVAR(seg, info->crtc_addr, crtc_addr);
+
+    /* XXX: read plane latches */
+    for (i=0; i<4; i++)
+        SET_FARVAR(seg, info->plane_latch[i], 0);
+}
+
+void
+vgahw_restore_state(u16 seg, struct saveVideoHardware *info)
+{
+    // Reset Attribute Ctl flip-flop
+    inb(VGAREG_ACTL_RESET);
+
+    u16 crtc_addr = GET_FARVAR(seg, info->crtc_addr);
+
+    u16 i;
+    for (i=0; i<4; i++) {
+        outb(i+1, VGAREG_SEQU_ADDRESS);
+        outb(GET_FARVAR(seg, info->sequ_regs[i]), VGAREG_SEQU_DATA);
+    }
+    outb(0, VGAREG_SEQU_ADDRESS);
+    outb(GET_FARVAR(seg, info->sequ0), VGAREG_SEQU_DATA);
+
+    // Disable CRTC write protection
+    outw(0x0011, crtc_addr);
+    // Set CRTC regs
+    for (i=0; i<25; i++)
+        if (i != 0x11) {
+            outb(i, crtc_addr);
+            outb(GET_FARVAR(seg, info->crtc_regs[i]), crtc_addr + 1);
+        }
+    // select crtc base address
+    u16 v = inb(VGAREG_READ_MISC_OUTPUT) & ~0x01;
+    if (crtc_addr == VGAREG_VGA_CRTC_ADDRESS)
+        v |= 0x01;
+    outb(v, VGAREG_WRITE_MISC_OUTPUT);
+
+    // enable write protection if needed
+    outb(0x11, crtc_addr);
+    outb(GET_FARVAR(seg, info->crtc_regs[0x11]), crtc_addr + 1);
+
+    // Set Attribute Ctl
+    u16 ar_index = GET_FARVAR(seg, info->actl_index);
+    inb(VGAREG_ACTL_RESET);
+    for (i=0; i<20; i++) {
+        outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
+        outb(GET_FARVAR(seg, info->actl_regs[i]), VGAREG_ACTL_WRITE_DATA);
+    }
+    outb(ar_index, VGAREG_ACTL_ADDRESS);
+    inb(VGAREG_ACTL_RESET);
+
+    for (i=0; i<9; i++) {
+        outb(i, VGAREG_GRDC_ADDRESS);
+        outb(GET_FARVAR(seg, info->grdc_regs[i]), VGAREG_GRDC_DATA);
+    }
+
+    outb(GET_FARVAR(seg, info->sequ_index), VGAREG_SEQU_ADDRESS);
+    outb(GET_FARVAR(seg, info->crtc_index), crtc_addr);
+    outb(GET_FARVAR(seg, info->grdc_index), VGAREG_GRDC_ADDRESS);
+    outb(GET_FARVAR(seg, info->feature), crtc_addr - 0x4 + 0xa);
+}
diff --git a/vgasrc/vgatables.h b/vgasrc/vgatables.h
index 2fa3a8d..1cf6fdb 100644
--- a/vgasrc/vgatables.h
+++ b/vgasrc/vgatables.h
@@ -103,6 +103,50 @@ struct vgamode_s {
     u16 dacsize;
 };
 
+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 saveBDAstate {
+    u8 video_mode;
+    u16 video_cols;
+    u16 video_pagesize;
+    u16 crtc_address;
+    u8 video_rows;
+    u16 char_height;
+    u8 video_ctl;
+    u8 video_switches;
+    u8 modeset_ctl;
+    u16 cursor_type;
+    u16 cursor_pos[8];
+    u16 video_pagestart;
+    u8 video_page;
+    /* current font */
+    u16 font0_off;
+    u16 font0_seg;
+    u16 font1_off;
+    u16 font1_seg;
+};
+
+struct saveDACcolors {
+    u8 rwmode;
+    u8 peladdr;
+    u8 pelmask;
+    u8 dac[768];
+    u8 color_select;
+};
+
 // vgatables.c
 struct vgamode_s *find_vga_entry(u8 mode);
 extern u16 video_save_pointer_table[];
@@ -150,6 +194,8 @@ void vgahw_set_dac_regs(u16 seg, u8 *data_far, u8 start, int count);
 void vgahw_get_dac_regs(u16 seg, u8 *data_far, u8 start, int count);
 void vgahw_set_pel_mask(u8 val);
 u8 vgahw_get_pel_mask();
+void vgahw_save_dac_state(u16 seg, struct saveDACcolors *info);
+void vgahw_restore_dac_state(u16 seg, struct saveDACcolors *info);
 void vgahw_set_text_block_specifier(u8 spec);
 void get_font_access();
 void release_font_access();
@@ -160,6 +206,8 @@ void vgahw_set_scan_lines(u8 lines);
 u16 vgahw_get_vde();
 void vgahw_enable_video_addressing(u8 disable);
 void vgahw_init();
+void vgahw_save_state(u16 seg, struct saveVideoHardware *info);
+void vgahw_restore_state(u16 seg, struct saveVideoHardware *info);
 
 // clext.c
 void cirrus_set_video_mode(u8 mode);