4 files changed, 198 insertions, 0 deletions

diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index a65312c95bd3..03228cb65518 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -1887,6 +1887,7 @@ static struct radeon_asic si_asic = {
                 .get_pcie_lanes = NULL,
                 .set_pcie_lanes = NULL,
                 .set_clock_gating = NULL,
+                .set_uvd_clocks = &si_set_uvd_clocks,
         },
         .pflip = {
                 .pre_page_flip = &evergreen_pre_page_flip,
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 54a7ef70805d..365c964399a5 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -545,5 +545,6 @@ int si_copy_dma(struct radeon_device *rdev,
 void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
 u32 si_get_xclk(struct radeon_device *rdev);
 uint64_t si_get_gpu_clock_counter(struct radeon_device *rdev);
+int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
 
 #endif
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 3e9782dc35bf..465053d461bb 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -4627,3 +4627,170 @@ uint64_t si_get_gpu_clock_counter(struct radeon_device *rdev)
         mutex_unlock(&rdev->gpu_clock_mutex);
         return clock;
 }
+
+static int si_uvd_calc_post_div(unsigned target_freq,
+                                unsigned vco_freq,
+                                unsigned *div)
+{
+        /* target larger than vco frequency ? */
+        if (vco_freq < target_freq)
+                return -1; /* forget it */
+
+        /* Fclk = Fvco / PDIV */
+        *div = vco_freq / target_freq;
+
+        /* we alway need a frequency less than or equal the target */
+        if ((vco_freq / *div) > target_freq)
+                *div += 1;
+
+        /* dividers above 5 must be even */
+        if (*div > 5 && *div % 2)
+                *div += 1;
+
+        /* out of range ? */
+        if (*div >= 128)
+                return -1; /* forget it */
+
+        return vco_freq / *div;
+}
+
+static int si_uvd_send_upll_ctlreq(struct radeon_device *rdev)
+{
+        unsigned i;
+
+        /* assert UPLL_CTLREQ */
+        WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_CTLREQ_MASK, ~UPLL_CTLREQ_MASK);
+
+        /* wait for CTLACK and CTLACK2 to get asserted */
+        for (i = 0; i < 100; ++i) {
+                uint32_t mask = UPLL_CTLACK_MASK | UPLL_CTLACK2_MASK;
+                if ((RREG32(CG_UPLL_FUNC_CNTL) & mask) == mask)
+                        break;
+                mdelay(10);
+        }
+        if (i == 100)
+                return -ETIMEDOUT;
+
+        /* deassert UPLL_CTLREQ */
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_CTLREQ_MASK);
+
+        return 0;
+}
+
+int si_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
+{
+        /* start off with something large */
+        int optimal_diff_score = 0x7FFFFFF;
+        unsigned optimal_fb_div = 0, optimal_vclk_div = 0;
+        unsigned optimal_dclk_div = 0, optimal_vco_freq = 0;
+        unsigned vco_freq;
+        int r;
+
+        /* loop through vco from low to high */
+        for (vco_freq = 125000; vco_freq <= 250000; vco_freq += 100) {
+                unsigned fb_div = vco_freq / rdev->clock.spll.reference_freq * 16384;
+                int calc_clk, diff_score, diff_vclk, diff_dclk;
+                unsigned vclk_div, dclk_div;
+
+                /* fb div out of range ? */
+                if (fb_div > 0x03FFFFFF)
+                        break; /* it can oly get worse */
+
+                /* calc vclk with current vco freq. */
+                calc_clk = si_uvd_calc_post_div(vclk, vco_freq, &vclk_div);
+                if (calc_clk == -1)
+                        break; /* vco is too big, it has to stop. */
+                diff_vclk = vclk - calc_clk;
+
+                /* calc dclk with current vco freq. */
+                calc_clk = si_uvd_calc_post_div(dclk, vco_freq, &dclk_div);
+                if (calc_clk == -1)
+                        break; /* vco is too big, it has to stop. */
+                diff_dclk = dclk - calc_clk;
+
+                /* determine if this vco setting is better than current optimal settings */
+                diff_score = abs(diff_vclk) + abs(diff_dclk);
+                if (diff_score < optimal_diff_score) {
+                        optimal_fb_div = fb_div;
+                        optimal_vclk_div = vclk_div;
+                        optimal_dclk_div = dclk_div;
+                        optimal_vco_freq = vco_freq;
+                        optimal_diff_score = diff_score;
+                        if (optimal_diff_score == 0)
+                                break; /* it can't get better than this */
+                }
+        }
+
+        /* set RESET_ANTI_MUX to 0 */
+        WREG32_P(CG_UPLL_FUNC_CNTL_5, 0, ~RESET_ANTI_MUX_MASK);
+
+        /* set VCO_MODE to 1 */
+        WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_VCO_MODE_MASK, ~UPLL_VCO_MODE_MASK);
+
+        /* toggle UPLL_SLEEP to 1 then back to 0 */
+        WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_SLEEP_MASK);
+
+        /* deassert UPLL_RESET */
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+        mdelay(1);
+
+        /* bypass vclk and dclk with bclk */
+        WREG32_P(CG_UPLL_FUNC_CNTL_2,
+                VCLK_SRC_SEL(1) | DCLK_SRC_SEL(1),
+                ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+        /* put PLL in bypass mode */
+        WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~UPLL_BYPASS_EN_MASK);
+
+        r = si_uvd_send_upll_ctlreq(rdev);
+        if (r)
+                return r;
+
+        /* assert UPLL_RESET again */
+        WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_RESET_MASK, ~UPLL_RESET_MASK);
+
+        /* disable spread spectrum. */
+        WREG32_P(CG_UPLL_SPREAD_SPECTRUM, 0, ~SSEN_MASK);
+
+        /* set feedback divider */
+        WREG32_P(CG_UPLL_FUNC_CNTL_3, UPLL_FB_DIV(optimal_fb_div), ~UPLL_FB_DIV_MASK);
+
+        /* set ref divider to 0 */
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_REF_DIV_MASK);
+
+        if (optimal_vco_freq < 187500)
+                WREG32_P(CG_UPLL_FUNC_CNTL_4, 0, ~UPLL_SPARE_ISPARE9);
+        else
+                WREG32_P(CG_UPLL_FUNC_CNTL_4, UPLL_SPARE_ISPARE9, ~UPLL_SPARE_ISPARE9);
+
+        /* set PDIV_A and PDIV_B */
+        WREG32_P(CG_UPLL_FUNC_CNTL_2,
+                UPLL_PDIV_A(optimal_vclk_div) | UPLL_PDIV_B(optimal_dclk_div),
+                ~(UPLL_PDIV_A_MASK | UPLL_PDIV_B_MASK));
+
+        /* give the PLL some time to settle */
+        mdelay(15);
+
+        /* deassert PLL_RESET */
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+        mdelay(15);
+
+        /* switch from bypass mode to normal mode */
+        WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_BYPASS_EN_MASK);
+
+        r = si_uvd_send_upll_ctlreq(rdev);
+        if (r)
+                return r;
+
+        /* switch VCLK and DCLK selection */
+        WREG32_P(CG_UPLL_FUNC_CNTL_2,
+                VCLK_SRC_SEL(2) | DCLK_SRC_SEL(2),
+                ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+        mdelay(100);
+
+        return 0;
+}
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 1fb8ee2c45dc..3a685855c3d8 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -29,6 +29,35 @@
 #define TAHITI_GB_ADDR_CONFIG_GOLDEN        0x12011003
 #define VERDE_GB_ADDR_CONFIG_GOLDEN         0x12010002
 
