drm/amd/display: Fix phy id mapping issue for secure display

[Why]
Under mst scenario, mst streams are from the same link_enc_hw_inst.
As the result, can't utilize that as the phy index for distinguising
different stream sinks.

[How]
Sort the connectors by:
link_enc_hw_instance->mst tree depth->mst RAD

After sorting the phy index assignment, store connector's relevant info
into dm mapping array. Once need the index, just look up the static
array.

Reviewed-by: HaoPing Liu <haoping.liu@amd.com>
Signed-off-by: Wayne Lin <Wayne.Lin@amd.com>
Signed-off-by: Aurabindo Pillai <aurabindo.pillai@amd.com>
Tested-by: Daniel Wheeler <daniel.wheeler@amd.com>
Signed-off-by: Alex Deucher <alexander.deucher@amd.com>

drivers/gpu/drm/amd/display/amdgpu_dm/amdgpu_dm.h

@@ -547,6 +547,10 @@ struct amdgpu_display_manager {
 	 * all crtcs.
 	 */
 	struct secure_display_context *secure_display_ctxs;
+
+	bool secure_display_phy_mapping_updated;
+	int phy_id_mapping_cnt;
+	struct phy_id_mapping phy_id_mapping[AMDGPU_DM_MAX_CRTC];
 #endif
 	/**
 	 * @hpd_rx_offload_wq:

drivers/gpu/drm/amd/display/amdgpu_dm/amdgpu_dm_crc.c

@@ -83,13 +83,226 @@ const char *const *amdgpu_dm_crtc_get_crc_sources(struct drm_crtc *crtc,
 }
 
 #ifdef CONFIG_DRM_AMD_SECURE_DISPLAY
+static void update_phy_id_mapping(struct amdgpu_device *adev)
+{
+	struct drm_device *ddev = adev_to_drm(adev);
+	struct amdgpu_display_manager *dm = &adev->dm;
+	struct drm_connector *connector;
+	struct amdgpu_dm_connector *aconnector;
+	struct amdgpu_dm_connector *sort_connector[AMDGPU_DM_MAX_CRTC] = {NULL};
+	struct drm_connector_list_iter iter;
+	uint8_t idx = 0, idx_2 = 0, connector_cnt = 0;
+
+	dm->secure_display_phy_mapping_updated = false;
+
+	mutex_lock(&ddev->mode_config.mutex);
+	drm_connector_list_iter_begin(ddev, &iter);
+	drm_for_each_connector_iter(connector, &iter) {
+
+		if (connector->status != connector_status_connected)
+			continue;
+
+		if (idx >= AMDGPU_DM_MAX_CRTC) {
+			DRM_WARN("%s connected connectors exceed max crtc\n", __func__);
+			mutex_unlock(&ddev->mode_config.mutex);
+			return;
+		}
+
+		aconnector = to_amdgpu_dm_connector(connector);
+
+		sort_connector[idx] = aconnector;
+		idx++;
+		connector_cnt++;
+	}
+	drm_connector_list_iter_end(&iter);
+
+	/* sort connectors by link_enc_hw_instance first */
+	for (idx = connector_cnt; idx > 1 ; idx--) {
+		for (idx_2 = 0; idx_2 < (idx - 1); idx_2++) {
+			if (sort_connector[idx_2]->dc_link->link_enc_hw_inst >
+				sort_connector[idx_2 + 1]->dc_link->link_enc_hw_inst) {
+				aconnector = sort_connector[idx_2];
+				sort_connector[idx_2] = sort_connector[idx_2 + 1];
+				sort_connector[idx_2 + 1] = aconnector;
+			}
+		}
+	}
+
+	/*
+	 * Sort mst connectors by RAD. mst connectors with the same enc_hw_instance are already
+	 * sorted together above.
+	 */
+	for (idx = 0; idx < connector_cnt; /*Do nothing*/) {
+		if (sort_connector[idx]->mst_root) {
+			uint8_t i, j, k;
+			uint8_t mst_con_cnt = 1;
+
+			for (idx_2 = (idx + 1); idx_2 < connector_cnt; idx_2++) {
+				if (sort_connector[idx_2]->mst_root == sort_connector[idx]->mst_root)
+					mst_con_cnt++;
+				else
+					break;
+			}
+
+			for (i = mst_con_cnt; i > 1; i--) {
+				for (j = idx; j < (idx + i - 2); j++) {
+					int mstb_lct = sort_connector[j]->mst_output_port->parent->lct;
+					int next_mstb_lct = sort_connector[j + 1]->mst_output_port->parent->lct;
+					u8 *rad;
+					u8 *next_rad;
+					bool swap = false;
+
+					/* Sort by mst tree depth first. Then compare RAD if depth is the same*/
+					if (mstb_lct > next_mstb_lct) {
+						swap = true;
+					} else if (mstb_lct == next_mstb_lct) {
+						if (mstb_lct == 1) {
+							if (sort_connector[j]->mst_output_port->port_num > sort_connector[j + 1]->mst_output_port->port_num)
+								swap = true;
+						} else if (mstb_lct > 1) {
+							rad = sort_connector[j]->mst_output_port->parent->rad;
+							next_rad = sort_connector[j + 1]->mst_output_port->parent->rad;
+
+							for (k = 0; k < mstb_lct - 1; k++) {
+								int shift = (k % 2) ? 0 : 4;
+								int port_num = (rad[k / 2] >> shift) & 0xf;
+								int next_port_num = (next_rad[k / 2] >> shift) & 0xf;
+
+								if (port_num > next_port_num) {
+									swap = true;
+									break;
+								}
+							}
+						} else {
+							DRM_ERROR("MST LCT shouldn't be set as < 1");
+							mutex_unlock(&ddev->mode_config.mutex);
+							return;
+						}
+					}
+
+					if (swap) {
+						aconnector = sort_connector[j];
+						sort_connector[j] = sort_connector[j + 1];
+						sort_connector[j + 1] = aconnector;
+					}
+				}
+			}
+
+			idx += mst_con_cnt;
+		} else {
+			idx++;
+		}
+	}
+
+	/* Complete sorting. Assign relavant result to dm->phy_id_mapping[]*/
+	memset(dm->phy_id_mapping, 0, sizeof(dm->phy_id_mapping));
+	for (idx = 0; idx < connector_cnt; idx++) {
+		aconnector = sort_connector[idx];
+
+		dm->phy_id_mapping[idx].assigned = true;
+		dm->phy_id_mapping[idx].is_mst = false;
+		dm->phy_id_mapping[idx].enc_hw_inst = aconnector->dc_link->link_enc_hw_inst;
+
+		if (sort_connector[idx]->mst_root) {
+			dm->phy_id_mapping[idx].is_mst = true;
+			dm->phy_id_mapping[idx].lct = aconnector->mst_output_port->parent->lct;
+			dm->phy_id_mapping[idx].port_num = aconnector->mst_output_port->port_num;
+			memcpy(dm->phy_id_mapping[idx].rad, aconnector->mst_output_port->parent->rad,
+				sizeof(aconnector->mst_output_port->parent->rad));
+		}
+	}
+	mutex_unlock(&ddev->mode_config.mutex);
+
+	dm->phy_id_mapping_cnt = connector_cnt;
+	dm->secure_display_phy_mapping_updated = true;
+}
+
+static bool get_phy_id(struct amdgpu_display_manager *dm,
+			struct amdgpu_dm_connector *aconnector, uint8_t *phy_id)
+{
+	int idx, idx_2;
+	bool found = false;
+
+	/*
+	 * Assume secure display start after all connectors are probed. The connection
+	 * config is static as well
+	 */
+	if (!dm->secure_display_phy_mapping_updated) {
+		DRM_WARN("%s Should update the phy id table before get it's value", __func__);
+		return false;
+	}
+
+	for (idx = 0; idx < dm->phy_id_mapping_cnt; idx++) {
+		if (!dm->phy_id_mapping[idx].assigned) {
+			DRM_ERROR("phy_id_mapping[%d] should be assigned", idx);
+			return false;
+		}
+
+		if (aconnector->dc_link->link_enc_hw_inst == dm->phy_id_mapping[idx].enc_hw_inst) {
+			if (!dm->phy_id_mapping[idx].is_mst) {
+				found = true;
+				goto out;
+			} else {
+				/* Could caused by wrongly pass mst root connector */
+				if (!aconnector->mst_output_port) {
+					DRM_ERROR("%s Check mst case but connector without a port assigned", __func__);
+					return false;
+				}
+
+				if (aconnector->mst_root &&
+					aconnector->mst_root->mst_mgr.mst_primary == NULL) {
+					DRM_WARN("%s pass in a stale mst connector", __func__);
+				}
+
+				if (aconnector->mst_output_port->parent->lct == dm->phy_id_mapping[idx].lct &&
+					aconnector->mst_output_port->port_num == dm->phy_id_mapping[idx].port_num) {
+					if (aconnector->mst_output_port->parent->lct == 1) {
+						found = true;
+						goto out;
+					} else if (aconnector->mst_output_port->parent->lct > 1) {
+						/* Check RAD */
+						for (idx_2 = 0; idx_2 < aconnector->mst_output_port->parent->lct - 1; idx_2++) {
+							int shift = (idx_2 % 2) ? 0 : 4;
+							int port_num = (aconnector->mst_output_port->parent->rad[idx_2 / 2] >> shift) & 0xf;
+							int port_num2 = (dm->phy_id_mapping[idx].rad[idx_2 / 2] >> shift) & 0xf;
+
+							if (port_num != port_num2)
+								break;
+						}
+
+						if (idx_2 == aconnector->mst_output_port->parent->lct - 1) {
+							found = true;
+							goto out;
+						}
+					} else {
+						DRM_ERROR("lCT should be >= 1");
+						return false;
+					}
+				}
+			}
+		}
+	}
+
+out:
+	if (found) {
+		DRM_DEBUG_DRIVER("Associated secure display PHY ID as %d", idx);
+		*phy_id = idx;
+	} else {
+		DRM_WARN("Can't find associated phy ID");
+		return false;
+	}
+
+	return true;
+}
+
 static void amdgpu_dm_set_crc_window_default(struct drm_crtc *crtc, struct dc_stream_state *stream)
 {
 	struct drm_device *drm_dev = crtc->dev;
 	struct amdgpu_display_manager *dm = &drm_to_adev(drm_dev)->dm;
 	struct amdgpu_crtc *acrtc = to_amdgpu_crtc(crtc);
 	bool was_activated;
-	uint8_t phy_id = stream->link->link_enc_hw_inst;
+	struct amdgpu_dm_connector *aconnector;
+	uint8_t phy_id;
 
 	spin_lock_irq(&drm_dev->event_lock);
 	was_activated = acrtc->dm_irq_params.window_param.activated;
@@ -107,7 +320,13 @@ static void amdgpu_dm_set_crc_window_default(struct drm_crtc *crtc, struct dc_st
 		/* stop ROI update on this crtc */
 		flush_work(&dm->secure_display_ctxs[crtc->index].notify_ta_work);
 		flush_work(&dm->secure_display_ctxs[crtc->index].forward_roi_work);
-		dc_stream_forward_crc_window(stream, NULL, phy_id, true);
+
+		aconnector = (struct amdgpu_dm_connector *)stream->dm_stream_context;
+
+		if (aconnector && get_phy_id(dm, aconnector, &phy_id))
+			dc_stream_forward_crc_window(stream, NULL, phy_id, true);
+		else
+			DRM_DEBUG_DRIVER("%s Can't find matching phy id", __func__);
 	}
 }
 
