Change in libgfxinit[master]: gma hsw: Implement CDClk switching

[Nico Huber (Code Review)]

Hello Arthur Heymans, Matt DeVillier, Thomas Heijligen,

I'd like you to do a code review. Please visit

https://review.coreboot.org/c/libgfxinit/+/35717

to review the following change.

Change subject: gma hsw: Implement CDClk switching
......................................................................

gma hsw: Implement CDClk switching

For Haswell, we only read out the current CDClk setting and work with
that (CDClk is supposed to be set only once per boot, so we leave that
to the firmware). Broadwell supports actual frequency switching, with
the following limits:

o a fuse may restrict us to 450MHz,
  o ULX supports at most 450MHz w/o additional cooling,
  o ULT supports at most 540MHz w/o additional cooling,
  o others support up to 675MHz.

We assume there is no `additional cooling`.

Dot clocks, on these platforms, may run with full CDClk speed.

Change-Id: I8fb23a49b334f9f0045884c3af4087764397b941
Signed-off-by: Nico Huber 
---
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
M common/hw-gfx-gma-config.ads.template
M common/hw-gfx-gma-registers.ads
M common/hw-gfx.ads
5 files changed, 183 insertions(+), 2 deletions(-)

git pull ssh://review.coreboot.org:29418/libgfxinit refs/changes/17/35717/1

diff --git a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
index 332c3df..f372779 100644
--- a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
+++ b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
@@ -22,6 +22,29 @@

package body HW.GFX.GMA.Power_And_Clocks_Haswell is

+   LCPLL_CTL_CD_FREQ_SEL_MASK          : constant := 3 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_450_MHZ       : constant := 0 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_HSW_ALTERNATE : constant := 1 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ   : constant := 1 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ : constant := 2 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ   : constant := 3 * 2 ** 26;
+   LCPLL_CTL_CD_SOURCE_SELECT_FCLK     : constant := 1 * 2 ** 21;
+   LCPLL_CTL_CD_SOURCE_FCLK_DONE       : constant := 1 * 2 ** 19;
+
+   function LCPLL_CTL_CD_FREQ_SEL_BDW (CDClk : CDClk_Range) return Word32 is
+     (case CDClk is
+         when 675_000_000 => LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ,
+         when 540_000_000 => LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ,
+         when 450_000_000 => LCPLL_CTL_CD_FREQ_SEL_450_MHZ,
+         when others      => LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ);
+
+   FUSE_STRAP_DISPLAY_CDCLK_LIMIT      : constant := 1 * 2 ** 24;
+
+   HSW_PCODE_DE_WRITE_FREQ             : constant := 16#17#;
+   BDW_PCODE_DISPLAY_FREQ_CHANGE       : constant := 16#18#;
+
+   ----------------------------------------------------------------------------
+
    PWR_WELL_CTL_ENABLE_REQUEST   : constant := 1 * 2 ** 31;
    PWR_WELL_CTL_DISABLE_REQUEST  : constant := 0 * 2 ** 31;
    PWR_WELL_CTL_STATE_ENABLED    : constant := 1 * 2 ** 30;
@@ -197,13 +220,149 @@
       IPS_Off;
    end Pre_All_Off;

