diff --git a/src/servers/app/ServerScreen.cpp b/src/servers/app/ServerScreen.cpp
index 3ef9b70460..bfb5e82aa7 100644
--- a/src/servers/app/ServerScreen.cpp
+++ b/src/servers/app/ServerScreen.cpp
@@ -22,6 +22,22 @@
 #include "ServerScreen.h"
 
 
+static float
+get_mode_frequency(const display_mode& mode)
+{
+	// Taken from Screen preferences
+	float timing = float(mode.timing.h_total * mode.timing.v_total);
+	if (timing == 0.0f)
+		return 0.0f;
+
+	return rint(10 * float(mode.timing.pixel_clock * 1000)
+		/ timing) / 10.0;
+}
+
+
+//	#pragma mark -
+
+
 Screen::Screen(::HWInterface *interface, int32 id)
 	: fID(id),
 	  fDriver(interface ? new DrawingEngine(interface) : NULL),
@@ -97,8 +113,25 @@ Screen::SetMode(uint16 width, uint16 height, uint32 colorspace,
 		status = _FindMode(640, 480, B_CMAP8, 60.0, &mode);
 	}
 
-	if (status >= B_OK)
+	if (status >= B_OK) {
+		float modeFrequency = get_mode_frequency(mode);
+		display_mode originalMode = mode;
+		bool adjusted = false;
+
+		if (modeFrequency != frequency) {
+			// adjust timing to fit the requested frequency if needed
+			// (taken from Screen preferences application)
+			mode.timing.pixel_clock = ((uint32)mode.timing.h_total
+				* mode.timing.v_total / 10 * int32(frequency * 10)) / 1000;
+			adjusted = true;
+		}
+
 		status = SetMode(mode, makeDefault);
+		if (status < B_OK) {
+			// try again with the unchanged mode
+			status = SetMode(originalMode, makeDefault);
+		}
+	}
 
 	return status;
 }
@@ -121,8 +154,7 @@ Screen::GetMode(uint16 &width, uint16 &height, uint32 &colorspace,
 	width = mode.virtual_width;
 	height = mode.virtual_height;
 	colorspace = mode.space;
-	frequency = mode.timing.pixel_clock * 1000.f
-		/ (mode.timing.h_total * mode.timing.v_total);
+	frequency = get_mode_frequency(mode);
 }
 
 
@@ -150,10 +182,10 @@ status_t
 Screen::_FindMode(uint16 width, uint16 height, uint32 colorspace,
 				  float frequency, display_mode* mode) const
 {
-	display_mode* dm = NULL;
+	display_mode* modes = NULL;
 	uint32 count;
 
-	status_t status = fHWInterface->GetModeList(&dm, &count);
+	status_t status = fHWInterface->GetModeList(&modes, &count);
 	if (status < B_OK || count <= 0) {
 		// We've run into quite a problem here! This is a function which is a requirement
 		// for a graphics module. The best thing that we can hope for is 640x480x8 without
@@ -166,61 +198,55 @@ Screen::_FindMode(uint16 width, uint16 height, uint32 colorspace,
 		return status;
 	}
 
-#ifndef HAIKU_TARGET_PLATFORM_LIBBE_TEST
-	int32 index = _FindMode(dm, count, width, height, colorspace, frequency);
+	int32 index = _FindMode(modes, count, width, height, colorspace, frequency);
 	if (index < 0) {
-		fprintf(stderr, "mode not found (%d, %d, %f) -> ignoring frequency and using B_CMAP8!\n", width, height, frequency);
-		index = _FindMode(dm, count, width, height, B_CMAP8, frequency, true);
-	}
-#else
-	// Ignore frequency in test mode
-	int32 index = _FindMode(dm, count, width, height, colorspace, frequency, true);
-#endif
-
-	if (index < 0) {
-		fprintf(stderr, "mode not found (%d, %d, %f) -> using first mode from list!\n", width, height, frequency);
+		fprintf(stderr, "mode not found (%d, %d, %f) -> using first mode from list!\n",
+			width, height, frequency);
 		// fallback to first mode from list - TODO: ?!?
 		// NOTE: count > 0 is checked above
-		*mode = dm[0];	
+		*mode = modes[0];	
 	} else {
-		*mode = dm[index];	
+		*mode = modes[index];	
 	}
 
-	delete[] dm;
+	delete[] modes;
 
 	return B_OK;
 }
 
 
+/*!
+	\brief Returns the mode that matches the given criteria. The frequency
+		doesn't have to match, though - the closest one found is used.
+*/
 int32
-Screen::_FindMode(const display_mode* dm, uint32 count,
-				  uint16 width, uint16 height, uint32 colorspace,
-				  float frequency, bool ignoreFrequency) const
+Screen::_FindMode(const display_mode* modes, uint32 count,
+	uint16 width, uint16 height, uint32 colorspace,
+	float frequency) const
 {
+	// we always try to choose the mode with the closest frequency
+	float bestFrequencyDiff = 0.0f;
 	int32 index = -1;
+
 	for (uint32 i = 0; i < count; i++) {
-		if (dm[i].virtual_width == width
-			&& dm[i].virtual_height == height
-			&& dm[i].space == colorspace) {
-			if (!ignoreFrequency) {
-				// we have found a mode with the correct width, height and format
-				// now see if the frequency matches (don't be too picky)
-				float modeFrequency = 0.0;
-				float t = dm[i].timing.h_total * dm[i].timing.v_total;
-				if (t != 0.0)
-					modeFrequency = dm[i].timing.pixel_clock * 1000.f / t;
-	
-				if (int(modeFrequency/* * 10.0*/ + 0.5) == int(frequency/* * 10.0*/ + 0.5)) {
-					index = i;
-					break;
-				}
-			} else {
-				// ignore frequency
+		if (modes[i].virtual_width == width
+			&& modes[i].virtual_height == height
+			&& modes[i].space == colorspace) {
+			// we have found a mode with the correct width, height and format
+			// now see if the frequency matches
+			float modeFrequency = get_mode_frequency(modes[i]);
+			float frequencyDiff = fabs(modeFrequency - frequency);
+
+			if (index == -1 || bestFrequencyDiff > frequencyDiff) {
 				index = i;
-				break;
+				if (frequencyDiff == 0.0f)
+					break;
+
+				bestFrequencyDiff = frequencyDiff;
 			}
 		}
 	}
+
 	return index;
 }
 
diff --git a/src/servers/app/ServerScreen.h b/src/servers/app/ServerScreen.h
index 4f324e2338..44656ad463 100644
--- a/src/servers/app/ServerScreen.h
+++ b/src/servers/app/ServerScreen.h
@@ -58,9 +58,8 @@ class Screen {
 										  display_mode* mode) const;
 
 			int32				_FindMode(const display_mode* modeList,
-										  uint32 count, uint16 width, uint16 height, uint32 colorspace,
-										  float frequency,
-										  bool ignoreFrequency = false) const;
+										  uint32 count, uint16 width, uint16 height,
+										  uint32 colorspace, float frequency) const;
 
 			int32				fID;
 			DrawingEngine*		fDriver;