VGA: Factor out hardware accesses from biosfn_set_video_mode.

Create vgahw_set_mode() that handles low-level vga setup in vgaio.c.
Move screen clearing to new function in vgafb.c.

4 files changed, 113 insertions, 96 deletions

diff --git a/vgasrc/vga.c b/vgasrc/vga.c
index 36f9dde5..fd4ae289 100644
--- a/vgasrc/vga.c
+++ b/vgasrc/vga.c
@@ -211,11 +211,6 @@ biosfn_set_video_mode(u8 mode)
     if (!vmode_g)
         return;
 
-    struct VideoParam_s *vparam_g = GET_GLOBAL(vmode_g->vparam);
-    u16 twidth = GET_GLOBAL(vparam_g->twidth);
-    u16 theightm1 = GET_GLOBAL(vparam_g->theightm1);
-    u16 cheight = GET_GLOBAL(vparam_g->cheight);
-
     // Read the bios mode set control
     u8 modeset_ctl = GET_BDA(modeset_ctl);
 
@@ -224,97 +219,42 @@ biosfn_set_video_mode(u8 mode)
 
     // if palette loading (bit 3 of modeset ctl = 0)
     if ((modeset_ctl & 0x08) == 0) {    // Set the PEL mask
-        outb(GET_GLOBAL(vmode_g->pelmask), VGAREG_PEL_MASK);
-
-        // Set the whole dac always, from 0
-        outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
+        vgahw_set_pel_mask(GET_GLOBAL(vmode_g->pelmask));
 
         // From which palette
         u8 *palette_g = GET_GLOBAL(vmode_g->dac);
         u16 palsize = GET_GLOBAL(vmode_g->dacsize) / 3;
+
         // Always 256*3 values
+        vgahw_set_dac_regs(get_global_seg(), palette_g, 0, palsize);
         u16 i;
-        for (i = 0; i < 0x0100; i++) {
-            if (i < palsize) {
-                outb(GET_GLOBAL(palette_g[(i * 3) + 0]), VGAREG_DAC_DATA);
-                outb(GET_GLOBAL(palette_g[(i * 3) + 1]), VGAREG_DAC_DATA);
-                outb(GET_GLOBAL(palette_g[(i * 3) + 2]), VGAREG_DAC_DATA);
-            } else {
-                outb(0, VGAREG_DAC_DATA);
-                outb(0, VGAREG_DAC_DATA);
-                outb(0, VGAREG_DAC_DATA);
-            }
+        for (i = palsize; i < 0x0100; i++) {
+            static u8 rgb[3] VAR16;
+            vgahw_set_dac_regs(get_global_seg(), rgb, i, 1);
         }
+
         if ((modeset_ctl & 0x02) == 0x02)
             biosfn_perform_gray_scale_summing(0x00, 0x100);
     }
-    // Reset Attribute Ctl flip-flop
-    inb(VGAREG_ACTL_RESET);
 
-    // Set Attribute Ctl
-    u16 i;
-    for (i = 0; i <= 0x13; i++) {
-        outb(i, VGAREG_ACTL_ADDRESS);
-        outb(GET_GLOBAL(vparam_g->actl_regs[i]), VGAREG_ACTL_WRITE_DATA);
-    }
-    outb(0x14, VGAREG_ACTL_ADDRESS);
-    outb(0x00, VGAREG_ACTL_WRITE_DATA);
-
-    // Set Sequencer Ctl
-    outb(0, VGAREG_SEQU_ADDRESS);
-    outb(0x03, VGAREG_SEQU_DATA);
-    for (i = 1; i <= 4; i++) {
-        outb(i, VGAREG_SEQU_ADDRESS);
-        outb(GET_GLOBAL(vparam_g->sequ_regs[i - 1]), VGAREG_SEQU_DATA);
-    }
+    struct VideoParam_s *vparam_g = GET_GLOBAL(vmode_g->vparam);
+    vgahw_set_mode(vparam_g);
 
-    // Set Grafx Ctl
-    for (i = 0; i <= 8; i++) {
-        outb(i, VGAREG_GRDC_ADDRESS);
-        outb(GET_GLOBAL(vparam_g->grdc_regs[i]), VGAREG_GRDC_DATA);
-    }
+    if (noclearmem == 0x00)
+        clear_screen(vmode_g);
 
     // Set CRTC address VGA or MDA
     u16 crtc_addr = VGAREG_VGA_CRTC_ADDRESS;
     if (GET_GLOBAL(vmode_g->memmodel) == MTEXT)
         crtc_addr = VGAREG_MDA_CRTC_ADDRESS;
 
-    // Disable CRTC write protection
-    outw(0x0011, crtc_addr);
-    // Set CRTC regs
-    for (i = 0; i <= 0x18; i++) {
-        outb(i, crtc_addr);
-        outb(GET_GLOBAL(vparam_g->crtc_regs[i]), crtc_addr + 1);
-    }
-
-    // Set the misc register
-    outb(GET_GLOBAL(vparam_g->miscreg), VGAREG_WRITE_MISC_OUTPUT);
-
-    // Enable video
-    outb(0x20, VGAREG_ACTL_ADDRESS);
-    inb(VGAREG_ACTL_RESET);
-
-    if (noclearmem == 0x00) {
-        if (GET_GLOBAL(vmode_g->class) == TEXT) {
-            memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0720, 32*1024);
-        } else {
-            if (mode < 0x0d) {
-                memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0000, 32*1024);
-            } else {
-                outb(0x02, VGAREG_SEQU_ADDRESS);
-                u8 mmask = inb(VGAREG_SEQU_DATA);
-                outb(0x0f, VGAREG_SEQU_DATA);   // all planes
-                memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0000, 64*1024);
-                outb(mmask, VGAREG_SEQU_DATA);
-            }
-        }
-    }
     // Set the BIOS mem
