diff --git a/drivers/gpu/drm/i915/intel_atomic.c b/drivers/gpu/drm/i915/intel_atomic.c
index 8286a79f4db1..fb79949783e3 100644
--- a/drivers/gpu/drm/i915/intel_atomic.c
+++ b/drivers/gpu/drm/i915/intel_atomic.c
@@ -386,21 +386,17 @@ int intel_atomic_setup_scalers(struct drm_device *dev,
 	return 0;
 }
 
-static void intel_atomic_duplicate_dpll_state(struct drm_device *dev,
-					      struct intel_atomic_state *state)
+void
+intel_atomic_duplicate_dpll_state(struct drm_i915_private *dev_priv,
+				  struct intel_shared_dpll_config *shared_dpll)
 {
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_shared_dpll *pll;
 	enum intel_dpll_id i;
 
-	state->dpll_set = true;
-
 	/* Copy shared dpll state */
 	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
-		pll = &dev_priv->shared_dplls[i];
+		struct intel_shared_dpll *pll = &dev_priv->shared_dplls[i];
 
-		memcpy(&state->shared_dpll[i],
-		       &pll->config, sizeof(pll->config));
+		shared_dpll[i] = pll->config;
 	}
 }
 
@@ -409,8 +405,12 @@ intel_atomic_get_shared_dpll_state(struct drm_atomic_state *s)
 {
 	struct intel_atomic_state *state = to_intel_atomic_state(s);
 
-	if (!state->dpll_set)
-		intel_atomic_duplicate_dpll_state(state->base.dev, state);
+	if (!state->dpll_set) {
+		state->dpll_set = true;
+
+		intel_atomic_duplicate_dpll_state(to_i915(state->base.dev),
+						  state->shared_dpll);
+	}
 
 	return state->shared_dpll;
 }
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 9a19d88cb5ce..386d9599d918 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -14685,111 +14685,216 @@ static bool primary_get_hw_state(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 
-	if (!crtc->base.state->active)
+	if (!crtc->base.enabled)
 		return false;
 
 	return I915_READ(DSPCNTR(crtc->plane)) & DISPLAY_PLANE_ENABLE;
 }
 
-static void intel_modeset_readout_hw_state(struct drm_device *dev,
-					   unsigned *crtc_mask)
+static int readout_hw_crtc_state(struct drm_atomic_state *state,
+				 struct intel_crtc *crtc)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum pipe pipe;
-	struct intel_crtc *crtc;
-	struct intel_encoder *encoder;
-	struct intel_connector *connector;
-	int i;
+	struct drm_i915_private *dev_priv = to_i915(state->dev);
+	struct intel_crtc_state *crtc_state;
+	struct drm_plane *primary = crtc->base.primary;
+	struct drm_plane_state *drm_plane_state;
+	struct intel_plane_state *plane_state;
 
-	*crtc_mask = 0;
-	for_each_intel_crtc(dev, crtc) {
-		struct drm_plane *primary = crtc->base.primary;
-		struct intel_plane_state *plane_state;
-		struct intel_crtc_state *pipe_config =
-			to_intel_crtc_state(crtc->base.state);
+	crtc_state = intel_atomic_get_crtc_state(state, crtc);
+	if (IS_ERR(crtc_state))
+		return PTR_ERR(crtc_state);
 
-		memset(pipe_config, 0, sizeof(*pipe_config));
+	memset(crtc_state, 0, sizeof(*crtc_state));
+	crtc_state->base.crtc = &crtc->base;
+	crtc_state->base.state = state;
 
-		pipe_config->quirks |= PIPE_CONFIG_QUIRK_INHERITED_MODE;
+	crtc_state->quirks |= PIPE_CONFIG_QUIRK_INHERITED_MODE;
 
-		if (crtc->base.state->active)
-			*crtc_mask |= drm_crtc_index(&crtc->base);
+	crtc_state->base.enable = crtc_state->base.active =
+	crtc->base.enabled = dev_priv->display.get_pipe_config(crtc, crtc_state);
 
-		crtc->base.enabled = pipe_config->base.enable =
-		pipe_config->base.active =
-			dev_priv->display.get_pipe_config(crtc, pipe_config);
+	drm_plane_state = drm_atomic_get_plane_state(state, primary);
+	if (IS_ERR(drm_plane_state))
+		return PTR_ERR(drm_plane_state);
 
-		plane_state = to_intel_plane_state(primary->state);
-		plane_state->hw_enabled = plane_state->visible =
-			primary_get_hw_state(crtc);
-		if (plane_state->visible)
-			crtc->base.state->plane_mask |=
-				1 << drm_plane_index(primary);
-		else
-			crtc->base.state->plane_mask &=
-				~(1 << drm_plane_index(primary));
+	plane_state = to_intel_plane_state(drm_plane_state);
+	plane_state->hw_enabled = plane_state->visible =
+		primary_get_hw_state(crtc);
 
-		DRM_DEBUG_KMS("[CRTC:%d] hw state readout: %s\n",
-			      crtc->base.base.id,
-			      crtc->base.state->active ? "enabled" : "disabled");
-	}
+	if (plane_state->visible)
+		crtc_state->base.plane_mask |= 1 << drm_plane_index(primary);
+	else
+		crtc_state->base.plane_mask &= ~(1 << drm_plane_index(primary));
+
+	DRM_DEBUG_KMS("[CRTC:%d] hw state readout: %s\n",
+		      crtc->base.base.id,
+		      crtc_state->base.active ? "enabled" : "disabled");
+
+	return 0;
+}
 
