diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index b3adf1445020..070f913d2dba 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -682,7 +682,7 @@ static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_n
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);
@@ -692,8 +692,16 @@ static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
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);
@@ -701,15 +709,23 @@ static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
}
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);
}
@@ -1563,7 +1579,7 @@ static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
}
drm_dp_dpcd_write_payload(mgr, id, payload);
- payload->payload_state = 0;
+ payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
return 0;
}
@@ -1590,7 +1606,7 @@ static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
*/
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;
@@ -1607,26 +1623,46 @@ int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
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);
return 0;
@@ -1657,9 +1693,9 @@ int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
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);
@@ -1861,6 +1897,7 @@ int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool ms
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:
@@ -2475,7 +2512,7 @@ void drm_dp_mst_dump_topology(struct seq_file *m,
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]) {
diff --git a/include/drm/drm_dp_mst_helper.h b/include/drm/drm_dp_mst_helper.h
index 9b446ada2532..338fc1053835 100644
--- a/include/drm/drm_dp_mst_helper.h
+++ b/include/drm/drm_dp_mst_helper.h
@@ -388,6 +388,7 @@ struct drm_dp_payload {
int payload_state;
int start_slot;
int num_slots;
+ int vcpi;
};
/**
@@ -454,6 +455,7 @@ struct drm_dp_mst_topology_mgr {
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;