+    u16 cheight = GET_GLOBAL(vparam_g->cheight);
     SET_BDA(video_mode, mode);
-    SET_BDA(video_cols, twidth);
+    SET_BDA(video_cols, GET_GLOBAL(vparam_g->twidth));
     SET_BDA(video_pagesize, GET_GLOBAL(vparam_g->slength));
     SET_BDA(crtc_address, crtc_addr);
-    SET_BDA(video_rows, theightm1);
+    SET_BDA(video_rows, GET_GLOBAL(vparam_g->theightm1));
     SET_BDA(char_height, cheight);
     SET_BDA(video_ctl, (0x60 | noclearmem));
     SET_BDA(video_switches, 0xF9);
@@ -333,6 +273,7 @@ biosfn_set_video_mode(u8 mode)
     if (GET_GLOBAL(vmode_g->class) == TEXT)
         biosfn_set_cursor_shape(0x06, 0x07);
     // Set cursor pos for page 0..7
+    int i;
     for (i = 0; i < 8; i++)
         biosfn_set_cursor_pos(i, 0x0000);
 
diff --git a/vgasrc/vgafb.c b/vgasrc/vgafb.c
index a34b6c03..6e107023 100644
--- a/vgasrc/vgafb.c
+++ b/vgasrc/vgafb.c
@@ -79,6 +79,24 @@ vgamem_fill_cga(u8 xstart, u8 ystart, u8 cols, u8 nbcols, u8 cheight,
 }
 
 void
+clear_screen(struct vgamode_s *vmode_g)
+{
+    if (GET_GLOBAL(vmode_g->class) == TEXT) {
+        memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0720, 32*1024);
+        return;
+    }
+    if (GET_GLOBAL(vmode_g->svgamode) < 0x0d) {
+        memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0000, 32*1024);
+        return;
+    }
+    outb(0x02, VGAREG_SEQU_ADDRESS);
+    u8 mmask = inb(VGAREG_SEQU_DATA);
+    outb(0x0f, VGAREG_SEQU_DATA);   // all planes
+    memset16_far(GET_GLOBAL(vmode_g->sstart), 0, 0x0000, 64*1024);
+    outb(mmask, VGAREG_SEQU_DATA);
+}
+
+void
 biosfn_scroll(u8 nblines, u8 attr, u8 rul, u8 cul, u8 rlr, u8 clr, u8 page,
               u8 dir)
 {
diff --git a/vgasrc/vgaio.c b/vgasrc/vgaio.c
index 6c1dbbd1..7c6a1c53 100644
--- a/vgasrc/vgaio.c
+++ b/vgasrc/vgaio.c
@@ -363,31 +363,13 @@ vgahw_get_vde()
 
 
 /****************************************************************
- * Misc
+ * Save/Restore/Set state
  ****************************************************************/
 
 void
-vgahw_enable_video_addressing(u8 disable)
-{
-    u8 v = (disable & 1) ? 0x00 : 0x02;
-    u8 v2 = inb(VGAREG_READ_MISC_OUTPUT) & ~0x02;
-    outb(v | v2, VGAREG_WRITE_MISC_OUTPUT);
-}
-
-void
-vgahw_init()
-{
-    // switch to color mode and enable CPU access 480 lines
-    outb(0xc3, VGAREG_WRITE_MISC_OUTPUT);
-    // more than 64k 3C4/04
-    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);
+    u16 crtc_addr = get_crtc();
     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));