@@ -118,7 +337,9 @@ static void amdgpu_dm_crtc_notify_ta_to_read(struct work_struct *work)
 	struct ta_securedisplay_cmd *securedisplay_cmd;
 	struct drm_crtc *crtc;
 	struct dc_stream_state *stream;
+	struct amdgpu_dm_connector *aconnector;
 	uint8_t phy_inst;
+	struct amdgpu_display_manager *dm;
 	int ret;
 
 	secure_display_ctx = container_of(work, struct secure_display_context, notify_ta_work);
@@ -134,8 +355,19 @@ static void amdgpu_dm_crtc_notify_ta_to_read(struct work_struct *work)
 		return;
 	}
 
+	dm = &drm_to_adev(crtc->dev)->dm;
 	stream = to_amdgpu_crtc(crtc)->dm_irq_params.stream;
-	phy_inst = stream->link->link_enc_hw_inst;
+	aconnector = (struct amdgpu_dm_connector *)stream->dm_stream_context;
+	if (!aconnector)
+		return;
+
+	mutex_lock(&crtc->dev->mode_config.mutex);
+	if (!get_phy_id(dm, aconnector, &phy_inst)) {
+		DRM_WARN("%s Can't find mapping phy id!", __func__);
+		mutex_unlock(&crtc->dev->mode_config.mutex);
+		return;
+	}
+	mutex_unlock(&crtc->dev->mode_config.mutex);
 
 	/* need lock for multiple crtcs to use the command buffer */
 	mutex_lock(&psp->securedisplay_context.mutex);
