From 2469f89528a7da83eb608dfc86fda5fc780b8f92 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sat, 4 Feb 2012 12:40:02 -0500 Subject: [PATCH] vgabios: Implement VBE save/restore state function (func 04). Signed-off-by: Kevin O'Connor --- vgasrc/bochsvga.c | 65 +++++++++++++++++++++++++++++++++++++++++++++++ vgasrc/bochsvga.h | 3 +++ vgasrc/clext.c | 24 +++++++++++++++++ vgasrc/clext.h | 3 +++ vgasrc/vbe.c | 32 +++++++++++++++++++++-- vgasrc/vgabios.c | 13 ++++++---- vgasrc/vgabios.h | 2 +- vgasrc/vgahw.h | 24 +++++++++++++++++ 8 files changed, 158 insertions(+), 8 deletions(-) diff --git a/vgasrc/bochsvga.c b/vgasrc/bochsvga.c index 0a36afc..957bde2 100644 --- a/vgasrc/bochsvga.c +++ b/vgasrc/bochsvga.c @@ -201,6 +201,71 @@ 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 (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) +{ + int ret = stdvga_save_state(seg, data, states); + if (ret < 0) + return ret; + + 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++; + if (!(en & VBE_DISPI_ENABLED)) + return 0; + int i; + for (i = VBE_DISPI_INDEX_XRES; i <= VBE_DISPI_INDEX_Y_OFFSET; i++) + if (i != VBE_DISPI_INDEX_ENABLE) { + u16 v = dispi_read(i); + SET_FARVAR(seg, *info, v); + info++; + } + return 0; +} + +int +bochsvga_restore_state(u16 seg, void *data, int states) +{ + int ret = stdvga_restore_state(seg, data, states); + if (ret < 0) + return ret; + + if (!(states & 8)) + return 0; + + u16 *info = (data + stdvga_size_state(states)); + u16 en = GET_FARVAR(seg, *info); + info++; + if (!(en & VBE_DISPI_ENABLED)) { + dispi_write(VBE_DISPI_INDEX_ENABLE, en); + return 0; + } + int i; + for (i = VBE_DISPI_INDEX_XRES; i <= VBE_DISPI_INDEX_Y_OFFSET; i++) + if (i == VBE_DISPI_INDEX_ENABLE) { + dispi_write(i, en); + } else { + dispi_write(i, GET_FARVAR(seg, *info)); + info++; + } + return 0; +} + /**************************************************************** * Mode setting diff --git a/vgasrc/bochsvga.h b/vgasrc/bochsvga.h index 78a9e0a..1c98203 100644 --- a/vgasrc/bochsvga.h +++ b/vgasrc/bochsvga.h @@ -62,6 +62,9 @@ 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_set_mode(struct vgamode_s *vmode_g, int flags); int bochsvga_init(void); diff --git a/vgasrc/clext.c b/vgasrc/clext.c index 0470d80..7d1a604 100644 --- a/vgasrc/clext.c +++ b/vgasrc/clext.c @@ -368,6 +368,30 @@ clext_set_displaystart(struct vgamode_s *vmode_g, int val) return 0; } +int +clext_size_state(int states) +{ + if (states & 8) + 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); +} + /**************************************************************** * Mode setting diff --git a/vgasrc/clext.h b/vgasrc/clext.h index 3d83507..78dba01 100644 --- a/vgasrc/clext.h +++ b/vgasrc/clext.h @@ -11,6 +11,9 @@ 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_set_mode(struct vgamode_s *vmode_g, int flags); struct bregs; void clext_1012(struct bregs *regs); diff --git a/vgasrc/vbe.c b/vgasrc/vbe.c index 0ee69b4..505cb61 100644 --- 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 diff --git a/vgasrc/vgabios.c b/vgasrc/vgabios.c index ea78c45..7b6c50a 100644 --- a/vgasrc/vgabios.c +++ b/vgasrc/vgabios.c @@ -307,7 +307,7 @@ write_string(struct cursorpos *pcp, u8 attr, u16 count, u16 seg, u8 *offset_far) void save_bda_state(u16 seg, struct saveBDAstate *info) { - SET_FARVAR(seg, info->video_mode, GET_BDA(video_mode)); + SET_FARVAR(seg, info->video_mode, GET_BDA(vbe_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)); @@ -331,8 +331,11 @@ void restore_bda_state(u16 seg, struct saveBDAstate *info) { u16 mode = GET_FARVAR(seg, info->video_mode); - SET_BDA(video_mode, mode); SET_BDA(vbe_mode, mode); + if (mode < 0x100) + SET_BDA(video_mode, mode); + else + SET_BDA(video_mode, 0xff); 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)); @@ -1140,18 +1143,18 @@ handle_101c(struct bregs *regs) int ret; switch (regs->al) { case 0x00: - ret = stdvga_size_state(states); + ret = vgahw_size_state(states); if (ret < 0) goto fail; regs->bx = ret / 64; break; case 0x01: - ret = stdvga_save_state(seg, data, states); + ret = vgahw_save_state(seg, data, states); if (ret) goto fail; break; case 0x02: - ret = stdvga_restore_state(seg, data, states); + ret = vgahw_restore_state(seg, data, states); if (ret) goto fail; break; diff --git a/vgasrc/vgabios.h b/vgasrc/vgabios.h index 76ecb88..d32e1b0 100644 --- a/vgasrc/vgabios.h +++ b/vgasrc/vgabios.h @@ -20,7 +20,7 @@ struct VideoParam_s { extern struct VideoParam_s video_param_table[29]; struct saveBDAstate { - u8 video_mode; + u16 video_mode; u16 video_cols; u16 video_pagesize; u16 crtc_address; diff --git a/vgasrc/vgahw.h b/vgasrc/vgahw.h index 585ae3d..044cd32 100644 --- a/vgasrc/vgahw.h +++ b/vgasrc/vgahw.h @@ -105,4 +105,28 @@ 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) { + if (CONFIG_VGA_CIRRUS) + return clext_size_state(states); + 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); +} + #endif // vgahw.h -- 2.25.1