[PATCH v2 3/4] drm/dp: Add DisplayPort link helpers

[Thierry Reding <thierry.reding@gmail.com>]

Add a helper to probe a DP link (reading out the maximum rate, link
count and capabilities) as well as configuring it accordingly.

Signed-off-by: Thierry Reding <treding@nvidia.com>
---
 drivers/gpu/drm/drm_dp_helper.c | 77 +++++++++++++++++++++++++++++++++++++++++
 include/drm/drm_dp_helper.h     | 30 ++++++++++++++++
 2 files changed, 107 insertions(+)

diff --git a/drivers/gpu/drm/drm_dp_helper.c b/drivers/gpu/drm/drm_dp_helper.c
index 8111b8e5a8bc..01a8173c6e83 100644
--- a/drivers/gpu/drm/drm_dp_helper.c
+++ b/drivers/gpu/drm/drm_dp_helper.c
@@ -478,3 +478,80 @@ int drm_dp_dpcd_read_link_status(struct drm_dp_aux *aux,
 				DP_LINK_STATUS_SIZE);
 }
 EXPORT_SYMBOL(drm_dp_dpcd_read_link_status);
+
+/**
+ * drm_dp_link_probe() - probe a DisplayPort link for capabilities
+ * @aux: DisplayPort AUX channel
+ * @link: pointer to structure in which to return link capabilities
+ *
+ * The structure filled in by this function can usually be passed directly
+ * into drm_dp_link_power_up() configure the link based on the link's
+ * capabilities.
+ *
+ * Returns 0 on success or a negative error code on failure.
+ */
+int drm_dp_link_probe(struct drm_dp_aux *aux, struct drm_dp_link *link)
+{
+	u8 value;
+	int err;
+
+	memset(link, 0, sizeof(*link));
+
+	err = drm_dp_dpcd_readb(aux, DP_MAX_LINK_RATE, &value);
+	if (err < 0)
+		return err;
+
+	link->rate = drm_dp_bw_code_to_link_rate(value);
+
+	err = drm_dp_dpcd_readb(aux, DP_MAX_LANE_COUNT, &value);
+	if (err < 0)
+		return err;
+
+	link->num_lanes = value & DP_MAX_LANE_COUNT_MASK;
+
+	if (value & DP_ENHANCED_FRAME_CAP)
+		link->capabilities |= DP_LINK_CAP_ENHANCED_FRAMING;
+
+	return 0;
+}
+
+/**
+ * drm_dp_link_power_up() - power up and configure a DisplayPort link
+ * @aux: DisplayPort AUX channel
+ * @link: pointer to a structure containing the link configuration
+ *
+ * Returns 0 on success or a negative error code on failure.
+ */
+int drm_dp_link_power_up(struct drm_dp_aux *aux, struct drm_dp_link *link)
+{
+	u8 value;
+	int err;
+
+	err = drm_dp_dpcd_readb(aux, DP_SET_POWER, &value);
+	if (err < 0)
+		return err;
+
+	value &= ~DP_SET_POWER_MASK;
+	value |= DP_SET_POWER_D0;
+
+	err = drm_dp_dpcd_writeb(aux, value, DP_SET_POWER);
+	if (err < 0)
+		return err;
+
+	value = drm_dp_link_rate_to_bw_code(link->rate);
+
+	err = drm_dp_dpcd_writeb(aux, value, DP_LINK_BW_SET);
+	if (err < 0)
+		return err;
+
+	value = link->num_lanes;
+
+	if (link->capabilities & DP_LINK_CAP_ENHANCED_FRAMING)
+		value |= DP_LANE_COUNT_ENHANCED_FRAME_EN;
+
+	err = drm_dp_dpcd_writeb(aux, value, DP_LANE_COUNT_SET);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
diff --git a/include/drm/drm_dp_helper.h b/include/drm/drm_dp_helper.h
index 38cbaef05005..bad9ee11a2e2 100644
--- a/include/drm/drm_dp_helper.h
+++ b/include/drm/drm_dp_helper.h
@@ -290,6 +290,7 @@
 #define DP_SET_POWER                        0x600
 # define DP_SET_POWER_D0                    0x1
 # define DP_SET_POWER_D3                    0x2
+# define DP_SET_POWER_MASK                  0x3
 
 #define DP_PSR_ERROR_STATUS                 0x2006  /* XXX 1.2? */
 # define DP_PSR_LINK_CRC_ERROR              (1 << 0)
@@ -464,4 +465,33 @@ static inline ssize_t drm_dp_dpcd_writeb(struct drm_dp_aux *aux, u8 value,
 int drm_dp_dpcd_read_link_status(struct drm_dp_aux *aux,
 				 u8 status[DP_LINK_STATUS_SIZE]);
 
+/*
+ * DisplayPort link
+ */
+#define DP_LINK_CAP_ENHANCED_FRAMING (1 << 0)
+
+struct drm_dp_link {
+	unsigned int rate;
+	unsigned int num_lanes;
+	unsigned long capabilities;
+};
+
+/**
+ * drm_dp_link_probe() - probe link properties and capabilities
+ * @aux: DisplayPort AUX channel
+ * @link: DisplayPort link
+ *
+ * Returns 0 on success or a negative error code on failure.
+ */
+int drm_dp_link_probe(struct drm_dp_aux *aux, struct drm_dp_link *link);
+
+/**
+ * drm_dp_link_power_up() - power up a link
+ * @aux: DisplayPort AUX channel
+ * @link: DisplayPort link
+ *
+ * Returns 0 on success or a negative error code on failure.
+ */
+int drm_dp_link_power_up(struct drm_dp_aux *aux, struct drm_dp_link *link);
+
 #endif /* _DRM_DP_HELPER_H_ */
-- 
1.8.4.2