@@ -482,3 +464,77 @@ vgahw_restore_state(u16 seg, struct saveVideoHardware *info)
     outb(GET_FARVAR(seg, info->grdc_index), VGAREG_GRDC_ADDRESS);
     outb(GET_FARVAR(seg, info->feature), crtc_addr - 0x4 + 0xa);
 }
+
+void
+vgahw_set_mode(struct VideoParam_s *vparam_g)
+{
+    // Reset Attribute Ctl flip-flop
+    inb(VGAREG_ACTL_RESET);
+
+    // Set Attribute Ctl
+    u16 i;
+    for (i = 0; i <= 0x13; i++) {
+        outb(i, VGAREG_ACTL_ADDRESS);
+        outb(GET_GLOBAL(vparam_g->actl_regs[i]), VGAREG_ACTL_WRITE_DATA);
+    }
+    outb(0x14, VGAREG_ACTL_ADDRESS);
+    outb(0x00, VGAREG_ACTL_WRITE_DATA);
+
+    // Set Sequencer Ctl
+    outb(0, VGAREG_SEQU_ADDRESS);
+    outb(0x03, VGAREG_SEQU_DATA);
+    for (i = 1; i <= 4; i++) {
+        outb(i, VGAREG_SEQU_ADDRESS);
+        outb(GET_GLOBAL(vparam_g->sequ_regs[i - 1]), VGAREG_SEQU_DATA);
+    }
+
+    // Set Grafx Ctl
+    for (i = 0; i <= 8; i++) {
+        outb(i, VGAREG_GRDC_ADDRESS);
+        outb(GET_GLOBAL(vparam_g->grdc_regs[i]), VGAREG_GRDC_DATA);
+    }
+
+    // Set CRTC address VGA or MDA
+    u8 miscreg = GET_GLOBAL(vparam_g->miscreg);
+    u16 crtc_addr = VGAREG_VGA_CRTC_ADDRESS;
+    if (!(miscreg & 1))
+        crtc_addr = VGAREG_MDA_CRTC_ADDRESS;
+
+    // Disable CRTC write protection
+    outw(0x0011, crtc_addr);
+    // Set CRTC regs
+    for (i = 0; i <= 0x18; i++) {
+        outb(i, crtc_addr);
+        outb(GET_GLOBAL(vparam_g->crtc_regs[i]), crtc_addr + 1);
+    }
+
+    // Set the misc register
+    outb(miscreg, VGAREG_WRITE_MISC_OUTPUT);
+
+    // Enable video
+    outb(0x20, VGAREG_ACTL_ADDRESS);
+    inb(VGAREG_ACTL_RESET);
+}
+
+
+/****************************************************************
+ * Misc
+ ****************************************************************/
+
+void
+vgahw_enable_video_addressing(u8 disable)
+{
+    u8 v = (disable & 1) ? 0x00 : 0x02;
+    u8 v2 = inb(VGAREG_READ_MISC_OUTPUT) & ~0x02;
+    outb(v | v2, VGAREG_WRITE_MISC_OUTPUT);
+}
+
+void
+vgahw_init()
+{
+    // switch to color mode and enable CPU access 480 lines
+    outb(0xc3, VGAREG_WRITE_MISC_OUTPUT);
+    // more than 64k 3C4/04
+    outb(0x04, VGAREG_SEQU_ADDRESS);
+    outb(0x02, VGAREG_SEQU_DATA);
+}
diff --git a/vgasrc/vgatables.h b/vgasrc/vgatables.h
index 1cf6fdb1..03ea6133 100644
--- a/vgasrc/vgatables.h
+++ b/vgasrc/vgatables.h
@@ -164,6 +164,7 @@ extern u8 vgafont16alt[];
 u16 biosfn_get_cursor_pos(u8 page);
 
 // vgafb.c
+void clear_screen(struct vgamode_s *vmode_g);
 void biosfn_scroll(u8 nblines, u8 attr, u8 rul, u8 cul, u8 rlr, u8 clr
                    , u8 page, u8 dir);
 void biosfn_write_char_attr(u8 car, u8 page, u8 attr, u16 count);
@@ -204,10 +205,11 @@ void vgahw_set_active_page(u16 address);
 void vgahw_set_cursor_pos(u16 address);
 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);
+void vgahw_set_mode(struct VideoParam_s *vparam_g);
+void vgahw_enable_video_addressing(u8 disable);
+void vgahw_init();
 
 // clext.c
 void cirrus_set_video_mode(u8 mode);