+static int readout_hw_pll_state(struct drm_atomic_state *state)
+{
+	struct drm_i915_private *dev_priv = to_i915(state->dev);
+	struct intel_shared_dpll_config *shared_dpll;
+	struct intel_crtc *crtc;
+	struct intel_crtc_state *crtc_state;
+	int i;
+
+	shared_dpll = intel_atomic_get_shared_dpll_state(state);
 	for (i = 0; i < dev_priv->num_shared_dpll; i++) {
 		struct intel_shared_dpll *pll = &dev_priv->shared_dplls[i];
 
 		pll->on = pll->get_hw_state(dev_priv, pll,
-					    &pll->config.hw_state);
+					    &shared_dpll[i].hw_state);
+
 		pll->active = 0;
-		pll->config.crtc_mask = 0;
-		for_each_intel_crtc(dev, crtc) {
-			if (crtc->base.state->active &&
-			    i == to_intel_crtc_state(crtc->base.state)->shared_dpll) {
+		shared_dpll[i].crtc_mask = 0;
+
+		for_each_intel_crtc(state->dev, crtc) {
+			crtc_state = intel_atomic_get_crtc_state(state, crtc);
+			if (IS_ERR(crtc_state))
+				return PTR_ERR(crtc_state);
+
+			if (crtc_state->base.active &&
+			    crtc_state->shared_dpll == i) {
 				pll->active++;
-				pll->config.crtc_mask |= 1 << crtc->pipe;
+				shared_dpll[i].crtc_mask |=
+					1 << crtc->pipe;
 			}
 		}
 
 		DRM_DEBUG_KMS("%s hw state readout: crtc_mask 0x%08x, on %i\n",
-			      pll->name, pll->config.crtc_mask, pll->on);
+			      pll->name, shared_dpll[i].crtc_mask,
+			      pll->on);
 
-		if (pll->config.crtc_mask)
+		if (shared_dpll[i].crtc_mask)
 			intel_display_power_get(dev_priv, POWER_DOMAIN_PLLS);
 	}
 
-	for_each_intel_encoder(dev, encoder) {
-		pipe = 0;
+	return 0;
+}
 
-		if (encoder->get_hw_state(encoder, &pipe)) {
-			crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-			encoder->base.crtc = &crtc->base;
-			encoder->get_config(encoder,
-				to_intel_crtc_state(crtc->base.state));
-		} else {
-			encoder->base.crtc = NULL;
-		}
+static struct drm_connector_state *
+get_connector_state_for_encoder(struct drm_atomic_state *state,
+				struct intel_encoder *encoder)
+{
+	struct drm_connector *connector;
+	struct drm_connector_state *connector_state;
+	int i;
 
-		encoder->connectors_active = false;
-		DRM_DEBUG_KMS("[ENCODER:%d:%s] hw state readout: %s, pipe %c\n",
-			      encoder->base.base.id,
-			      encoder->base.name,
-			      encoder->base.crtc ? "enabled" : "disabled",
-			      pipe_name(pipe));
-	}
+	for_each_connector_in_state(state, connector, connector_state, i)
+		if (connector_state->best_encoder == &encoder->base)
+			return connector_state;
+
+	return NULL;
+}
+
+static int readout_hw_connector_encoder_state(struct drm_atomic_state *state)
+{
+	struct drm_device *dev = state->dev;
+	struct drm_i915_private *dev_priv = to_i915(state->dev);
+	struct intel_crtc *crtc;
+	struct drm_crtc_state *drm_crtc_state;
+	struct intel_crtc_state *crtc_state;
+	struct intel_encoder *encoder;
+	struct intel_connector *connector;
+	struct drm_connector_state *connector_state;
+	enum pipe pipe;
 
 	for_each_intel_connector(dev, connector) {
+		connector_state =
+			drm_atomic_get_connector_state(state, &connector->base);
+		if (IS_ERR(connector_state))
+			return PTR_ERR(connector_state);
+
 		if (connector->get_hw_state(connector)) {
 			connector->base.dpms = DRM_MODE_DPMS_ON;
-			connector->encoder->connectors_active = true;
 			connector->base.encoder = &connector->encoder->base;
 		} else {
 			connector->base.dpms = DRM_MODE_DPMS_OFF;
 			connector->base.encoder = NULL;
 		}
+
+		/* We'll update the crtc field when reading encoder state */
+		connector_state->crtc = NULL;
+
+		connector_state->best_encoder = connector->base.encoder;
+
 		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] hw state readout: %s\n",
 			      connector->base.base.id,
 			      connector->base.name,
 			      connector->base.encoder ? "enabled" : "disabled");
 	}