@@ -165,6 +397,8 @@ amdgpu_dm_forward_crc_window(struct work_struct *work)
 	struct amdgpu_display_manager *dm;
 	struct drm_crtc *crtc;
 	struct dc_stream_state *stream;
+	struct amdgpu_dm_connector *aconnector;
+	uint8_t phy_id;
 
 	secure_display_ctx = container_of(work, struct secure_display_context, forward_roi_work);
 	crtc = secure_display_ctx->crtc;
@@ -174,10 +408,22 @@ amdgpu_dm_forward_crc_window(struct work_struct *work)
 
 	dm = &drm_to_adev(crtc->dev)->dm;
 	stream = to_amdgpu_crtc(crtc)->dm_irq_params.stream;
+	aconnector = (struct amdgpu_dm_connector *)stream->dm_stream_context;
+
+	if (!aconnector)
+		return;
+
+	mutex_lock(&crtc->dev->mode_config.mutex);
+	if (!get_phy_id(dm, aconnector, &phy_id)) {
+		DRM_WARN("%s Can't find mapping phy id!", __func__);
+		mutex_unlock(&crtc->dev->mode_config.mutex);
+		return;
+	}
+	mutex_unlock(&crtc->dev->mode_config.mutex);
 
 	mutex_lock(&dm->dc_lock);
 	dc_stream_forward_crc_window(stream, &secure_display_ctx->rect,
-		stream->link->link_enc_hw_inst, false);
+		phy_id, false);
 	mutex_unlock(&dm->dc_lock);
 }
 
