1 files changed, 43 insertions, 13 deletions

diff --git a/drivers/video/via/viamode.c b/drivers/video/via/viamode.c
index 8a9f4fcb50e9..88096e5fa077 100644
--- a/drivers/video/via/viamode.c
+++ b/drivers/video/via/viamode.c
@@ -863,26 +863,56 @@ int NUM_TOTAL_CLE266_ModeXregs = ARRAY_SIZE(CLE266_ModeXregs);
 int NUM_TOTAL_PATCH_MODE = ARRAY_SIZE(res_patch_table);
 
 
-struct VideoModeTable *viafb_get_mode(int hres, int vres)
+static struct VideoModeTable *get_modes(struct VideoModeTable *vmt, int n,
+	int hres, int vres)
 {
-	u32 i;
-	for (i = 0; i < ARRAY_SIZE(viafb_modes); i++)
-		if (viafb_modes[i].mode_array &&
-			viafb_modes[i].crtc[0].crtc.hor_addr == hres &&
-			viafb_modes[i].crtc[0].crtc.ver_addr == vres)
+	int i;
+
+	for (i = 0; i < n; i++)
+		if (vmt[i].mode_array &&
+			vmt[i].crtc[0].crtc.hor_addr == hres &&
+			vmt[i].crtc[0].crtc.ver_addr == vres)
 			return &viafb_modes[i];
 
 	return NULL;
 }
 
+static struct crt_mode_table *get_best_mode(struct VideoModeTable *vmt,
+	int refresh)
+{
+	struct crt_mode_table *best;
+	int i;
+
+	if (!vmt)
+		return NULL;
+
+	best = &vmt->crtc[0];
+	for (i = 1; i < vmt->mode_array; i++) {
+		if (abs(vmt->crtc[i].refresh_rate - refresh)
+			< abs(best->refresh_rate - refresh))
+			best = &vmt->crtc[i];
+	}
+
+	return best;
+}
+
+struct VideoModeTable *viafb_get_mode(int hres, int vres)
+{
+	return get_modes(viafb_modes, ARRAY_SIZE(viafb_modes), hres, vres);
+}
+
+struct crt_mode_table *viafb_get_best_mode(int hres, int vres, int refresh)
+{
+	return get_best_mode(viafb_get_mode(hres, vres), refresh);
+}
+
 struct VideoModeTable *viafb_get_rb_mode(int hres, int vres)
 {
-	u32 i;
-	for (i = 0; i < ARRAY_SIZE(viafb_rb_modes); i++)
-		if (viafb_rb_modes[i].mode_array &&
-			viafb_rb_modes[i].crtc[0].crtc.hor_addr == hres &&
-			viafb_rb_modes[i].crtc[0].crtc.ver_addr == vres)
-			return &viafb_rb_modes[i];
+	return get_modes(viafb_rb_modes, ARRAY_SIZE(viafb_rb_modes), hres,
+		vres);
+}
 
-	return NULL;
+struct crt_mode_table *viafb_get_best_rb_mode(int hres, int vres, int refresh)
+{
+	return get_best_mode(viafb_get_rb_mode(hres, vres), refresh);
 }