From: Abhinav Kumar <[email protected]>

DP MST streams share 64 MTP slots in a time-multiplexed manner. Add
support for calculating the rate governor, slot allocation, and slot
reservation in the DP controller.

The per-stream slot allocation is not tracked by the driver. Callers
pass in a msm_dp_mst_slot_info table derived from
drm_dp_mst_topology_state::payloads, which the MST helpers keep up to
date, and the controller only translates it into the per-stream
timeslot registers and the rate governor inputs.

Signed-off-by: Abhinav Kumar <[email protected]>
Reviewed-by: Dmitry Baryshkov <[email protected]>
Signed-off-by: Yongxing Mou <[email protected]>
---
 drivers/gpu/drm/msm/dp/dp_ctrl.c    | 146 +++++++++++++++++++++++++++++++++++-
 drivers/gpu/drm/msm/dp/dp_ctrl.h    |   4 +-
 drivers/gpu/drm/msm/dp/dp_display.c |   9 ++-
 drivers/gpu/drm/msm/dp/dp_panel.h   |   6 ++
 4 files changed, 160 insertions(+), 5 deletions(-)

diff --git a/drivers/gpu/drm/msm/dp/dp_ctrl.c b/drivers/gpu/drm/msm/dp/dp_ctrl.c
index 2ff36d3fcb67f..a9d0956a9751b 100644
--- a/drivers/gpu/drm/msm/dp/dp_ctrl.c
+++ b/drivers/gpu/drm/msm/dp/dp_ctrl.c
@@ -299,6 +299,37 @@ static void msm_dp_ctrl_mst_config(struct 
msm_dp_ctrl_private *ctrl, bool enable
        msm_dp_write_link(ctrl, REG_DP_MAINLINK_CTRL, mainlink_ctrl);
 }
 
+static void msm_dp_ctrl_mst_update_timeslot(struct msm_dp_ctrl_private *ctrl,
+                                         enum msm_dp_stream_id stream_id, u32 
ch_start_slot,
+                                         u32 tot_slot_cnt)
+{
+       u32 slot_reg_1 = 0, slot_reg_2 = 0;
+
+       drm_dbg_dp(ctrl->drm_dev, "stream_id %d, start_slot %d, tot_slot %d\n",
+                  stream_id, ch_start_slot, tot_slot_cnt);
+
+       if (ch_start_slot && tot_slot_cnt) {
+               u64 mask = GENMASK_ULL(ch_start_slot + tot_slot_cnt - 2, 
ch_start_slot - 1);
+
+               slot_reg_1 = mask & 0xFFFFFFFF;
+               slot_reg_2 = (mask >> 32) & 0xFFFFFFFF;
+       }
+
+       msm_dp_write_stream_link(ctrl, stream_id, REG_DP_DP0_TIMESLOT_1_32, 
slot_reg_1);
+       msm_dp_write_stream_link(ctrl, stream_id, REG_DP_DP0_TIMESLOT_33_63, 
slot_reg_2);
+}
+
+static void msm_dp_ctrl_update_rg(struct msm_dp_ctrl_private *ctrl,
+                                 enum msm_dp_stream_id stream_id, u32 x_int, 
u32 y_frac_enum)
+{
+       u32 rg = y_frac_enum | (x_int << 16);
+
+       drm_dbg_dp(ctrl->drm_dev, "stream_id: %d x_int:%d y_frac_enum:%d 
rg:%d\n",
+                  stream_id, x_int, y_frac_enum, rg);
+
+       msm_dp_write_stream_link(ctrl, stream_id, REG_DP_DP0_RG, rg);
+}
+
 /*
  * NOTE: resetting DP controller will also clear any pending HPD related 
interrupts
  */
@@ -2710,6 +2741,100 @@ static void msm_dp_ctrl_config_msa(struct 
msm_dp_ctrl_private *ctrl,
        msm_dp_write_stream_link(ctrl, panel->stream_id, REG_DP_SOFTWARE_NVID, 
nvid);
 }
 
