+ req->u.port_num.port_number = (buf[idx] >> 4) & 0xf;
+ break;
+ case DP_ALLOCATE_PAYLOAD:
+ {
+ struct drm_dp_allocate_payload *a =
+ &req->u.allocate_payload;
+
+ a->number_sdp_streams = buf[idx] & 0xf;
+ a->port_number = (buf[idx] >> 4) & 0xf;
+
+ WARN_ON(buf[++idx] & 0x80);
+ a->vcpi = buf[idx] & 0x7f;
+
+ a->pbn = buf[++idx] << 8;
+ a->pbn |= buf[++idx];
+
+ idx++;
+ for (i = 0; i < a->number_sdp_streams; i++) {
+ a->sdp_stream_sink[i] =
+ (buf[idx + (i / 2)] >> ((i % 2) ? 0 : 4)) & 0xf;
+ }
+ }
+ break;
+ case DP_QUERY_PAYLOAD:
+ req->u.query_payload.port_number = (buf[idx] >> 4) & 0xf;
+ WARN_ON(buf[++idx] & 0x80);
+ req->u.query_payload.vcpi = buf[idx] & 0x7f;
+ break;
+ case DP_REMOTE_DPCD_READ:
+ {
+ struct drm_dp_remote_dpcd_read *r = &req->u.dpcd_read;
+
+ r->port_number = (buf[idx] >> 4) & 0xf;
+
+ r->dpcd_address = (buf[idx] << 16) & 0xf0000;
+ r->dpcd_address |= (buf[++idx] << 8) & 0xff00;
+ r->dpcd_address |= buf[++idx] & 0xff;
+
+ r->num_bytes = buf[++idx];
+ }
+ break;
+ case DP_REMOTE_DPCD_WRITE:
+ {
+ struct drm_dp_remote_dpcd_write *w =
+ &req->u.dpcd_write;
+
+ w->port_number = (buf[idx] >> 4) & 0xf;
+
+ w->dpcd_address = (buf[idx] << 16) & 0xf0000;
+ w->dpcd_address |= (buf[++idx] << 8) & 0xff00;
+ w->dpcd_address |= buf[++idx] & 0xff;
+
+ w->num_bytes = buf[++idx];
+
+ w->bytes = kmemdup(&buf[++idx], w->num_bytes,
+ GFP_KERNEL);
+ if (!w->bytes)
+ return -ENOMEM;
+ }
+ break;
+ case DP_REMOTE_I2C_READ:
+ {
+ struct drm_dp_remote_i2c_read *r = &req->u.i2c_read;
+ struct drm_dp_remote_i2c_read_tx *tx;
+ bool failed = false;
+
+ r->num_transactions = buf[idx] & 0x3;
+ r->port_number = (buf[idx] >> 4) & 0xf;
+ for (i = 0; i < r->num_transactions; i++) {
+ tx = &r->transactions[i];
+
+ tx->i2c_dev_id = buf[++idx] & 0x7f;
+ tx->num_bytes = buf[++idx];
+ tx->bytes = kmemdup(&buf[++idx],
+ tx->num_bytes,
+ GFP_KERNEL);
+ if (!tx->bytes) {
+ failed = true;
+ break;
+ }
+ idx += tx->num_bytes;
+ tx->no_stop_bit = (buf[idx] >> 5) & 0x1;
+ tx->i2c_transaction_delay = buf[idx] & 0xf;
+ }
+
+ if (failed) {
+ for (i = 0; i < r->num_transactions; i++)
+ kfree(tx->bytes);
+ return -ENOMEM;
+ }
+
+ r->read_i2c_device_id = buf[++idx] & 0x7f;
+ r->num_bytes_read = buf[++idx];
+ }
+ break;
+ case DP_REMOTE_I2C_WRITE:
+ {
+ struct drm_dp_remote_i2c_write *w = &req->u.i2c_write;
+
+ w->port_number = (buf[idx] >> 4) & 0xf;
+ w->write_i2c_device_id = buf[++idx] & 0x7f;
+ w->num_bytes = buf[++idx];
+ w->bytes = kmemdup(&buf[++idx], w->num_bytes,
+ GFP_KERNEL);
+ if (!w->bytes)
+ return -ENOMEM;
+ }
+ break;
+ }
+
+ return 0;
+}
+EXPORT_SYMBOL_FOR_TESTS_ONLY(drm_dp_decode_sideband_req);
+
+void
+drm_dp_dump_sideband_msg_req_body(const struct drm_dp_sideband_msg_req_body *req,
+ int indent, struct drm_printer *printer)
+{
+ int i;
+
+#define P(f, ...) drm_printf_indent(printer, indent, f, ##__VA_ARGS__)
+ if (req->req_type == DP_LINK_ADDRESS) {
+ /* No contents to print */
+ P("type=%s\n", drm_dp_mst_req_type_str(req->req_type));
+ return;
+ }
+
+ P("type=%s contents:\n", drm_dp_mst_req_type_str(req->req_type));
+ indent++;
+
+ switch (req->req_type) {
+ case DP_ENUM_PATH_RESOURCES:
+ case DP_POWER_DOWN_PHY:
+ case DP_POWER_UP_PHY:
+ P("port=%d\n", req->u.port_num.port_number);
+ break;
+ case DP_ALLOCATE_PAYLOAD:
+ P("port=%d vcpi=%d pbn=%d sdp_streams=%d %*ph\n",
+ req->u.allocate_payload.port_number,
+ req->u.allocate_payload.vcpi, req->u.allocate_payload.pbn,
+ req->u.allocate_payload.number_sdp_streams,
+ req->u.allocate_payload.number_sdp_streams,
+ req->u.allocate_payload.sdp_stream_sink);
+ break;
+ case DP_QUERY_PAYLOAD:
+ P("port=%d vcpi=%d\n",
+ req->u.query_payload.port_number,
+ req->u.query_payload.vcpi);
+ break;
+ case DP_REMOTE_DPCD_READ:
+ P("port=%d dpcd_addr=%05x len=%d\n",
+ req->u.dpcd_read.port_number, req->u.dpcd_read.dpcd_address,
+ req->u.dpcd_read.num_bytes);
+ break;
+ case DP_REMOTE_DPCD_WRITE:
+ P("port=%d addr=%05x len=%d: %*ph\n",
+ req->u.dpcd_write.port_number,
+ req->u.dpcd_write.dpcd_address,
+ req->u.dpcd_write.num_bytes, req->u.dpcd_write.num_bytes,
+ req->u.dpcd_write.bytes);
+ break;
+ case DP_REMOTE_I2C_READ:
+ P("port=%d num_tx=%d id=%d size=%d:\n",
+ req->u.i2c_read.port_number,
+ req->u.i2c_read.num_transactions,
+ req->u.i2c_read.read_i2c_device_id,
+ req->u.i2c_read.num_bytes_read);
+
+ indent++;
+ for (i = 0; i < req->u.i2c_read.num_transactions; i++) {
+ const struct drm_dp_remote_i2c_read_tx *rtx =
+ &req->u.i2c_read.transactions[i];
+
+ P("%d: id=%03d size=%03d no_stop_bit=%d tx_delay=%03d: %*ph\n",
+ i, rtx->i2c_dev_id, rtx->num_bytes,
+ rtx->no_stop_bit, rtx->i2c_transaction_delay,
+ rtx->num_bytes, rtx->bytes);
+ }
+ break;
+ case DP_REMOTE_I2C_WRITE:
+ P("port=%d id=%d size=%d: %*ph\n",
+ req->u.i2c_write.port_number,
+ req->u.i2c_write.write_i2c_device_id,
+ req->u.i2c_write.num_bytes, req->u.i2c_write.num_bytes,
+ req->u.i2c_write.bytes);
+ break;
+ default:
+ P("???\n");
+ break;
+ }
+#undef P
+}
+EXPORT_SYMBOL_FOR_TESTS_ONLY(drm_dp_dump_sideband_msg_req_body);
+
+static inline void
+drm_dp_mst_dump_sideband_msg_tx(struct drm_printer *p,
+ const struct drm_dp_sideband_msg_tx *txmsg)
+{
+ struct drm_dp_sideband_msg_req_body req;
+ char buf[64];
+ int ret;
+ int i;
+
+ drm_dp_mst_rad_to_str(txmsg->dst->rad, txmsg->dst->lct, buf,
+ sizeof(buf));
+ drm_printf(p, "txmsg cur_offset=%x cur_len=%x seqno=%x state=%s path_msg=%d dst=%s\n",
+ txmsg->cur_offset, txmsg->cur_len, txmsg->seqno,
+ drm_dp_mst_sideband_tx_state_str(txmsg->state),
+ txmsg->path_msg, buf);
+
+ ret = drm_dp_decode_sideband_req(txmsg, &req);
+ if (ret) {
+ drm_printf(p, "<failed to decode sideband req: %d>\n", ret);
+ return;
+ }
+ drm_dp_dump_sideband_msg_req_body(&req, 1, p);
+
+ switch (req.req_type) {
+ case DP_REMOTE_DPCD_WRITE:
+ kfree(req.u.dpcd_write.bytes);
+ break;
+ case DP_REMOTE_I2C_READ:
+ for (i = 0; i < req.u.i2c_read.num_transactions; i++)
+ kfree(req.u.i2c_read.transactions[i].bytes);
+ break;
+ case DP_REMOTE_I2C_WRITE:
+ kfree(req.u.i2c_write.bytes);