diff --git a/lib/drmtest.c b/lib/drmtest.c
index 3ad77a8..7368077 100644
--- a/lib/drmtest.c
+++ b/lib/drmtest.c
@@ -1317,3 +1317,137 @@ int drmtest_set_vt_graphics_mode(void)
 	return orig_vt_mode < 0 ? -1 : 0;
 }
 
+static int get_connector_default_mode(int drm_fd, drmModeConnector *connector,
+				      drmModeModeInfo *mode)
+{
+	drmModeRes *resources;
+	int i;
+
+	resources = drmModeGetResources(drm_fd);
+	if (!resources) {
+		perror("drmModeGetResources failed");
+
+		return -1;
+	}
+
+	if (!connector->count_modes) {
+		fprintf(stderr, "no modes for connector %d\n",
+			connector->connector_id);
+		drmModeFreeResources(resources);
+
+		return -1;
+	}
+
+	for (i = 0; i < connector->count_modes; i++) {
+		if (i == 0 ||
+		    connector->modes[i].type & DRM_MODE_TYPE_PREFERRED) {
+			*mode = connector->modes[i];
+			if (mode->type & DRM_MODE_TYPE_PREFERRED)
+				break;
+		}
+	}
+
+	drmModeFreeResources(resources);
+
+	return 0;
+}
+
+int kmstest_get_connector_config(int drm_fd, uint32_t connector_id,
+				 unsigned long crtc_idx_mask,
+				 struct kmstest_connector_config *config)
+{
+	drmModeRes *resources;
+	drmModeConnector *connector;
+	drmModeEncoder *encoder;
+	int i, j;
+
+	resources = drmModeGetResources(drm_fd);
+	if (!resources) {
+		perror("drmModeGetResources failed");
+		goto err1;
+	}
+
+	/* First, find the connector & mode */
+	connector = drmModeGetConnector(drm_fd, connector_id);
+	if (!connector)
+		goto err2;
+
+	if (connector->connection != DRM_MODE_CONNECTED)
+		goto err3;
+
+	if (!connector->count_modes) {
+		fprintf(stderr, "connector %d has no modes\n", connector_id);
+		goto err3;
+	}
+
+	if (connector->connector_id != connector_id) {
+		fprintf(stderr, "connector id doesn't match (%d != %d)\n",
+			connector->connector_id, connector_id);
+		goto err3;
+	}
+
+	/*
+	 * Find given CRTC if crtc_id != 0 or else the first CRTC not in use.
+	 * In both cases find the first compatible encoder and skip the CRTC
+	 * if there is non such.
+	 */
+	encoder = NULL;		/* suppress GCC warning */
+	for (i = 0; i < resources->count_crtcs; i++) {
+		if (!resources->crtcs[i] || !(crtc_idx_mask & (1 << i)))
+			continue;
+
+		/* Now get a compatible encoder */
+		for (j = 0; j < connector->count_encoders; j++) {
+			encoder = drmModeGetEncoder(drm_fd,
+						    connector->encoders[j]);
+
+			if (!encoder) {
+				fprintf(stderr, "could not get encoder %d: %s\n",
+					resources->encoders[j], strerror(errno));
+
+				continue;
+			}
+
+			if (encoder->possible_crtcs & (1 << i))
+				goto found;
+
+			drmModeFreeEncoder(encoder);
+		}
+	}
+
+	fprintf(stderr,
+		"no crtc with a compatible encoder (crtc_idx_mask %08lx)\n",
+		crtc_idx_mask);
+	goto err3;
+
+found:
+	if (get_connector_default_mode(drm_fd, connector,
+				       &config->default_mode) < 0)
+		goto err4;
+
+	config->connector = connector;
+	config->encoder = encoder;
+	config->crtc = drmModeGetCrtc(drm_fd, resources->crtcs[i]);
+	config->crtc_idx = i;
+	config->pipe = kmstest_get_pipe_from_crtc_id(drm_fd,
+						     config->crtc->crtc_id);
+
+	drmModeFreeResources(resources);
+
+	return 0;
+err4:
+	drmModeFreeEncoder(encoder);
+err3:
+	drmModeFreeConnector(connector);
+err2:
+	drmModeFreeResources(resources);
+err1:
+	return -1;
+}
+
+void kmstest_free_connector_config(struct kmstest_connector_config *config)
+{
+	drmModeFreeCrtc(config->crtc);
+	drmModeFreeEncoder(config->encoder);
+	drmModeFreeConnector(config->connector);
+}
diff --git a/lib/drmtest.h b/lib/drmtest.h
index 3c1368d..89ded11 100644
--- a/lib/drmtest.h
+++ b/lib/drmtest.h
@@ -101,6 +101,20 @@ void drmtest_init_aperture_trashers(drm_intel_bufmgr *bufmgr);
 void drmtest_trash_aperture(void);
 void drmtest_cleanup_aperture_trashers(void);
 
