geodevga: Setup output based on .config

The geode hardware can be configured to use different
outputs. This patch adds support this feature based on
the current .config

Signed-off-by: Christian Gmeiner <christian.gmeiner@gmail.com>

1 files changed, 27 insertions, 2 deletions

diff --git a/vgasrc/geodevga.c b/vgasrc/geodevga.c
index 263ef68..9429856 100644
--- a/vgasrc/geodevga.c
+++ b/vgasrc/geodevga.c
@@ -244,12 +244,37 @@ static void dc_setup(void)
 */
 static void vp_setup(void)
 {
+    u32 msr_addr;
+    u64 msr;
+
     dprintf(2,"VP_SETUP\n");
+
     /* set output to crt and RGB/YUV */
     if (CONFIG_VGA_GEODEGX2)
-        geode_msr_mask(VP_MSR_CONFIG_GX2, 0xf8, 0);
+        msr_addr = VP_MSR_CONFIG_GX2;
     else
-        geode_msr_mask(VP_MSR_CONFIG_LX, 0xf8, 0);
+        msr_addr = VP_MSR_CONFIG_LX;
+
+    /* set output mode (RGB/YUV) */
+    msr = geode_msr_read(msr_addr);
+    msr &= ~VP_MSR_CONFIG_FMT;         // mask out FMT (bits 5:3)
+
+    if (CONFIG_VGA_OUTPUT_PANEL || CONFIG_VGA_OUTPUT_CRT_PANEL) {
+        msr |= VP_MSR_CONFIG_FMT_FP;   // flat panel
+
+        if (CONFIG_VGA_OUTPUT_CRT_PANEL) {
+            msr |= VP_MSR_CONFIG_FPC;  // simultaneous Flat Panel and CRT
+            dprintf(1, "output: simultaneous Flat Panel and CRT\n");
+        } else {
+            msr &= ~VP_MSR_CONFIG_FPC; // no simultaneous Flat Panel and CRT
+            dprintf(1, "ouput: flat panel\n");
+        }
+    } else {
+        msr |= VP_MSR_CONFIG_FMT_CRT;  // CRT only
+       dprintf(1, "output: CRT\n");
+    }
+    geode_msr_mask(msr_addr, ~msr, msr);
+
 
     /* Set mmio registers
     * there may be some timing issues here, the reads seem