-   procedure Initialize is
+   function Normalize_CDClk (CDClk : in Frequency_Type) return CDClk_Range is
+     (   if CDClk <= 337_500_000 then 337_500_000
+      elsif CDClk <= 450_000_000 then 450_000_000
+      elsif CDClk <= 540_000_000 then 540_000_000
+                                 else 675_000_000);
+
+   procedure Get_Cur_CDClk (CDClk : out CDClk_Range)
+   is
+      LCPLL_CTL : Word32;
+   begin
+      Registers.Read (Registers.LCPLL_CTL, LCPLL_CTL);
+      CDClk :=
+        (if Config.Has_Broadwell_CDClk then
+           (case LCPLL_CTL and LCPLL_CTL_CD_FREQ_SEL_MASK is
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ    => 540_000_000,
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ  => 337_500_000,
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ    => 675_000_000,
+               when others                               => 450_000_000)
+         else
+           (case LCPLL_CTL and LCPLL_CTL_CD_FREQ_SEL_MASK is
+               when LCPLL_CTL_CD_FREQ_SEL_HSW_ALTERNATE =>
+                 (if    Config.Is_ULX  then 337_500_000
+                  elsif Config.Is_ULT  then 450_000_000
+                                       else 540_000_000),
+               when others => 450_000_000));
+   end Get_Cur_CDClk;
+
+   procedure Get_Max_CDClk (CDClk : out CDClk_Range)
+   is
+      FUSE_STRAP : Word32;
+   begin
+      if Config.Has_Broadwell_CDClk then
+         Registers.Read (Registers.FUSE_STRAP, FUSE_STRAP);
+         CDClk :=
+           (if (FUSE_STRAP and FUSE_STRAP_DISPLAY_CDCLK_LIMIT) /= 0 then
+               450_000_000
+            elsif Config.Is_ULX then
+               450_000_000
+            elsif Config.Is_ULT then
+               540_000_000
+            else
+               675_000_000);
+      else
+         -- We may never switch CDClk on Haswell. So from our point
+         -- of view, the CDClk we start with is the maximum.
+         Get_Cur_CDClk (CDClk);
+      end if;
+   end Get_Max_CDClk;
+
+   procedure Set_CDClk (CDClk_In : Frequency_Type)
+   is
+      CDClk : constant CDClk_Range :=
+         Normalize_CDClk (Frequency_Type'Min (CDClk_In, Config.Max_CDClk));
+      Success : Boolean;
+   begin
+      if not Config.Can_Switch_CDClk then
+         return;
+      end if;
+
+      PCode.Mailbox_Write
+        (MBox        => BDW_PCODE_DISPLAY_FREQ_CHANGE,
+         Command     => 0,
+         Wait_Ready  => True,
+         Success     => Success);
+
+      if not Success then
+         pragma Debug (Debug.Put_Line
+           ("ERROR: PCODE didn't acknowledge frequency change."));
+         return;
+      end if;
+
+      Registers.Set_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_SELECT_FCLK);
+      Registers.Wait_Set_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_FCLK_DONE);
+
+      Registers.Unset_And_Set_Mask
+        (Register    => Registers.LCPLL_CTL,
+         Mask_Unset  => LCPLL_CTL_CD_FREQ_SEL_MASK,
+         Mask_Set    => LCPLL_CTL_CD_FREQ_SEL_BDW (CDClk));
+      Registers.Posting_Read (Registers.LCPLL_CTL);
+
+      Registers.Unset_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_SELECT_FCLK);
+      Registers.Wait_Unset_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_FCLK_DONE);
+
+      PCode.Mailbox_Write
+        (MBox        => HSW_PCODE_DE_WRITE_FREQ,
+         Command     => (case CDClk is
+                           when 675_000_000 => 3,
+                           when 540_000_000 => 1,
+                           when 450_000_000 => 0,
+                           when others      => 2));
+
+      Registers.Write
+        (Register => Registers.CDCLK_FREQ,
+         Value    => Word32 (Div_Round_Closest (CDClk, 1_000_000) - 1));
+
+      Config.CDClk := CDClk;
+   end Set_CDClk;
+
+   procedure Initialize
+   is
+      CDClk : CDClk_Range;
    begin
       -- HSW: disable power down well
       PDW_Off;
+
+      Get_Cur_CDClk (CDClk);
+      Config.CDClk := CDClk;
+      Get_Max_CDClk (CDClk);
+      Config.Max_CDClk := CDClk;
+      Set_CDClk (Config.Default_CDClk_Freq);
+
       Config.Raw_Clock := Config.Default_RawClk_Freq;
    end Initialize;

