4 files changed, 181 insertions, 20 deletions

diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index 4a5b099c3bc5..eefcef6fa32f 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -202,13 +202,12 @@ static inline void CDV_MSG_WRITE32(uint port, uint offset, u32 value)
         pci_dev_put(pci_root);
 }
 
-#define PSB_APM_CMD                     0x0
-#define PSB_APM_STS                     0x04
 #define PSB_PM_SSC                      0x20
 #define PSB_PM_SSS                      0x30
-#define PSB_PWRGT_GFX_MASK              0x3
-#define CDV_PWRGT_DISPLAY_CNTR          0x000fc00c
-#define CDV_PWRGT_DISPLAY_STS           0x000fc00c
+#define PSB_PWRGT_GFX_ON                0x02
+#define PSB_PWRGT_GFX_OFF               0x01
+#define PSB_PWRGT_GFX_D0                0x00
+#define PSB_PWRGT_GFX_D3                0x03
 
 static void cdv_init_pm(struct drm_device *dev)
 {
@@ -221,26 +220,22 @@ static void cdv_init_pm(struct drm_device *dev)
         dev_priv->ospm_base = CDV_MSG_READ32(PSB_PUNIT_PORT,
                                                         PSB_OSPMBA) & 0xFFFF;
 
-        /* Force power on for now */
+        /* Power status */
         pwr_cnt = inl(dev_priv->apm_base + PSB_APM_CMD);
-        pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
 
+        /* Enable the GPU */
+        pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
+        pwr_cnt |= PSB_PWRGT_GFX_ON;
         outl(pwr_cnt, dev_priv->apm_base + PSB_APM_CMD);
+
+        /* Wait for the GPU power */
         for (i = 0; i < 5; i++) {
                 u32 pwr_sts = inl(dev_priv->apm_base + PSB_APM_STS);
                 if ((pwr_sts & PSB_PWRGT_GFX_MASK) == 0)
-                        break;
-                udelay(10);
-        }
-        pwr_cnt = inl(dev_priv->ospm_base + PSB_PM_SSC);
-        pwr_cnt &= ~CDV_PWRGT_DISPLAY_CNTR;
-        outl(pwr_cnt, dev_priv->ospm_base + PSB_PM_SSC);
-        for (i = 0; i < 5; i++) {
-                u32 pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
-                if ((pwr_sts & CDV_PWRGT_DISPLAY_STS) == 0)
-                        break;
+                        return;
                 udelay(10);
         }
+        dev_err(dev->dev, "GPU: power management timed out.\n");
 }
 
 /**
@@ -249,11 +244,50 @@ static void cdv_init_pm(struct drm_device *dev)
  *
  *      Save the state we need in order to be able to restore the interface
  *      upon resume from suspend
- *
- *      FIXME: review
  */
 static int cdv_save_display_registers(struct drm_device *dev)
 {
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_save_area *regs = &dev_priv->regs;
+        struct drm_connector *connector;
+
+        dev_info(dev->dev, "Saving GPU registers.\n");
+
+        pci_read_config_byte(dev->pdev, 0xF4, &regs->cdv.saveLBB);
+
+        regs->cdv.saveDSPCLK_GATE_D = REG_READ(DSPCLK_GATE_D);
+        regs->cdv.saveRAMCLK_GATE_D = REG_READ(RAMCLK_GATE_D);
+
+        regs->cdv.saveDSPARB = REG_READ(DSPARB);
+        regs->cdv.saveDSPFW[0] = REG_READ(DSPFW1);
+        regs->cdv.saveDSPFW[1] = REG_READ(DSPFW2);
+        regs->cdv.saveDSPFW[2] = REG_READ(DSPFW3);
+        regs->cdv.saveDSPFW[3] = REG_READ(DSPFW4);
+        regs->cdv.saveDSPFW[4] = REG_READ(DSPFW5);
+        regs->cdv.saveDSPFW[5] = REG_READ(DSPFW6);
+
+        regs->cdv.saveADPA = REG_READ(ADPA);
+
+        regs->cdv.savePP_CONTROL = REG_READ(PP_CONTROL);
+        regs->cdv.savePFIT_PGM_RATIOS = REG_READ(PFIT_PGM_RATIOS);
+        regs->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+        regs->saveBLC_PWM_CTL2 = REG_READ(BLC_PWM_CTL2);
+        regs->cdv.saveLVDS = REG_READ(LVDS);
+
+        regs->cdv.savePFIT_CONTROL = REG_READ(PFIT_CONTROL);
+
+        regs->cdv.savePP_ON_DELAYS = REG_READ(PP_ON_DELAYS);
+        regs->cdv.savePP_OFF_DELAYS = REG_READ(PP_OFF_DELAYS);
+        regs->cdv.savePP_CYCLE = REG_READ(PP_CYCLE);
+
+        regs->cdv.saveVGACNTRL = REG_READ(VGACNTRL);
+
+        regs->cdv.saveIER = REG_READ(PSB_INT_ENABLE_R);
+        regs->cdv.saveIMR = REG_READ(PSB_INT_MASK_R);
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+                connector->funcs->dpms(connector, DRM_MODE_DPMS_OFF);
+
         return 0;
 }
 