@@ -260,6 +506,10 @@ int amdgpu_dm_crtc_set_crc_source(struct drm_crtc *crtc, const char *src_name)
 	struct drm_crtc_commit *commit;
 	struct dm_crtc_state *crtc_state;
 	struct drm_device *drm_dev = crtc->dev;
+#if defined(CONFIG_DRM_AMD_SECURE_DISPLAY)
+	struct amdgpu_device *adev = drm_to_adev(drm_dev);
+	struct amdgpu_display_manager *dm = &adev->dm;
+#endif
 	struct amdgpu_crtc *acrtc = to_amdgpu_crtc(crtc);
 	struct drm_dp_aux *aux = NULL;
 	bool enable = false;
@@ -404,6 +654,12 @@ int amdgpu_dm_crtc_set_crc_source(struct drm_crtc *crtc, const char *src_name)
 	/* Reset crc_skipped on dm state */
 	crtc_state->crc_skip_count = 0;
 
+#if defined(CONFIG_DRM_AMD_SECURE_DISPLAY)
+	/* Initialize phy id mapping table for secure display*/
+	if (!dm->secure_display_phy_mapping_updated)
+		update_phy_id_mapping(adev);
+#endif
+
 cleanup:
 	if (commit)
 		drm_crtc_commit_put(commit);

drivers/gpu/drm/amd/display/amdgpu_dm/amdgpu_dm_crc.h

@@ -40,6 +40,15 @@ enum amdgpu_dm_pipe_crc_source {
 };
 
 #ifdef CONFIG_DRM_AMD_SECURE_DISPLAY
+struct phy_id_mapping {
+	bool assigned;
+	bool is_mst;
+	uint8_t enc_hw_inst;
+	u8 lct;
+	u8 port_num;
+	u8 rad[8];
+};
+
 struct crc_window_param {
 	uint16_t x_start;
 	uint16_t y_start;