+   procedure Limit_Dotclocks
+     (Configs        : in out Pipe_Configs;
+      CDClk_Switch   :    out Boolean)
+   is
+   begin
+      Config_Helpers.Limit_Dotclocks (Configs, Config.Max_CDClk);
+      CDClk_Switch :=
+         Config.Can_Switch_CDClk and then
+         Config.CDClk /= Normalize_CDClk
+           (Config_Helpers.Highest_Dotclock (Configs));
+   end Limit_Dotclocks;
+
+   procedure Update_CDClk (Configs : in out Pipe_Configs)
+   is
+      New_CDClk : constant Frequency_Type :=
+         Config_Helpers.Highest_Dotclock (Configs);
+   begin
+      Set_CDClk (New_CDClk);
+      Config_Helpers.Limit_Dotclocks (Configs, Config.CDClk);
+   end Update_CDClk;
+
    procedure Power_Set_To (Configs : Pipe_Configs) is
    begin
       if Need_PDW (Configs) then
diff --git a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
index 7c5a647..b9c85e1 100644
--- a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
+++ b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
@@ -12,6 +12,8 @@
 -- GNU General Public License for more details.
 --

+with HW.GFX.GMA.Config_Helpers;
+
 private package HW.GFX.GMA.Power_And_Clocks_Haswell is

procedure PSR_Off;
@@ -21,6 +23,15 @@

procedure Initialize;