+
+	for_each_intel_encoder(dev, encoder) {
+		pipe = 0;
+
+		connector_state =
+			get_connector_state_for_encoder(state, encoder);
+
+		encoder->connectors_active = !!connector_state;
+
+		if (encoder->get_hw_state(encoder, &pipe)) {
+			encoder->base.crtc =
+				dev_priv->pipe_to_crtc_mapping[pipe];
+			crtc = to_intel_crtc(encoder->base.crtc);
+
+			drm_crtc_state =
+				state->crtc_states[drm_crtc_index(&crtc->base)];
+			crtc_state = to_intel_crtc_state(drm_crtc_state);
+
+			encoder->get_config(encoder, crtc_state);
+
+			if (connector_state)
+				connector_state->crtc = &crtc->base;
+		} else {
+			encoder->base.crtc = NULL;
+		}
+
+		DRM_DEBUG_KMS("[ENCODER:%d:%s] hw state readout: %s, pipe %c\n",
+			      encoder->base.base.id,
+			      encoder->base.name,
+			      encoder->base.crtc ? "enabled" : "disabled",
+			      pipe_name(pipe));
+	}
+
+	return 0;
+}
+
+static struct drm_atomic_state *
+intel_modeset_readout_hw_state(struct drm_device *dev)
+{
+	struct intel_crtc *crtc;
+	int ret = 0;
+
+	struct drm_atomic_state *state;
+
+	state = drm_atomic_state_alloc(dev);
+	if (!state)
+		return ERR_PTR(-ENOMEM);
+
+	state->acquire_ctx = dev->mode_config.acquire_ctx;
+
+	for_each_intel_crtc(dev, crtc) {
+		ret = readout_hw_crtc_state(state, crtc);
+		if (ret)
+			goto err_free;
+	}
+
+	ret = readout_hw_pll_state(state);
+	if (ret)
+		goto err_free;
+
+	ret = readout_hw_connector_encoder_state(state);
+	if (ret)
+		goto err_free;
+
+	return state;
+
+err_free:
+	drm_atomic_state_free(state);
+	return ERR_PTR(ret);
 }
 
 /* Scan out the current hw modeset state, sanitizes it and maps it into the drm
@@ -14798,43 +14903,61 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 				  bool force_restore)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum pipe pipe;
-	struct intel_crtc *crtc;
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *crtc_state;
 	struct intel_encoder *encoder;
+	struct drm_atomic_state *state;
+	struct intel_shared_dpll_config shared_dplls[I915_NUM_PLLS];
 	int i;
 
-	unsigned crtc_mask;
+	state = intel_modeset_readout_hw_state(dev);
+	if (IS_ERR(state)) {
+		DRM_ERROR("Failed to read out hw state\n");
+		return;
+	}
+
+	drm_atomic_helper_swap_state(dev, state);
 
-	intel_modeset_readout_hw_state(dev, &crtc_mask);
+	/* swap sw/hw dpll state */
+	BUILD_BUG_ON(sizeof(shared_dplls) !=
+		     sizeof(to_intel_atomic_state(state)->shared_dpll));
 
