3 files changed, 53 insertions, 35 deletions

diff --git a/drivers/video/via/hw.c b/drivers/video/via/hw.c
index 400373fda210..ecbb01f05112 100644
--- a/drivers/video/via/hw.c
+++ b/drivers/video/via/hw.c
@@ -685,6 +685,54 @@ void viafb_set_secondary_pitch(u32 pitch)
 	viafb_write_reg_mask(0x71, VIACR, (pitch >> (10 - 7)) & 0x80, 0x80);
 }
 
+void viafb_set_primary_color_depth(u8 depth)
+{
+	u8 value;
+
+	DEBUG_MSG(KERN_DEBUG "viafb_set_primary_color_depth(%d)\n", depth);
+	switch (depth) {
+	case 6:
+		value = 0x00;
+		break;
+	case 16:
+		value = 0x14;
+		break;
+	case 24:
+		value = 0x0C;
+		break;
+	default:
+		printk(KERN_WARNING "viafb_set_primary_color_depth: "
+			"Unsupported depth: %d\n", depth);
+		return;
+	}
+
+	viafb_write_reg_mask(0x15, VIASR, value, 0x1C);
+}
+
+void viafb_set_secondary_color_depth(u8 depth)
+{
+	u8 value;
+
+	DEBUG_MSG(KERN_DEBUG "viafb_set_secondary_color_depth(%d)\n", depth);
+	switch (depth) {
+	case 6:
+		value = 0x00;
+		break;
+	case 16:
+		value = 0x40;
+		break;
+	case 24:
+		value = 0xC0;
+		break;
+	default:
+		printk(KERN_WARNING "viafb_set_secondary_color_depth: "
+			"Unsupported depth: %d\n", depth);
+		return;
+	}
+
+	viafb_write_reg_mask(0x67, VIACR, value, 0xC0);
+}
+
 void viafb_set_output_path(int device, int set_iga, int output_interface)
 {
 	switch (device) {
@@ -1761,35 +1809,6 @@ void viafb_load_crtc_timing(struct display_timing device_timing,
 	viafb_lock_crt();
 }
 
-void viafb_set_color_depth(int bpp_byte, int set_iga)
-{
-	if (set_iga == IGA1) {
-		switch (bpp_byte) {
-		case MODE_8BPP:
-			viafb_write_reg_mask(SR15, VIASR, 0x22, 0x7E);
-			break;
-		case MODE_16BPP:
-			viafb_write_reg_mask(SR15, VIASR, 0xB6, 0xFE);
-			break;
-		case MODE_32BPP:
-			viafb_write_reg_mask(SR15, VIASR, 0xAE, 0xFE);
-			break;
-		}
-	} else {
-		switch (bpp_byte) {
-		case MODE_8BPP:
-			viafb_write_reg_mask(CR67, VIACR, 0x00, BIT6 + BIT7);
-			break;
-		case MODE_16BPP:
-			viafb_write_reg_mask(CR67, VIACR, 0x40, BIT6 + BIT7);
-			break;
-		case MODE_32BPP:
-			viafb_write_reg_mask(CR67, VIACR, 0xC0, BIT6 + BIT7);
-			break;
-		}
-	}
-}
-
 void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
 	struct VideoModeTable *video_mode, int bpp_byte, int set_iga)
 {
@@ -1867,9 +1886,6 @@ void viafb_fill_crtc_timing(struct crt_mode_table *crt_table,
 	    && (viaparinfo->chip_info->gfx_chip_name != UNICHROME_K400))
 		viafb_load_FIFO_reg(set_iga, h_addr, v_addr);
 
-	/* load SR Register About Memory and Color part */
-	viafb_set_color_depth(bpp_byte, set_iga);
-
 	pll_D_N = viafb_get_clk_value(crt_table[index].clk);
 	DEBUG_MSG(KERN_INFO "PLL=%x", pll_D_N);
 	viafb_set_vclock(pll_D_N, set_iga);
@@ -2206,6 +2222,8 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
 		outb(VPIT.SR[i - 1], VIASR + 1);
 	}
 
+	viafb_write_reg_mask(0x15, VIASR, viafbinfo->fix.visual
+		== FB_VISUAL_PSEUDOCOLOR ? 0x22 : 0xA2, 0xA2);
 	viafb_set_iga_path();
 
 	/* Write CRTC */
@@ -2245,6 +2263,9 @@ int viafb_setmode(struct VideoModeTable *vmode_tbl, int video_bpp,
 	viafb_set_primary_pitch(viafbinfo->fix.line_length);
 	viafb_set_secondary_pitch(viafb_dual_fb ? viafbinfo1->fix.line_length
 		: viafbinfo->fix.line_length);
+	viafb_set_primary_color_depth(viaparinfo->depth);
+	viafb_set_secondary_color_depth(viafb_dual_fb ? viaparinfo1->depth
+		: viaparinfo->depth);
 	/* Update Refresh Rate Setting */
 
 	/* Clear On Screen */
diff --git a/drivers/video/via/hw.h b/drivers/video/via/hw.h
index 789c58a7bab3..dd54d3e56159 100644
--- a/drivers/video/via/hw.h
+++ b/drivers/video/via/hw.h
@@ -895,7 +895,6 @@ void viafb_load_fetch_count_reg(int h_addr, int bpp_byte, int set_iga);
 void viafb_write_regx(struct io_reg RegTable[], int ItemNum);
 u32 viafb_get_clk_value(int clk);
 void viafb_load_FIFO_reg(int set_iga, int hor_active, int ver_active);
-void viafb_set_color_depth(int bpp_byte, int set_iga);
 void viafb_set_dpa_gfx(int output_interface, struct GFX_DPA_SETTING\
 					*p_gfx_dpa_setting);
 
diff --git a/drivers/video/via/lcd.c b/drivers/video/via/lcd.c
index 3a83e5bc25d9..49651e0a914c 100644
--- a/drivers/video/via/lcd.c
+++ b/drivers/video/via/lcd.c
@@ -1006,8 +1006,6 @@ void viafb_lcd_set_mode(struct crt_mode_table *mode_crt_table,
 		if ((viaparinfo->chip_info->gfx_chip_name != UNICHROME_CLE266)
 		    && (viaparinfo->chip_info->gfx_chip_name != UNICHROME_K400))
 			viafb_load_FIFO_reg(set_iga, set_hres, set_vres);
-
-		viafb_set_color_depth(mode_bpp / 8, set_iga);
 	}
 
 	fill_lcd_format();