+/* discrete uvd clocks */
+#define CG_UPLL_FUNC_CNTL                               0x634
+#       define UPLL_RESET_MASK                          0x00000001
+#       define UPLL_SLEEP_MASK                          0x00000002
+#       define UPLL_BYPASS_EN_MASK                      0x00000004
+#       define UPLL_CTLREQ_MASK                         0x00000008
+#       define UPLL_VCO_MODE_MASK                       0x00000600
+#       define UPLL_REF_DIV_MASK                        0x001F0000
+#       define UPLL_CTLACK_MASK                         0x40000000
+#       define UPLL_CTLACK2_MASK                        0x80000000
+#define CG_UPLL_FUNC_CNTL_2                             0x638
+#       define UPLL_PDIV_A(x)                           ((x) << 0)
+#       define UPLL_PDIV_A_MASK                         0x0000007F
+#       define UPLL_PDIV_B(x)                           ((x) << 8)
+#       define UPLL_PDIV_B_MASK                         0x00007F00
+#       define VCLK_SRC_SEL(x)                          ((x) << 20)
+#       define VCLK_SRC_SEL_MASK                        0x01F00000
+#       define DCLK_SRC_SEL(x)                          ((x) << 25)
+#       define DCLK_SRC_SEL_MASK                        0x3E000000
+#define CG_UPLL_FUNC_CNTL_3                             0x63C
+#       define UPLL_FB_DIV(x)                           ((x) << 0)
+#       define UPLL_FB_DIV_MASK                         0x01FFFFFF
+#define CG_UPLL_FUNC_CNTL_4                             0x644
+#       define UPLL_SPARE_ISPARE9                       0x00020000
+#define CG_UPLL_FUNC_CNTL_5                             0x648
+#       define RESET_ANTI_MUX_MASK                      0x00000200
+#define CG_UPLL_SPREAD_SPECTRUM                         0x650
+#       define SSEN_MASK                                0x00000001
+
 #define CG_MULT_THERMAL_STATUS                                  0x714
 #define         ASIC_MAX_TEMP(x)                                ((x) << 0)
 #define         ASIC_MAX_TEMP_MASK                              0x000001ff