+/*
+ * Calculate MST Rate Governor parameters x_int and y_frac_enum.
+ *
+ * The RG paces symbol delivery per MTP via: M = x_int + y_frac_enum/256
+ * where M is the target symbol count per MTP across all lanes.
+ *
+ * min_slot_cnt = (pclk * bpp/8) / (lclk * lanes) * 64   -- slots at 1.0x BW
+ * max_slot_cnt = pbn * 54 / (lclk * lanes)               -- slots at PBN limit
+ * raw_target_sc = (min + max) / 2                        -- midpoint (~1.003x)
+ *
+ * Quantize raw_target_sc to 1/(256*lanes) steps, then:
+ *   M           = Chosen_TARGET_Slot_Count * lanes
+ *   x_int       = INT(M)
+ *   y_frac_enum = CEIL(256 * MOD(M, 1))
+ */
+static void msm_dp_ctrl_mst_calculate_rg(struct msm_dp_ctrl_private *ctrl,
+                                        struct msm_dp_panel *panel,
+                                        const struct msm_dp_mst_slot_info 
*slots,
+                                        u32 *p_x_int, u32 *p_y_frac_enum)
+{
+       u64 min_slot_cnt, max_slot_cnt;
+       u64 raw_target_sc, target_sc_fixp, scale_fixp;
+       u64 pclk = panel->msm_dp_mode.drm_mode.clock;
+       u64 lclk = 0;
+       u64 lanes = ctrl->link->link_params.num_lanes;
+       u64 bpp = panel->msm_dp_mode.bpp;
+       u64 pbn = slots[panel->stream_id].pbn;
+       u64 numerator, denominator, temp, temp1, scaled_target_sc;
+       u32 x_int = 0, y_frac_enum = 0;
+       u64 target_strm_sym, y_frac_enum_fixp;
+
+       lclk = ctrl->link->link_params.rate;
+
+       /* min_slot_cnt */
+       numerator = pclk * bpp * 64 * 1000;
+       denominator = lclk * lanes * 8 * 1000;
+       min_slot_cnt = drm_fixp_from_fraction(numerator, denominator);
+
+       /* max_slot_cnt */
+       numerator = pbn * 54 * 1000;
+       denominator = lclk * lanes;
+       max_slot_cnt = drm_fixp_from_fraction(numerator, denominator);
+
+       /* raw_target_sc */
+       numerator = max_slot_cnt + min_slot_cnt;
+       denominator = drm_fixp_from_fraction(2, 1);
+       raw_target_sc = drm_fixp_div(numerator, denominator);
+
+       /* Quantize target_sc to the nearest 1 / (256 * lanes). */
+       scale_fixp = drm_fixp_from_fraction(256 * lanes, 1);
+       scaled_target_sc = drm_fixp_mul(raw_target_sc, scale_fixp);
+       numerator = 
drm_fixp_from_fraction(drm_fixp2int_round(scaled_target_sc), 1);
+       target_sc_fixp = drm_fixp_div(numerator, scale_fixp);
+
+       /* target_strm_sym */
+       temp1 = drm_fixp_from_fraction(lanes, 1);
+       target_strm_sym = drm_fixp_mul(target_sc_fixp, temp1);
+
+       /* x_int */
+       x_int = drm_fixp2int(target_strm_sym);
+
+       /* y_enum_frac */
+       temp = drm_fixp_from_fraction(x_int, 1);
+       temp1 = target_strm_sym - temp;
+       temp = drm_fixp_from_fraction(256, 1);
+       y_frac_enum_fixp = drm_fixp_mul(temp1, temp);
+
+       y_frac_enum = drm_fixp2int(y_frac_enum_fixp);
+
+       *p_x_int = x_int;
+       *p_y_frac_enum = y_frac_enum;
+
+       drm_dbg_dp(ctrl->drm_dev, "MST lane_cnt:%llu, rate:%llu x_int:%d, 
y_frac:%d\n",
+                  lanes, lclk, x_int, y_frac_enum);
+}
+
+static void msm_dp_ctrl_mst_stream_setup(struct msm_dp_ctrl_private *ctrl,
+                                        struct msm_dp_panel *panel,
+                                        const struct msm_dp_mst_slot_info 
*slots)
+{
+       u32 x_int, y_frac_enum;
+
+       if (!ctrl->mst_mode || !slots)
+               return;
+
+       drm_dbg_dp(ctrl->drm_dev, "MST stream channel allocation\n");
+
+       msm_dp_ctrl_mst_timeslot_setup(&ctrl->msm_dp_ctrl, slots);
+
+       msm_dp_ctrl_mst_calculate_rg(ctrl, panel, slots, &x_int, &y_frac_enum);
+
+       msm_dp_ctrl_update_rg(ctrl, panel->stream_id, x_int, y_frac_enum);
+}
+
 int msm_dp_ctrl_prepare_stream_on(struct msm_dp_ctrl *msm_dp_ctrl,
                                  struct msm_dp_panel *panel,
                                  bool force_link_train)