@@ -267,16 +301,113 @@ static int cdv_save_display_registers(struct drm_device *dev)
  */
 static int cdv_restore_display_registers(struct drm_device *dev)
 {
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_save_area *regs = &dev_priv->regs;
+        struct drm_connector *connector;
+        u32 temp;
+
+        pci_write_config_byte(dev->pdev, 0xF4, regs->cdv.saveLBB);
+
+        REG_WRITE(DSPCLK_GATE_D, regs->cdv.saveDSPCLK_GATE_D);
+        REG_WRITE(RAMCLK_GATE_D, regs->cdv.saveRAMCLK_GATE_D);
+
+        /* BIOS does below anyway */
+        REG_WRITE(DPIO_CFG, 0);
+        REG_WRITE(DPIO_CFG, DPIO_MODE_SELECT_0 | DPIO_CMN_RESET_N);
+
+        temp = REG_READ(DPLL_A);
+        if ((temp & DPLL_SYNCLOCK_ENABLE) == 0) {
+                REG_WRITE(DPLL_A, temp | DPLL_SYNCLOCK_ENABLE);
+                REG_READ(DPLL_A);
+        }
+
+        temp = REG_READ(DPLL_B);
+        if ((temp & DPLL_SYNCLOCK_ENABLE) == 0) {
+                REG_WRITE(DPLL_B, temp | DPLL_SYNCLOCK_ENABLE);
+                REG_READ(DPLL_B);
+        }
+
+        udelay(500);
+
+        REG_WRITE(DSPFW1, regs->cdv.saveDSPFW[0]);
+        REG_WRITE(DSPFW2, regs->cdv.saveDSPFW[1]);
+        REG_WRITE(DSPFW3, regs->cdv.saveDSPFW[2]);
+        REG_WRITE(DSPFW4, regs->cdv.saveDSPFW[3]);
+        REG_WRITE(DSPFW5, regs->cdv.saveDSPFW[4]);
+        REG_WRITE(DSPFW6, regs->cdv.saveDSPFW[5]);
+
+        REG_WRITE(DSPARB, regs->cdv.saveDSPARB);
+        REG_WRITE(ADPA, regs->cdv.saveADPA);
+
+        REG_WRITE(BLC_PWM_CTL2, regs->saveBLC_PWM_CTL2);
+        REG_WRITE(LVDS, regs->cdv.saveLVDS);
+        REG_WRITE(PFIT_CONTROL, regs->cdv.savePFIT_CONTROL);
+        REG_WRITE(PFIT_PGM_RATIOS, regs->cdv.savePFIT_PGM_RATIOS);
+        REG_WRITE(BLC_PWM_CTL, regs->saveBLC_PWM_CTL);
+        REG_WRITE(PP_ON_DELAYS, regs->cdv.savePP_ON_DELAYS);
+        REG_WRITE(PP_OFF_DELAYS, regs->cdv.savePP_OFF_DELAYS);
+        REG_WRITE(PP_CYCLE, regs->cdv.savePP_CYCLE);
+        REG_WRITE(PP_CONTROL, regs->cdv.savePP_CONTROL);
+
+        REG_WRITE(VGACNTRL, regs->cdv.saveVGACNTRL);
+
+        REG_WRITE(PSB_INT_ENABLE_R, regs->cdv.saveIER);
+        REG_WRITE(PSB_INT_MASK_R, regs->cdv.saveIMR);
+
+        /* Fix arbitration bug */
+        CDV_MSG_WRITE32(3, 0x30, 0x08027108);
+
+        drm_mode_config_reset(dev);
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+                connector->funcs->dpms(connector, DRM_MODE_DPMS_ON);
+
+        /* Resume the modeset for every activated CRTC */
+        drm_helper_resume_force_mode(dev);
         return 0;
 }
 
 static int cdv_power_down(struct drm_device *dev)
 {
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pwr_cnt, pwr_mask, pwr_sts;
+        int tries = 5;
+
+        pwr_cnt = inl(dev_priv->apm_base + PSB_APM_CMD);
+        pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
+        pwr_cnt |= PSB_PWRGT_GFX_OFF;
+        pwr_mask = PSB_PWRGT_GFX_MASK;
+
+        outl(pwr_cnt, dev_priv->apm_base + PSB_APM_CMD);
+
+        while (tries--) {
+                pwr_sts = inl(dev_priv->apm_base + PSB_APM_STS);
+                if ((pwr_sts & pwr_mask) == PSB_PWRGT_GFX_D3)
+                        return 0;
+                udelay(10);
+        }
         return 0;
 }
 
 static int cdv_power_up(struct drm_device *dev)
 {
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pwr_cnt, pwr_mask, pwr_sts;
+        int tries = 5;
+
+        pwr_cnt = inl(dev_priv->apm_base + PSB_APM_CMD);
+        pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
+        pwr_cnt |= PSB_PWRGT_GFX_ON;
+        pwr_mask = PSB_PWRGT_GFX_MASK;
+
+        outl(pwr_cnt, dev_priv->apm_base + PSB_APM_CMD);
+
+        while (tries--) {
+                pwr_sts = inl(dev_priv->apm_base + PSB_APM_STS);
+                if ((pwr_sts & pwr_mask) == PSB_PWRGT_GFX_D0)
+                        return 0;
+                udelay(10);
+        }
         return 0;
 }
 