-	/*
-	 * Now that we have the config, copy it to each CRTC struct
-	 * Note that this could go away if we move to using crtc_config
-	 * checking everywhere.
-	 */
-	for_each_intel_crtc(dev, crtc) {
-		if (crtc->base.state->active && i915.fastboot) {
-			intel_mode_from_pipe_config(&crtc->base.mode,
-				to_intel_crtc_state(crtc->base.state));
-			DRM_DEBUG_KMS("[CRTC:%d] found active mode: ",
-				      crtc->base.base.id);
-			drm_mode_debug_printmodeline(&crtc->base.mode);
-		}
-	}
+	intel_atomic_duplicate_dpll_state(dev_priv, shared_dplls);
+	intel_shared_dpll_commit(state);
+	memcpy(to_intel_atomic_state(state)->shared_dpll,
+	       shared_dplls, sizeof(*shared_dplls) * dev_priv->num_shared_dpll);
 
 	/* HW state is read out, now we need to sanitize this mess. */
 	for_each_intel_encoder(dev, encoder) {
 		intel_sanitize_encoder(encoder);
 	}
 
-	for_each_pipe(dev_priv, pipe) {
-		struct intel_crtc_state *pipe_config;
+	for_each_crtc_in_state(state, crtc, crtc_state, i) {
+		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+		/* prevent unnneeded restores with force_restore */
+		crtc_state->active_changed =
+		crtc_state->mode_changed =
+		crtc_state->planes_changed = false;
+
+		if (crtc->enabled) {
+			intel_mode_from_pipe_config(&crtc->state->mode,
+				to_intel_crtc_state(crtc->state));
+
+			drm_mode_copy(&crtc->mode, &crtc->state->mode);
+			drm_mode_copy(&crtc->hwmode,
+				      &crtc->state->adjusted_mode);
+		}
 
-		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-		pipe_config = to_intel_crtc_state(crtc->base.state);
+		intel_sanitize_crtc(intel_crtc,
+				    to_intel_crtc_state(crtc->state));
 
-		intel_sanitize_crtc(crtc, pipe_config);
-		intel_dump_pipe_config(crtc, pipe_config,
+		/*
+		 * sanitize_crtc may have forced an update of crtc->state,
+		 * so reload in intel_dump_pipe_config
+		 */
+		intel_dump_pipe_config(intel_crtc,
+				       to_intel_crtc_state(crtc->state),
 				       "[setup_hw_state]");
 	}
 
@@ -14858,19 +14981,17 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 		ilk_wm_get_hw_state(dev);
 
 	if (force_restore) {
-		i915_redisable_vga(dev);
+		int ret;
 
-		/*
-		 * We need to use raw interfaces for restoring state to avoid
-		 * checking (bogus) intermediate states.
-		 */
-		for_each_pipe(dev_priv, pipe) {
-			struct drm_crtc *crtc =
-				dev_priv->pipe_to_crtc_mapping[pipe];
+		i915_redisable_vga(dev);
 
-			if (crtc_mask & (1 << drm_crtc_index(crtc)))
-				intel_crtc_restore_mode(crtc);
+		ret = intel_set_mode(state);
+		if (ret) {
+			DRM_ERROR("Failed to restore previous mode\n");
+			drm_atomic_state_free(state);
 		}
+	} else {
+		drm_atomic_state_free(state);
 	}
 
 	intel_modeset_check_state(dev);
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 4971a5a35db4..48520993c3ad 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -1396,10 +1396,13 @@ int intel_connector_atomic_get_property(struct drm_connector *connector,
 struct drm_crtc_state *intel_crtc_duplicate_state(struct drm_crtc *crtc);
 void intel_crtc_destroy_state(struct drm_crtc *crtc,
 			       struct drm_crtc_state *state);
+
 struct drm_atomic_state *intel_atomic_new_state(struct drm_device *dev);
 void intel_atomic_clear_state(struct drm_atomic_state *);
 struct intel_shared_dpll_config *
 intel_atomic_get_shared_dpll_state(struct drm_atomic_state *s);
+void intel_atomic_duplicate_dpll_state(struct drm_i915_private *,
+				       struct intel_shared_dpll_config *);
 
 static inline struct intel_crtc_state *
 intel_atomic_get_crtc_state(struct drm_atomic_state *state,