@@ -2747,7 +2872,7 @@ int msm_dp_ctrl_prepare_stream_on(struct msm_dp_ctrl 
*msm_dp_ctrl,
 }
 
 int msm_dp_ctrl_on_stream(struct msm_dp_ctrl *msm_dp_ctrl, struct msm_dp_panel 
*panel,
-                         bool mst_active)
+                         bool mst_active, const struct msm_dp_mst_slot_info 
*slots)
 {
        int ret = 0;
        bool mainlink_ready = false;
@@ -2793,6 +2918,8 @@ int msm_dp_ctrl_on_stream(struct msm_dp_ctrl 
*msm_dp_ctrl, struct msm_dp_panel *
        if (!ctrl->mst_mode)
                msm_dp_ctrl_setup_tr_unit(ctrl, panel);
 
+       msm_dp_ctrl_mst_stream_setup(ctrl, panel, slots);
+
        msm_dp_write_link(ctrl, REG_DP_STATE_CTRL, DP_STATE_CTRL_SEND_VIDEO);
 
        ret = msm_dp_ctrl_mst_send_act(msm_dp_ctrl, panel);
@@ -2848,6 +2975,23 @@ void msm_dp_ctrl_off_link(struct msm_dp_ctrl 
*msm_dp_ctrl,
        phy_power_off(phy);
 }
 
+void msm_dp_ctrl_mst_timeslot_setup(struct msm_dp_ctrl *msm_dp_ctrl,
+                                   const struct msm_dp_mst_slot_info *slots)
+{
+       struct msm_dp_ctrl_private *ctrl;
+       int i;
+
+       ctrl = container_of(msm_dp_ctrl, struct msm_dp_ctrl_private, 
msm_dp_ctrl);
+
+       if (!ctrl->mst_mode || !slots)
+               return;
+
+       for (i = DP_STREAM_0; i < ctrl->num_pixel_clks; i++) {
+               msm_dp_ctrl_mst_update_timeslot(ctrl, i, slots[i].start_slot,
+                                               slots[i].tot_slots);
+       }
+}
+
 irqreturn_t msm_dp_ctrl_isr(struct msm_dp_ctrl *msm_dp_ctrl,
                            struct msm_dp_panel *panel)
 {
diff --git a/drivers/gpu/drm/msm/dp/dp_ctrl.h b/drivers/gpu/drm/msm/dp/dp_ctrl.h
index 84ea37d8db2e3..9478f127c6d34 100644
--- a/drivers/gpu/drm/msm/dp/dp_ctrl.h
+++ b/drivers/gpu/drm/msm/dp/dp_ctrl.h
@@ -19,7 +19,7 @@ struct phy;
 int msm_dp_ctrl_on_link(struct msm_dp_ctrl *msm_dp_ctrl,
                        struct msm_dp_panel *panel);
 int msm_dp_ctrl_on_stream(struct msm_dp_ctrl *msm_dp_ctrl, struct msm_dp_panel 
*panel,
-                         bool mst_active);
+                         bool mst_active, const struct msm_dp_mst_slot_info 
*slots);
 int msm_dp_ctrl_prepare_stream_on(struct msm_dp_ctrl *msm_dp_ctrl,
                                  struct msm_dp_panel *panel,
                                  bool force_link_train);
@@ -63,4 +63,6 @@ bool msm_dp_ctrl_stream_clks_on(struct msm_dp_ctrl *dp_ctrl,
                                enum msm_dp_stream_id stream_id);
 int msm_dp_ctrl_mst_send_act(struct msm_dp_ctrl *msm_dp_ctrl,
                             struct msm_dp_panel *panel);
+void msm_dp_ctrl_mst_timeslot_setup(struct msm_dp_ctrl *msm_dp_ctrl,
+                                   const struct msm_dp_mst_slot_info *slots);
 #endif /* _DP_CTRL_H_ */
diff --git a/drivers/gpu/drm/msm/dp/dp_display.c 
b/drivers/gpu/drm/msm/dp/dp_display.c
index f6d16c57afcc1..e046184a30f50 100644
--- a/drivers/gpu/drm/msm/dp/dp_display.c
+++ b/drivers/gpu/drm/msm/dp/dp_display.c
@@ -668,7 +668,8 @@ static int msm_dp_display_prepare_link(struct 
msm_dp_display_private *dp)
 }
 
 static int msm_dp_display_enable(struct msm_dp_display_private *dp,
-                                struct msm_dp_panel *msm_dp_panel)
+                                struct msm_dp_panel *msm_dp_panel,
+                                const struct msm_dp_mst_slot_info *slots)
 {
        int rc = 0;
        struct msm_dp *msm_dp_display = &dp->msm_dp_display;
@@ -679,7 +680,8 @@ static int msm_dp_display_enable(struct 
msm_dp_display_private *dp,
                return 0;
        }
 
-       rc = msm_dp_ctrl_on_stream(dp->ctrl, msm_dp_panel, 
msm_dp_display->mst_active);
+       rc = msm_dp_ctrl_on_stream(dp->ctrl, msm_dp_panel, 
msm_dp_display->mst_active,
+                                  slots);
        if (!rc)
                msm_dp_display->power_on = true;
 
@@ -1491,7 +1493,7 @@ void msm_dp_display_atomic_enable(struct msm_dp 
*msm_dp_display,
                return;
        }
 
-       rc = msm_dp_display_enable(dp, dp->panel);
+       rc = msm_dp_display_enable(dp, dp->panel, NULL);
        if (rc)
                DRM_ERROR("DP display enable failed, rc=%d\n", rc);
 
@@ -1525,6 +1527,7 @@ void msm_dp_display_atomic_disable(struct msm_dp *dp)
                return;
 
        msm_dp_ctrl_push_vcpf(msm_dp_display->ctrl, msm_dp_display->panel);
+       msm_dp_ctrl_mst_timeslot_setup(msm_dp_display->ctrl, NULL);
        msm_dp_ctrl_mst_send_act(msm_dp_display->ctrl, msm_dp_display->panel);
 }
 
diff --git a/drivers/gpu/drm/msm/dp/dp_panel.h 
b/drivers/gpu/drm/msm/dp/dp_panel.h
index 04938b9d2091f..9f34cc9010d38 100644
--- a/drivers/gpu/drm/msm/dp/dp_panel.h
+++ b/drivers/gpu/drm/msm/dp/dp_panel.h
@@ -36,6 +36,12 @@ enum msm_dp_stream_id {
        DP_STREAM_MAX,
 };
 
+struct msm_dp_mst_slot_info {
+       u8 start_slot;
+       u8 tot_slots;
+       u32 pbn;
+};
+
 struct msm_dp_panel {
        /* dpcd raw data */
        u8 dpcd[DP_RECEIVER_CAP_SIZE];

-- 
2.43.0

Reply via email to