drm/mst: rework payload table allocation to conform better.

The old code has problems with the Dell MST monitors due to some
assumptions I made that weren't true.

I initially thought the Virtual Channel Payload IDs had to be in
the DPCD table in ascending order, however it appears that assumption
is bogus.

The old code also assumed it was possible to insert a member
into the table and it would move other members up, like it does
when you remove table entries, however reality has shown this
isn't true.

So the new code allocates VCPIs separate from entries in the payload
tracking table, and when we remove an entry from the DPCD table,
I shuffle the tracking payload entries around in the struct.

This appears to make VT switch more robust (still not perfect)
with an MST enabled Dell monitor.

Signed-off-by: Dave Airlie <[e-mail]>

drivers/gpu/drm/drm_dp_mst_topology.c

···
 static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
 					struct drm_dp_vcpi *vcpi)
 {
-	int ret;
+	int ret, vcpi_ret;
 
 	mutex_lock(&mgr->payload_lock);
 	ret = find_first_zero_bit(&mgr->payload_mask, mgr->max_payloads + 1);
···
 		goto out_unlock;
 	}
 
+	vcpi_ret = find_first_zero_bit(&mgr->vcpi_mask, mgr->max_payloads + 1);
+	if (vcpi_ret > mgr->max_payloads) {
+		ret = -EINVAL;
+		DRM_DEBUG_KMS("out of vcpi ids %d\n", ret);
+		goto out_unlock;
+	}
+
 	set_bit(ret, &mgr->payload_mask);
-	vcpi->vcpi = ret;
+	set_bit(vcpi_ret, &mgr->vcpi_mask);
+	vcpi->vcpi = vcpi_ret + 1;
 	mgr->proposed_vcpis[ret - 1] = vcpi;
 out_unlock:
 	mutex_unlock(&mgr->payload_lock);
···
 }
 
 static void drm_dp_mst_put_payload_id(struct drm_dp_mst_topology_mgr *mgr,
-				      int id)
+				      int vcpi)
 {
-	if (id == 0)
+	int i;
+	if (vcpi == 0)
 		return;
 
 	mutex_lock(&mgr->payload_lock);
-	DRM_DEBUG_KMS("putting payload %d\n", id);
-	clear_bit(id, &mgr->payload_mask);
-	mgr->proposed_vcpis[id - 1] = NULL;
+	DRM_DEBUG_KMS("putting payload %d\n", vcpi);
+	clear_bit(vcpi - 1, &mgr->vcpi_mask);
+
+	for (i = 0; i < mgr->max_payloads; i++) {
+		if (mgr->proposed_vcpis[i])
+			if (mgr->proposed_vcpis[i]->vcpi == vcpi) {
+				mgr->proposed_vcpis[i] = NULL;
+				clear_bit(i + 1, &mgr->payload_mask);
+			}
+	}
 	mutex_unlock(&mgr->payload_lock);
 }
 
···
 	}
 
 	drm_dp_dpcd_write_payload(mgr, id, payload);
-	payload->payload_state = 0;
+	payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
 	return 0;
 }
 
···
  */
 int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
 {
-	int i;
+	int i, j;
 	int cur_slots = 1;
 	struct drm_dp_payload req_payload;
 	struct drm_dp_mst_port *port;
···
 			port = NULL;
 			req_payload.num_slots = 0;
 		}
+
+		if (mgr->payloads[i].start_slot != req_payload.start_slot) {
+			mgr->payloads[i].start_slot = req_payload.start_slot;
+		}
 		/* work out what is required to happen with this payload */
-		if (mgr->payloads[i].start_slot != req_payload.start_slot ||
-		    mgr->payloads[i].num_slots != req_payload.num_slots) {
+		if (mgr->payloads[i].num_slots != req_payload.num_slots) {
 
 			/* need to push an update for this payload */
 			if (req_payload.num_slots) {
-				drm_dp_create_payload_step1(mgr, i + 1, &req_payload);
+				drm_dp_create_payload_step1(mgr, mgr->proposed_vcpis[i]->vcpi, &req_payload);
 				mgr->payloads[i].num_slots = req_payload.num_slots;
 			} else if (mgr->payloads[i].num_slots) {
 				mgr->payloads[i].num_slots = 0;
-				drm_dp_destroy_payload_step1(mgr, port, i + 1, &mgr->payloads[i]);
+				drm_dp_destroy_payload_step1(mgr, port, port->vcpi.vcpi, &mgr->payloads[i]);
 				req_payload.payload_state = mgr->payloads[i].payload_state;
-			} else
-				req_payload.payload_state = 0;
-
-			mgr->payloads[i].start_slot = req_payload.start_slot;
+				mgr->payloads[i].start_slot = 0;
+			}
 			mgr->payloads[i].payload_state = req_payload.payload_state;
 		}
 		cur_slots += req_payload.num_slots;
+	}
+
+	for (i = 0; i < mgr->max_payloads; i++) {
+		if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
+			DRM_DEBUG_KMS("removing payload %d\n", i);
+			for (j = i; j < mgr->max_payloads - 1; j++) {
+				memcpy(&mgr->payloads[j], &mgr->payloads[j + 1], sizeof(struct drm_dp_payload));
+				mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
+				if (mgr->proposed_vcpis[j] && mgr->proposed_vcpis[j]->num_slots) {
+					set_bit(j + 1, &mgr->payload_mask);
+				} else {
+					clear_bit(j + 1, &mgr->payload_mask);
+				}
+			}
+			memset(&mgr->payloads[mgr->max_payloads - 1], 0, sizeof(struct drm_dp_payload));
+			mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
+			clear_bit(mgr->max_payloads, &mgr->payload_mask);
+
+		}
 	}
 	mutex_unlock(&mgr->payload_lock);
 
···
 
 		DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
 		if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
-			ret = drm_dp_create_payload_step2(mgr, port, i + 1, &mgr->payloads[i]);
+			ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
 		} else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
-			ret = drm_dp_destroy_payload_step2(mgr, i + 1, &mgr->payloads[i]);
+			ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
 		}
 		if (ret) {
 			mutex_unlock(&mgr->payload_lock);
···
 		memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
 		mgr->payload_mask = 0;
 		set_bit(0, &mgr->payload_mask);
+		mgr->vcpi_mask = 0;
 	}
 
 out_unlock:
···
 	mutex_unlock(&mgr->lock);
 
 	mutex_lock(&mgr->payload_lock);
-	seq_printf(m, "vcpi: %lx\n", mgr->payload_mask);
+	seq_printf(m, "vcpi: %lx %lx\n", mgr->payload_mask, mgr->vcpi_mask);
 
 	for (i = 0; i < mgr->max_payloads; i++) {
 		if (mgr->proposed_vcpis[i]) {

include/drm/drm_dp_mst_helper.h

···
 	int payload_state;
 	int start_slot;
 	int num_slots;
+	int vcpi;
 };
 
 /**
···
 	struct drm_dp_vcpi **proposed_vcpis;
 	struct drm_dp_payload *payloads;
 	unsigned long payload_mask;
+	unsigned long vcpi_mask;
 
 	wait_queue_head_t tx_waitq;
 	struct work_struct work;