vgabios: Implement VBE save/restore state function (func 04).

[seabios.git] / vgasrc / vbe.c

diff --git a/vgasrc/vbe.c b/vgasrc/vbe.c
index 77b09344c5019a15bfa24a53603f6727108ba09e..505cb61cbea902c352b195ec2e9258e041c93ecc 100644 (file)
--- a/vgasrc/vbe.c
+++ b/vgasrc/vbe.c
@@ -201,8 +201,36 @@ vbe_104f03(struct bregs *regs)
 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
@@ -305,8 +333,29 @@ fail:
 static void
 vbe_104f08(struct bregs *regs)
 {
-    debug_stub(regs);
-    regs->ax = 0x0100;
+    struct vgamode_s *vmode_g = get_current_mode();
+    if (! vmode_g)
+        goto fail;
+    u8 memmodel = GET_GLOBAL(vmode_g->memmodel);
+    if (memmodel == MM_DIRECT || memmodel == MM_YUV) {
+        regs->ax = 0x034f;
+        return;
+    }
+    if (regs->bl > 1)
+        goto fail;
+    if (regs->bl == 0) {
+        int ret = vgahw_set_dacformat(vmode_g, regs->bh);
+        if (ret < 0)
+            goto fail;
+    }
+    int ret = vgahw_get_dacformat(vmode_g);
+    if (ret < 0)
+        goto fail;
+    regs->bh = ret;
+    regs->ax = 0x004f;
+    return;
+fail:
+    regs->ax = 0x014f;
 }
 
 static void