diff --git a/drivers/gpu/drm/gma500/power.c b/drivers/gpu/drm/gma500/power.c
index 8d23c45b5bc6..889b854751da 100644
--- a/drivers/gpu/drm/gma500/power.c
+++ b/drivers/gpu/drm/gma500/power.c
@@ -193,6 +193,7 @@ int gma_power_suspend(struct device *_dev)
         if (!dev_priv->suspended) {
                 if (dev_priv->display_count) {
                         mutex_unlock(&power_mutex);
+                        dev_err(dev->dev, "GPU hardware busy, cannot suspend\n");
                         return -EBUSY;
                 }
                 psb_irq_uninstall(dev);
@@ -300,7 +301,7 @@ int psb_runtime_suspend(struct device *dev)
 
 int psb_runtime_resume(struct device *dev)
 {
-        return gma_power_resume(dev);;
+        return gma_power_resume(dev);
 }
 
 int psb_runtime_idle(struct device *dev)
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index af1c99752000..40ce2c9bc2e4 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -456,12 +456,32 @@ struct medfield_state {
         uint32_t saveHDMIB_CONTROL;
 };
 
+struct cdv_state {
+        uint32_t saveDSPCLK_GATE_D;
+        uint32_t saveRAMCLK_GATE_D;
+        uint32_t saveDSPARB;
+        uint32_t saveDSPFW[6];
+        uint32_t saveADPA;
+        uint32_t savePP_CONTROL;
+        uint32_t savePFIT_PGM_RATIOS;
+        uint32_t saveLVDS;
+        uint32_t savePFIT_CONTROL;
+        uint32_t savePP_ON_DELAYS;
+        uint32_t savePP_OFF_DELAYS;
+        uint32_t savePP_CYCLE;
+        uint32_t saveVGACNTRL;
+        uint32_t saveIER;
+        uint32_t saveIMR;
+        u8       saveLBB;
+};
+
 struct psb_save_area {
         uint32_t saveBSM;
         uint32_t saveVBT;
         union {
                 struct psb_state psb;
                 struct medfield_state mdfld;
+                struct cdv_state cdv;
         };
         uint32_t saveBLC_PWM_CTL2;
         uint32_t saveBLC_PWM_CTL;
diff --git a/drivers/gpu/drm/gma500/psb_intel_reg.h b/drivers/gpu/drm/gma500/psb_intel_reg.h
index fcc0af03d685..e89d3a2e8fdc 100644
--- a/drivers/gpu/drm/gma500/psb_intel_reg.h
+++ b/drivers/gpu/drm/gma500/psb_intel_reg.h
@@ -177,6 +177,9 @@
 #define LVDSPP_OFF              0x6120c
 #define PP_CYCLE                0x61210
 
+#define PP_ON_DELAYS            0x61208         /* Cedartrail */
+#define PP_OFF_DELAYS           0x6120c         /* Cedartrail */
+
 #define PFIT_CONTROL            0x61230
 #define PFIT_ENABLE                     (1 << 31)
 #define PFIT_PIPE_MASK                  (3 << 29)
@@ -1252,6 +1255,12 @@ No status bits are changed.
 # define SB_BYTE_ENABLE_SHIFT                   4
 # define SB_BUSY                                (1 << 0)
 
+#define DSPCLK_GATE_D           0x6200
+# define VRHUNIT_CLOCK_GATE_DISABLE             (1 << 28) /* Fixed value on CDV */
+# define DPOUNIT_CLOCK_GATE_DISABLE             (1 << 11)
+# define DPIOUNIT_CLOCK_GATE_DISABLE            (1 << 6)
+
+#define RAMCLK_GATE_D           0x6210
 
 /* 32-bit value read/written from the DPIO reg. */
 #define SB_DATA         0x02104 /* cedarview */