+   procedure Limit_Dotclocks
+     (Configs        : in out Pipe_Configs;
+      CDClk_Switch   :    out Boolean)
+   with
+      Post => Config_Helpers.Stable_FB (Configs'Old, Configs);
+   procedure Update_CDClk (Configs : in out Pipe_Configs)
+   with
+      Post => Config_Helpers.Stable_FB (Configs'Old, Configs);
+
    procedure Power_Set_To (Configs : Pipe_Configs);
    procedure Power_Up (Old_Configs, New_Configs : Pipe_Configs);
    procedure Power_Down (Old_Configs, Tmp_Configs, New_Configs : Pipe_Configs);
diff --git a/common/hw-gfx-gma-config.ads.template b/common/hw-gfx-gma-config.ads.template
index 8e4c266..3e3e3bb 100644
--- a/common/hw-gfx-gma-config.ads.template
+++ b/common/hw-gfx-gma-config.ads.template
@@ -195,8 +195,11 @@

Has_FDI_RX_Power_Down         : <genbool> := Gen_Haswell;

+   ---------- Clocks: -----------
    Has_GMCH_RawClk               : <genbool> := Gen_G45;
    Has_GMCH_Mobile_VCO           : <g45bool> := GMCH_GM45;
+   Has_Broadwell_CDClk           : <hswbool> := CPU_Broadwell;
+   Can_Switch_CDClk              : <hswbool> := Broadwell_On;

----------- DDI: -------------
    End_EDP_Training_Late         : <genbool> := Gen_Haswell;
diff --git a/common/hw-gfx-gma-registers.ads b/common/hw-gfx-gma-registers.ads
index 1d38ffd..4eb2c04 100644
--- a/common/hw-gfx-gma-registers.ads
+++ b/common/hw-gfx-gma-registers.ads
@@ -92,6 +92,7 @@
       CPU_VGACNTRL,
       FUSE_STATUS,
       ILK_DISPLAY_CHICKEN2,
+      FUSE_STRAP,
       DSPCLK_GATE_D,
       FBA_CFB_BASE,
       FBC_CTL,
@@ -141,6 +142,7 @@
       TRANSA_CLK_SEL,
       TRANSB_CLK_SEL,
       TRANSC_CLK_SEL,
+      CDCLK_FREQ,
       NDE_RSTWRN_OPT,
       BLC_PWM_CPU_CTL2,
       BLC_PWM_CPU_CTL,
@@ -710,6 +712,7 @@
       FDI_RXC_TUSIZE1,
       QUIRK_F2060,
       TRANSC_CHICKEN2,
+      LCPLL_CTL,
       BXT_P_CR_GT_DISP_PWRON,
       GT_MAILBOX,
       GT_MAILBOX_DATA,
@@ -1263,6 +1266,9 @@
       PORT_CLK_SEL_DDID     => 16#04_610c# / Register_Width,
       PORT_CLK_SEL_DDIE     => 16#04_6110# / Register_Width,

+      -- Haswell LCPLL registers
+      LCPLL_CTL             => 16#13_0040# / Register_Width,
+
       -- Skylake I_boost configuration
       DISPIO_CR_TX_BMU_CR0  => 16#06_c00c# / Register_Width,

@@ -1279,6 +1285,7 @@

-- CD CLK register
       CDCLK_CTL             => 16#04_6000# / Register_Width,
+      CDCLK_FREQ            => 16#04_6200# / Register_Width,

-- Skylake LCPLL registers
       LCPLL1_CTL            => 16#04_6010# / Register_Width,
@@ -1456,6 +1463,7 @@
       CPU_VGACNTRL          => 16#04_1000# / Register_Width,
       GMCH_VGACNTRL         => 16#07_1400# / Register_Width,
       FUSE_STATUS           => 16#04_2000# / Register_Width,
+      FUSE_STRAP            => 16#04_2014# / Register_Width,
       FBA_CFB_BASE          => 16#04_3200# / Register_Width,
       IPS_CTL               => 16#04_3408# / Register_Width,
       ARB_CTL               => 16#04_5000# / Register_Width,
diff --git a/common/hw-gfx.ads b/common/hw-gfx.ads
index 93a3684..9a4015c 100644
--- a/common/hw-gfx.ads
+++ b/common/hw-gfx.ads
@@ -66,7 +66,7 @@
       Rotation => No_Rotation,
       Offset   => 0);

-   subtype Frequency_Type is Pos64 range 19_200_000 .. 624_000_000;
+   subtype Frequency_Type is Pos64 range 19_200_000 .. 675_000_000;

type DP_Lane_Count is (DP_Lane_Count_1, DP_Lane_Count_2, DP_Lane_Count_4);
    subtype DP_Lane_Count_Type is Pos64 range 1 .. 4;

[Nico Huber (Code Review)]

Hello Arthur Heymans, Matt DeVillier, Thomas Heijligen,

I'd like you to reexamine a change. Please visit

https://review.coreboot.org/c/libgfxinit/+/35717

to look at the new patch set (#2).

Change subject: gma hsw: Implement CDClk switching
......................................................................

gma hsw: Implement CDClk switching

For Haswell, we only read out the current CDClk setting and work with
that (CDClk is supposed to be set only once per boot, so we leave that
to the firmware). Broadwell supports actual frequency switching, with
the following limits:

o a fuse may restrict us to 450MHz,
  o ULX supports at most 450MHz w/o additional cooling,
  o ULT supports at most 540MHz w/o additional cooling,
  o others support up to 675MHz.

We assume there is no `additional cooling`. With the CDClk now being
switchable, we have to update `DP_Aux_Request` to assume the current
CDClk frequency instead of the default.

Dot clocks, on these platforms, may run with full CDClk speed.

Change-Id: I8fb23a49b334f9f0045884c3af4087764397b941
Signed-off-by: Nico Huber 
---
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
M common/hw-gfx-gma-config.ads.template
M common/hw-gfx-gma-dp_aux_request.adb
M common/hw-gfx-gma-registers.ads
M common/hw-gfx.ads
6 files changed, 184 insertions(+), 3 deletions(-)

git pull ssh://review.coreboot.org:29418/libgfxinit refs/changes/17/35717/2

[Nico Huber (Code Review)]

Nico Huber has posted comments on this change. ( https://review.coreboot.org/c/libgfxinit/+/35717 )

Change subject: gma hsw: Implement CDClk switching
......................................................................

Patch Set 2: Verified+1

[Nico Huber (Code Review)]

Nico Huber has posted comments on this change. ( https://review.coreboot.org/c/libgfxinit/+/35717 )

Change subject: gma hsw: Implement CDClk switching
......................................................................

Patch Set 3: Verified+1

[Matt DeVillier (Code Review)]

Matt DeVillier has posted comments on this change. ( https://review.coreboot.org/c/libgfxinit/+/35717 )

Change subject: gma hsw: Implement CDClk switching
......................................................................

Patch Set 4: Code-Review+2

tested on Purism Librem 15v4 w/4k display

[Arthur Heymans (Code Review)]

Arthur Heymans has posted comments on this change. ( https://review.coreboot.org/c/libgfxinit/+/35717 )

Change subject: gma hsw: Implement CDClk switching
......................................................................

Patch Set 4: Code-Review+2

[Nico Huber (Code Review)]

Nico Huber has submitted this change. ( https://review.coreboot.org/c/libgfxinit/+/35717 )

Change subject: gma hsw: Implement CDClk switching
......................................................................

gma hsw: Implement CDClk switching

For Haswell, we only read out the current CDClk setting and work with
that (CDClk is supposed to be set only once per boot, so we leave that
to the firmware). Broadwell supports actual frequency switching, with
the following limits:

o a fuse may restrict us to 450MHz,
  o ULX supports at most 450MHz w/o additional cooling,
  o ULT supports at most 540MHz w/o additional cooling,
  o others support up to 675MHz.

We assume there is no `additional cooling`. With the CDClk now being
switchable, we have to update `DP_Aux_Request` to assume the current
CDClk frequency instead of the default.

Dot clocks, on these platforms, may run with full CDClk speed.

Change-Id: I8fb23a49b334f9f0045884c3af4087764397b941
Signed-off-by: Nico Huber 
Reviewed-on: https://review.coreboot.org/c/libgfxinit/+/35717
Reviewed-by: Matt DeVillier 
Reviewed-by: Arthur Heymans 
---
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
M common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
M common/hw-gfx-gma-config.ads.template
M common/hw-gfx-gma-dp_aux_request.adb
M common/hw-gfx-gma-registers.ads
5 files changed, 186 insertions(+), 2 deletions(-)

Approvals:
  Nico Huber: Verified
  Matt DeVillier: Looks good to me, approved
  Arthur Heymans: Looks good to me, approved

diff --git a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
index 332c3df..44d9acd 100644
--- a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
+++ b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.adb
@@ -1,5 +1,6 @@
 --
 -- Copyright (C) 2014-2018 secunet Security Networks AG
+-- Copyright (C) 2019 Nico Huber 
 --
 -- This program is free software; you can redistribute it and/or modify
 -- it under the terms of the GNU General Public License as published by
@@ -22,6 +23,30 @@

package body HW.GFX.GMA.Power_And_Clocks_Haswell is

+   LCPLL_CTL_CD_FREQ_SEL_MASK          : constant := 3 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_450_MHZ       : constant := 0 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_HSW_ALTERNATE : constant := 1 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ   : constant := 1 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ : constant := 2 * 2 ** 26;
+   LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ   : constant := 3 * 2 ** 26;
+   LCPLL_CTL_CD_SOURCE_SELECT_FCLK     : constant := 1 * 2 ** 21;
+   LCPLL_CTL_CD_SOURCE_FCLK_DONE       : constant := 1 * 2 ** 19;
+
+   function LCPLL_CTL_CD_FREQ_SEL_BDW (CDClk : Config.CDClk_Range) return Word32
+   is
+     (case CDClk is
+         when 675_000_000 => LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ,
+         when 540_000_000 => LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ,
+         when 450_000_000 => LCPLL_CTL_CD_FREQ_SEL_450_MHZ,
+         when others      => LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ);
+
+   FUSE_STRAP_DISPLAY_CDCLK_LIMIT      : constant := 1 * 2 ** 24;
+
+   HSW_PCODE_DE_WRITE_FREQ             : constant := 16#17#;
+   BDW_PCODE_DISPLAY_FREQ_CHANGE       : constant := 16#18#;
+
+   ----------------------------------------------------------------------------
+
    PWR_WELL_CTL_ENABLE_REQUEST   : constant := 1 * 2 ** 31;
    PWR_WELL_CTL_DISABLE_REQUEST  : constant := 0 * 2 ** 31;
    PWR_WELL_CTL_STATE_ENABLED    : constant := 1 * 2 ** 30;
@@ -197,13 +222,149 @@
       IPS_Off;
    end Pre_All_Off;

-   procedure Initialize is
+   function Normalize_CDClk (CDClk : in Int64) return Config.CDClk_Range is
+     (   if CDClk <= 337_500_000 then 337_500_000
+      elsif CDClk <= 450_000_000 then 450_000_000
+      elsif CDClk <= 540_000_000 then 540_000_000
+                                 else 675_000_000);
+
+   procedure Get_Cur_CDClk (CDClk : out Config.CDClk_Range)
+   is
+      LCPLL_CTL : Word32;
+   begin
+      Registers.Read (Registers.LCPLL_CTL, LCPLL_CTL);
+      CDClk :=
+        (if Config.Has_Broadwell_CDClk then
+           (case LCPLL_CTL and LCPLL_CTL_CD_FREQ_SEL_MASK is
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_540_MHZ    => 540_000_000,
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_337_5_MHZ  => 337_500_000,
+               when LCPLL_CTL_CD_FREQ_SEL_BDW_675_MHZ    => 675_000_000,
+               when others                               => 450_000_000)
+         else
+           (case LCPLL_CTL and LCPLL_CTL_CD_FREQ_SEL_MASK is
+               when LCPLL_CTL_CD_FREQ_SEL_HSW_ALTERNATE =>
+                 (if    Config.Is_ULX  then 337_500_000
+                  elsif Config.Is_ULT  then 450_000_000
+                                       else 540_000_000),
+               when others => 450_000_000));
+   end Get_Cur_CDClk;
+
+   procedure Get_Max_CDClk (CDClk : out Config.CDClk_Range)
+   is
+      FUSE_STRAP : Word32;
+   begin
+      if Config.Has_Broadwell_CDClk then
+         Registers.Read (Registers.FUSE_STRAP, FUSE_STRAP);
+         CDClk :=
+           (if (FUSE_STRAP and FUSE_STRAP_DISPLAY_CDCLK_LIMIT) /= 0 then
+               450_000_000
+            elsif Config.Is_ULX then
+               450_000_000
+            elsif Config.Is_ULT then
+               540_000_000
+            else
+               675_000_000);
+      else
+         -- We may never switch CDClk on Haswell. So from our point
+         -- of view, the CDClk we start with is the maximum.
+         Get_Cur_CDClk (CDClk);
+      end if;
+   end Get_Max_CDClk;
+
+   procedure Set_CDClk (CDClk_In : Frequency_Type)
+   is
+      CDClk : constant Config.CDClk_Range :=
+         Normalize_CDClk (Frequency_Type'Min (CDClk_In, Config.Max_CDClk));
+      Success : Boolean;
+   begin
+      if not Config.Can_Switch_CDClk then
+         return;
+      end if;
+
+      PCode.Mailbox_Write
+        (MBox        => BDW_PCODE_DISPLAY_FREQ_CHANGE,
+         Command     => 0,
+         Wait_Ready  => True,
+         Success     => Success);
+
+      if not Success then
+         pragma Debug (Debug.Put_Line
+           ("ERROR: PCODE didn't acknowledge frequency change."));
+         return;
+      end if;
+
+      Registers.Set_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_SELECT_FCLK);
+      Registers.Wait_Set_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_FCLK_DONE);
+
+      Registers.Unset_And_Set_Mask
+        (Register    => Registers.LCPLL_CTL,
+         Mask_Unset  => LCPLL_CTL_CD_FREQ_SEL_MASK,
+         Mask_Set    => LCPLL_CTL_CD_FREQ_SEL_BDW (CDClk));
+      Registers.Posting_Read (Registers.LCPLL_CTL);
+
+      Registers.Unset_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_SELECT_FCLK);
+      Registers.Wait_Unset_Mask
+        (Register => Registers.LCPLL_CTL,
+         Mask     => LCPLL_CTL_CD_SOURCE_FCLK_DONE);
+
+      PCode.Mailbox_Write
+        (MBox        => HSW_PCODE_DE_WRITE_FREQ,
+         Command     => (case CDClk is
+                           when 675_000_000 => 3,
+                           when 540_000_000 => 1,
+                           when 450_000_000 => 0,
+                           when others      => 2));
+
+      Registers.Write
+        (Register => Registers.CDCLK_FREQ,
+         Value    => Word32 (Div_Round_Closest (CDClk, 1_000_000) - 1));
+
+      Config.CDClk := CDClk;
+   end Set_CDClk;
+
+   procedure Initialize
+   is
+      CDClk : Config.CDClk_Range;
    begin
       -- HSW: disable power down well
       PDW_Off;
+
+      Get_Cur_CDClk (CDClk);
+      Config.CDClk := CDClk;
+      Get_Max_CDClk (CDClk);
+      Config.Max_CDClk := CDClk;
+      Set_CDClk (Config.Default_CDClk_Freq);
+
       Config.Raw_Clock := Config.Default_RawClk_Freq;
    end Initialize;

+   procedure Limit_Dotclocks
+     (Configs        : in out Pipe_Configs;
+      CDClk_Switch   :    out Boolean)
+   is
+   begin
+      Config_Helpers.Limit_Dotclocks (Configs, Config.Max_CDClk);
+      CDClk_Switch :=
+         Config.Can_Switch_CDClk and then
+         Config.CDClk /= Normalize_CDClk
+           (Config_Helpers.Highest_Dotclock (Configs));
+   end Limit_Dotclocks;
+
+   procedure Update_CDClk (Configs : in out Pipe_Configs)
+   is
+      New_CDClk : constant Frequency_Type :=
+         Config_Helpers.Highest_Dotclock (Configs);
+   begin
+      Set_CDClk (New_CDClk);
+      Config_Helpers.Limit_Dotclocks (Configs, Config.CDClk);
+   end Update_CDClk;
+
    procedure Power_Set_To (Configs : Pipe_Configs) is
    begin
       if Need_PDW (Configs) then
diff --git a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
index 7c5a647..2762d82 100644
--- a/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
+++ b/common/haswell_shared/hw-gfx-gma-power_and_clocks_haswell.ads
@@ -12,6 +12,8 @@
 -- GNU General Public License for more details.
 --

+with HW.GFX.GMA.Config_Helpers;
+
 private package HW.GFX.GMA.Power_And_Clocks_Haswell is

procedure PSR_Off;
@@ -21,6 +23,16 @@

procedure Initialize;

+   procedure Limit_Dotclocks
+     (Configs        : in out Pipe_Configs;
+      CDClk_Switch   :    out Boolean)
+   with
+      Post => Config_Helpers.Stable_FB (Configs'Old, Configs);
+   procedure Update_CDClk (Configs : in out Pipe_Configs)
+   with
+      Post => Config_Helpers.Stable_FB (Configs'Old, Configs);
+   procedure Enable_CDClk is null;
+
    procedure Power_Set_To (Configs : Pipe_Configs);
    procedure Power_Up (Old_Configs, New_Configs : Pipe_Configs);
    procedure Power_Down (Old_Configs, Tmp_Configs, New_Configs : Pipe_Configs);
diff --git a/common/hw-gfx-gma-config.ads.template b/common/hw-gfx-gma-config.ads.template
index 481897c..0e49943 100644
--- a/common/hw-gfx-gma-config.ads.template
+++ b/common/hw-gfx-gma-config.ads.template
@@ -205,8 +205,11 @@

Has_FDI_RX_Power_Down         : <genbool> := Gen_Haswell;

+   ---------- Clocks: -----------
    Has_GMCH_RawClk               : <genbool> := Gen_G45;
    Has_GMCH_Mobile_VCO           : <g45bool> := GMCH_GM45;
+   Has_Broadwell_CDClk           : <hswbool> := CPU_Broadwell;
+   Can_Switch_CDClk              : <hswbool> := Broadwell_On;

----------- DDI: -------------
    End_EDP_Training_Late         : <genbool> := Gen_Haswell;
diff --git a/common/hw-gfx-gma-dp_aux_request.adb b/common/hw-gfx-gma-dp_aux_request.adb
index c057059..54500e2 100644
--- a/common/hw-gfx-gma-dp_aux_request.adb
+++ b/common/hw-gfx-gma-dp_aux_request.adb
@@ -232,7 +232,7 @@
       DP_AUX_CTL_2x_Clock : constant Word32 :=
         (if Config.Has_PCH_Aux_Channels then
            (if Port = DP_A then
-               Word32 ((Config.Default_CDClk_Freq + 1_000_000) / 2_000_000)
+               Word32 ((Config.CDClk + 1_000_000) / 2_000_000)
             else
                Word32 ((Config.Raw_Clock + 1_000_000) / 2_000_000))
          elsif Config.Has_GMCH_RawClk then
diff --git a/common/hw-gfx-gma-registers.ads b/common/hw-gfx-gma-registers.ads
index 1d38ffd..4eb2c04 100644
--- a/common/hw-gfx-gma-registers.ads
+++ b/common/hw-gfx-gma-registers.ads
@@ -92,6 +92,7 @@
       CPU_VGACNTRL,
       FUSE_STATUS,
       ILK_DISPLAY_CHICKEN2,
+      FUSE_STRAP,
       DSPCLK_GATE_D,
       FBA_CFB_BASE,
       FBC_CTL,
@@ -141,6 +142,7 @@
       TRANSA_CLK_SEL,
       TRANSB_CLK_SEL,
       TRANSC_CLK_SEL,
+      CDCLK_FREQ,
       NDE_RSTWRN_OPT,
       BLC_PWM_CPU_CTL2,
       BLC_PWM_CPU_CTL,
@@ -710,6 +712,7 @@
       FDI_RXC_TUSIZE1,
       QUIRK_F2060,
       TRANSC_CHICKEN2,
+      LCPLL_CTL,
       BXT_P_CR_GT_DISP_PWRON,
       GT_MAILBOX,
       GT_MAILBOX_DATA,
@@ -1263,6 +1266,9 @@
       PORT_CLK_SEL_DDID     => 16#04_610c# / Register_Width,
       PORT_CLK_SEL_DDIE     => 16#04_6110# / Register_Width,

+      -- Haswell LCPLL registers
+      LCPLL_CTL             => 16#13_0040# / Register_Width,
+
       -- Skylake I_boost configuration
       DISPIO_CR_TX_BMU_CR0  => 16#06_c00c# / Register_Width,

@@ -1279,6 +1285,7 @@

-- CD CLK register
       CDCLK_CTL             => 16#04_6000# / Register_Width,
+      CDCLK_FREQ            => 16#04_6200# / Register_Width,

-- Skylake LCPLL registers
       LCPLL1_CTL            => 16#04_6010# / Register_Width,
@@ -1456,6 +1463,7 @@
       CPU_VGACNTRL          => 16#04_1000# / Register_Width,
       GMCH_VGACNTRL         => 16#07_1400# / Register_Width,
       FUSE_STATUS           => 16#04_2000# / Register_Width,
+      FUSE_STRAP            => 16#04_2014# / Register_Width,
       FBA_CFB_BASE          => 16#04_3200# / Register_Width,
       IPS_CTL               => 16#04_3408# / Register_Width,
       ARB_CTL               => 16#04_5000# / Register_Width,