/* SBI access */
  static void
 -intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value)
 +intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value,
 +              enum intel_sbi_destination destination)
  {
-       unsigned long flags;
 +      u32 tmp;
 +
-       spin_lock_irqsave(&dev_priv->dpio_lock, flags);
-       if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0, 100)) {
+       WARN_ON(!mutex_is_locked(&dev_priv->dpio_lock));
+ 
+       if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0,
+                               100)) {
                DRM_ERROR("timeout waiting for SBI to become ready\n");
-               goto out_unlock;
+               return;
        }
  
 -      I915_WRITE(SBI_ADDR,
 -                      (reg << 16));
 -      I915_WRITE(SBI_DATA,
 -                      value);
 -      I915_WRITE(SBI_CTL_STAT,
 -                      SBI_BUSY |
 -                      SBI_CTL_OP_CRWR);
 +      I915_WRITE(SBI_ADDR, (reg << 16));
 +      I915_WRITE(SBI_DATA, value);
 +
 +      if (destination == SBI_ICLK)
 +              tmp = SBI_CTL_DEST_ICLK | SBI_CTL_OP_CRWR;
 +      else
 +              tmp = SBI_CTL_DEST_MPHY | SBI_CTL_OP_IOWR;
 +      I915_WRITE(SBI_CTL_STAT, SBI_BUSY | tmp);
  
        if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_BUSY | SBI_RESPONSE_FAIL)) == 0,
                                100)) {
  }
  
  static u32
 -intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg)
 +intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg,
 +             enum intel_sbi_destination destination)
  {
-       unsigned long flags;
 +      u32 value = 0;
+       WARN_ON(!mutex_is_locked(&dev_priv->dpio_lock));
  
-       spin_lock_irqsave(&dev_priv->dpio_lock, flags);
-       if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0, 100)) {
+       if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0,
+                               100)) {
                DRM_ERROR("timeout waiting for SBI to become ready\n");
-               goto out_unlock;
+               return 0;
        }
  
 -      I915_WRITE(SBI_ADDR,
 -                      (reg << 16));
 -      I915_WRITE(SBI_CTL_STAT,
 -                      SBI_BUSY |
 -                      SBI_CTL_OP_CRRD);
 +      I915_WRITE(SBI_ADDR, (reg << 16));
 +
 +      if (destination == SBI_ICLK)
 +              value = SBI_CTL_DEST_ICLK | SBI_CTL_OP_CRRD;
 +      else
 +              value = SBI_CTL_DEST_MPHY | SBI_CTL_OP_IORD;
 +      I915_WRITE(SBI_CTL_STAT, value | SBI_BUSY);
  
        if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_BUSY | SBI_RESPONSE_FAIL)) == 0,
                                100)) {
        }
  
        if (force_restore) {
 -              for_each_pipe(pipe)
 +              for_each_pipe(pipe) {
-                       crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-                       intel_set_mode(&crtc->base, &crtc->base.mode,
-                                      crtc->base.x, crtc->base.y, crtc->base.fb);
+                       intel_crtc_restore_mode(dev_priv->pipe_to_crtc_mapping[pipe]);
 +              }
 +
 +              i915_redisable_vga(dev);
        } else {
                intel_modeset_update_staged_output_state(dev);
        }