+struct kmstest_connector_config {
+	drmModeCrtc *crtc;
+	drmModeConnector *connector;
+	drmModeEncoder *encoder;
+	drmModeModeInfo default_mode;
+	int crtc_idx;
+	int pipe;
+};
+
+int kmstest_get_connector_config(int drm_fd, uint32_t connector_id,
+				 unsigned long crtc_idx_mask,
+				 struct kmstest_connector_config *config);
+void kmstest_free_connector_config(struct kmstest_connector_config *config);
+
 /* helpers to create nice-looking framebuffers */
 struct kmstest_fb {
 	uint32_t fb_id;
diff --git a/tests/kms_flip.c b/tests/kms_flip.c
index 735b4dd..c9b3d8a 100644
--- a/tests/kms_flip.c
+++ b/tests/kms_flip.c
@@ -825,97 +825,23 @@ static void update_all_state(struct test_output *o,
 		update_state(&o->vblank_state);
 }
 
-static void connector_find_preferred_mode(struct test_output *o, int crtc_id)
+static void connector_find_preferred_mode(uint32_t connector_id, int crtc_idx,
+					  struct test_output *o)
 {
-	drmModeConnector *connector;
-	drmModeEncoder *encoder = NULL;
-	int i, j;
-
-	/* First, find the connector & mode */
-	o->mode_valid = 0;
-	o->crtc = 0;
-	connector = drmModeGetConnector(drm_fd, o->id);
-	assert(connector);
-
-	if (connector->connection != DRM_MODE_CONNECTED) {
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	if (!connector->count_modes) {
-		fprintf(stderr, "connector %d has no modes\n", o->id);
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	if (connector->connector_id != o->id) {
-		fprintf(stderr, "connector id doesn't match (%d != %d)\n",
-			connector->connector_id, o->id);
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	for (j = 0; j < connector->count_modes; j++) {
-		o->mode = connector->modes[j];
-		if (o->mode.type & DRM_MODE_TYPE_PREFERRED) {
-			o->mode_valid = 1;
-			break;
-		}
-	}
-
-	if (!o->mode_valid) {
-		if (connector->count_modes > 0) {
-			/* use the first mode as test mode */
-			o->mode = connector->modes[0];
-			o->mode_valid = 1;
-		}
-		else {
-			fprintf(stderr, "failed to find any modes on connector %d\n",
-				o->id);
-			return;
-		}
-	}
+	struct kmstest_connector_config config;
 
-	/* Now get the encoder */
-	for (i = 0; i < connector->count_encoders; i++) {
-		encoder = drmModeGetEncoder(drm_fd, connector->encoders[i]);
-
-		if (!encoder) {
-			fprintf(stderr, "could not get encoder %i: %s\n",
-				resources->encoders[i], strerror(errno));
-			drmModeFreeEncoder(encoder);
-			continue;
-		}
-
-		break;
-	}
-
-	o->encoder = encoder;
-
-	if (i == resources->count_encoders) {
-		fprintf(stderr, "failed to find encoder\n");
-		o->mode_valid = 0;
-		return;
-	}
-
-	/* Find first CRTC not in use */
-	for (i = 0; i < resources->count_crtcs; i++) {
-		if (resources->crtcs[i] != crtc_id)
-			continue;
-		if (resources->crtcs[i] &&
-		    (o->encoder->possible_crtcs & (1<<i))) {
-			o->crtc = resources->crtcs[i];
-			break;
-		}
-	}
-
-	if (!o->crtc) {
-		fprintf(stderr, "could not find requested crtc %d\n", crtc_id);
+	if (kmstest_get_connector_config(drm_fd, connector_id, 1 << crtc_idx,
+					 &config) < 0) {
 		o->mode_valid = 0;
 		return;
 	}
 
-	o->connector = connector;
+	o->connector = config.connector;
+	o->encoder = config.encoder;
+	o->crtc = config.crtc->crtc_id;
+	o->pipe = config.pipe;
+	o->mode = config.default_mode;
+	o->mode_valid = 1;
 }
 
 static void
@@ -1042,21 +968,21 @@ static unsigned event_loop(struct test_output *o, unsigned duration_sec)
 	return end - start;
 }
 
-static void run_test_on_crtc(struct test_output *o, int crtc, int duration)
+static void run_test_on_crtc(struct test_output *o, int crtc_idx, int duration)
 {
 	unsigned ellapsed;
 
 	o->bpp = 32;
 	o->depth = 24;
 
-	connector_find_preferred_mode(o, crtc);
+	connector_find_preferred_mode(o->id, crtc_idx, o);
 	if (!o->mode_valid)
 		return;
 
 	last_connector = o->connector;
 
 	fprintf(stdout, "Beginning %s on crtc %d, connector %d\n",
-		o->test_name, crtc, o->id);
+		o->test_name, o->crtc, o->id);
 
 	o->fb_width = o->mode.hdisplay;
 	o->fb_height = o->mode.vdisplay;
@@ -1116,7 +1042,7 @@ static void run_test_on_crtc(struct test_output *o, int crtc, int duration)
 		check_final_state(o, &o->vblank_state, ellapsed);
 
 	fprintf(stdout, "\n%s on crtc %d, connector %d: PASSED\n\n",
-		o->test_name, crtc, o->id);
+		o->test_name, o->crtc, o->id);
 
 	kmstest_remove_fb(drm_fd, o->fb_ids[2]);
 	kmstest_remove_fb(drm_fd, o->fb_ids[1]);
@@ -1131,7 +1057,8 @@ static void run_test_on_crtc(struct test_output *o, int crtc, int duration)
 static int run_test(int duration, int flags, const char *test_name)
 {
 	struct test_output o;
-	int c, i;
+	int c;
+	int crtc_idx;
 
 	resources = drmModeGetResources(drm_fd);
 	if (!resources) {
@@ -1142,19 +1069,15 @@ static int run_test(int duration, int flags, const char *test_name)
 
 	/* Find any connected displays */
 	for (c = 0; c < resources->count_connectors; c++) {
-		for (i = 0; i < resources->count_crtcs; i++) {
-			int crtc;
-
+		for (crtc_idx = 0; crtc_idx < resources->count_crtcs; crtc_idx++) {
 			memset(&o, 0, sizeof(o));
 			o.test_name = test_name;
 			o.id = resources->connectors[c];
 			o.flags = flags;
 			o.flip_state.name = "flip";
 			o.vblank_state.name = "vblank";
-			crtc = resources->crtcs[i];
-			o.pipe = kmstest_get_pipe_from_crtc_id(drm_fd, crtc);
 
-			run_test_on_crtc(&o, crtc, duration);
+			run_test_on_crtc(&o, crtc_idx, duration);
 		}
 	}
 
diff --git a/tests/testdisplay.c b/tests/testdisplay.c
index b10c3b9..4470339 100644
--- a/tests/testdisplay.c
+++ b/tests/testdisplay.c
@@ -102,6 +102,7 @@ struct connector {
 	drmModeEncoder *encoder;
 	drmModeConnector *connector;
 	int crtc;
+	int crtc_idx;
 	int pipe;
 };
 
@@ -185,101 +186,31 @@ static void dump_crtcs_fd(int drmfd)
 	drmModeFreeResources(mode_resources);
 }
 
-static void connector_find_preferred_mode(struct connector *c)
+static void connector_find_preferred_mode(uint32_t connector_id,
+					  unsigned long crtc_idx_mask,
+					  int mode_num, struct connector *c)
 {
-	drmModeConnector *connector;
-	drmModeEncoder *encoder = NULL;
-	int i, j;
-
-	/* First, find the connector & mode */
-	c->mode_valid = 0;
-	connector = drmModeGetConnector(drm_fd, c->id);
-	if (!connector) {
-		fprintf(stderr, "could not get connector %d: %s\n",
-			c->id, strerror(errno));
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	if (connector->connection != DRM_MODE_CONNECTED) {
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	if (!connector->count_modes) {
-		fprintf(stderr, "connector %d has no modes\n", c->id);
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	if (connector->connector_id != c->id) {
-		fprintf(stderr, "connector id doesn't match (%d != %d)\n",
-			connector->connector_id, c->id);
-		drmModeFreeConnector(connector);
-		return;
-	}
-
-	for (j = 0; j < connector->count_modes; j++) {
-		c->mode = connector->modes[j];
-		if (c->mode.type & DRM_MODE_TYPE_PREFERRED) {
-			c->mode_valid = 1;
-			break;
-		}
-	}
-
-	if ( specified_mode_num != -1 ){
-		c->mode = connector->modes[specified_mode_num];
-		if (c->mode.type & DRM_MODE_TYPE_PREFERRED)
-			c->mode_valid = 1;
-	}
-
-	if (!c->mode_valid) {
-		if (connector->count_modes > 0) {
-			/* use the first mode as test mode */
-			c->mode = connector->modes[0];
-			c->mode_valid = 1;
-		}
-		else {
-			fprintf(stderr, "failed to find any modes on connector %d\n",
-				c->id);
-			return;
-		}
-	}
-
-	/* Now get the encoder */
-	for (i = 0; i < connector->count_encoders; i++) {
-		encoder = drmModeGetEncoder(drm_fd, connector->encoders[i]);
-
-		if (!encoder) {
-			fprintf(stderr, "could not get encoder %i: %s\n",
-				resources->encoders[i], strerror(errno));
-			drmModeFreeEncoder(encoder);
-			continue;
-		}
-
-		break;
-	}
-
-	c->encoder = encoder;
+	struct kmstest_connector_config config;
 
-	if (i == resources->count_encoders) {
-		fprintf(stderr, "failed to find encoder\n");
+	if (kmstest_get_connector_config(drm_fd, connector_id, crtc_idx_mask,
+					 &config) < 0) {
 		c->mode_valid = 0;
 		return;
 	}
 
-	/* Find first CRTC not in use */
-	for (i = 0; i < resources->count_crtcs; i++) {
-		if (resources->crtcs[i] && (c->encoder->possible_crtcs & (1<<i)))
-			break;
+	c->connector = config.connector;
+	c->encoder = config.encoder;
+	c->crtc = config.crtc->crtc_id;
+	c->crtc_idx = config.crtc_idx;
+	c->pipe = config.pipe;
+
+	if (mode_num != -1) {
+		assert(mode_num < config.connector->count_modes);
+		c->mode = config.connector->modes[mode_num];
+	} else {
+		c->mode = config.default_mode;
 	}
-	c->crtc = resources->crtcs[i];
-	c->pipe = i;
-
-	if(test_preferred_mode || force_mode || specified_mode_num != -1)
-		resources->crtcs[i] = 0;
-
-	c->connector = connector;
+	c->mode_valid = 1;
 }
 
 static void
@@ -409,10 +340,6 @@ set_mode(struct connector *c)
 	else if (depth > 16 && depth <= 32)
 		bpp = 32;
 
-	connector_find_preferred_mode(c);
-	if (!c->mode_valid)
-		return;
-
 	test_mode_num = 1;
 	if (force_mode){
 		memcpy( &c->mode, &force_timing, sizeof(force_timing));
@@ -506,13 +433,30 @@ int update_display(void)
 	}
 
 	if (test_preferred_mode || test_all_modes || force_mode || specified_disp_id != -1) {
+		unsigned long crtc_idx_mask = -1UL;
+
 		/* Find any connected displays */
 		for (c = 0; c < resources->count_connectors; c++) {
-			connectors[c].id = resources->connectors[c];
-			if ( specified_disp_id != -1 && connectors[c].id != specified_disp_id )
+			struct connector *connector = &connectors[c];
+
+			connector->id = resources->connectors[c];
+			if (specified_disp_id != -1 &&
+			    connector->id != specified_disp_id)
+				continue;
+
+			connector_find_preferred_mode(connector->id,
+						      crtc_idx_mask,
+						      specified_mode_num,
+						      connector);
+			if (!connector->mode_valid)
 				continue;
 
-			set_mode(&connectors[c]);
+			set_mode(connector);
+
+			if (test_preferred_mode || force_mode ||
+			    specified_mode_num != -1)
+				crtc_idx_mask &= ~(1 << connector->crtc_idx);
+
 		}
 	}
 	drmModeFreeResources(resources);
