WebSVN – Kolibri OS – Blame – /drivers/video/drm/drm_dp_mst_topology.c

Rev	Author	Line No.	Line
5060	serge	1	/*
		2	* Copyright © 2014 Red Hat
		3	*
		4	* Permission to use, copy, modify, distribute, and sell this software and its
		5	* documentation for any purpose is hereby granted without fee, provided that
		6	* the above copyright notice appear in all copies and that both that copyright
		7	* notice and this permission notice appear in supporting documentation, and
		8	* that the name of the copyright holders not be used in advertising or
		9	* publicity pertaining to distribution of the software without specific,
		10	* written prior permission. The copyright holders make no representations
		11	* about the suitability of this software for any purpose. It is provided "as
		12	* is" without express or implied warranty.
		13	*
		14	* THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
		15	* INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
		16	* EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
		17	* CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
		18	* DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
		19	* TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
		20	* OF THIS SOFTWARE.
		21	*/
		22
		23	#include
		24	#include
6088	serge	25	#include
5060	serge	26	#include
		27	#include
		28	#include
		29	#include
		30	#include
		31	#include
		32
		33	#include
		34
		35	u64 get_jiffies_64(void)
		36	{
		37	return jiffies;
		38	}
		39	/**
		40	* DOC: dp mst helper
		41	*
		42	* These functions contain parts of the DisplayPort 1.2a MultiStream Transport
		43	* protocol. The helpers contain a topology manager and bandwidth manager.
		44	* The helpers encapsulate the sending and received of sideband msgs.
		45	*/
		46	static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
		47	char *buf);
		48	static int test_calc_pbn_mode(void);
		49
		50	static void drm_dp_put_port(struct drm_dp_mst_port *port);
		51
		52	static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
		53	int id,
		54	struct drm_dp_payload *payload);
		55
		56	static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
		57	struct drm_dp_mst_port *port,
		58	int offset, int size, u8 *bytes);
		59
6084	serge	60	static void drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
		61	struct drm_dp_mst_branch *mstb);
5060	serge	62	static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
		63	struct drm_dp_mst_branch *mstb,
		64	struct drm_dp_mst_port *port);
		65	static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
		66	u8 *guid);
		67
		68	static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux);
		69	static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux);
		70	static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr);
		71	/* sideband msg handling */
		72	static u8 drm_dp_msg_header_crc4(const uint8_t *data, size_t num_nibbles)
		73	{
		74	u8 bitmask = 0x80;
		75	u8 bitshift = 7;
		76	u8 array_index = 0;
		77	int number_of_bits = num_nibbles * 4;
		78	u8 remainder = 0;
		79
		80	while (number_of_bits != 0) {
		81	number_of_bits--;
		82	remainder <<= 1;
		83	remainder \|= (data[array_index] & bitmask) >> bitshift;
		84	bitmask >>= 1;
		85	bitshift--;
		86	if (bitmask == 0) {
		87	bitmask = 0x80;
		88	bitshift = 7;
		89	array_index++;
		90	}
		91	if ((remainder & 0x10) == 0x10)
		92	remainder ^= 0x13;
		93	}
		94
		95	number_of_bits = 4;
		96	while (number_of_bits != 0) {
		97	number_of_bits--;
		98	remainder <<= 1;
		99	if ((remainder & 0x10) != 0)
		100	remainder ^= 0x13;
		101	}
		102
		103	return remainder;
		104	}
		105
		106	static u8 drm_dp_msg_data_crc4(const uint8_t *data, u8 number_of_bytes)
		107	{
		108	u8 bitmask = 0x80;
		109	u8 bitshift = 7;
		110	u8 array_index = 0;
		111	int number_of_bits = number_of_bytes * 8;
		112	u16 remainder = 0;
		113
		114	while (number_of_bits != 0) {
		115	number_of_bits--;
		116	remainder <<= 1;
		117	remainder \|= (data[array_index] & bitmask) >> bitshift;
		118	bitmask >>= 1;
		119	bitshift--;
		120	if (bitmask == 0) {
		121	bitmask = 0x80;
		122	bitshift = 7;
		123	array_index++;
		124	}
		125	if ((remainder & 0x100) == 0x100)
		126	remainder ^= 0xd5;
		127	}
		128
		129	number_of_bits = 8;
		130	while (number_of_bits != 0) {
		131	number_of_bits--;
		132	remainder <<= 1;
		133	if ((remainder & 0x100) != 0)
		134	remainder ^= 0xd5;
		135	}
		136
		137	return remainder & 0xff;
		138	}
		139	static inline u8 drm_dp_calc_sb_hdr_size(struct drm_dp_sideband_msg_hdr *hdr)
		140	{
		141	u8 size = 3;
		142	size += (hdr->lct / 2);
		143	return size;
		144	}
		145
		146	static void drm_dp_encode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
		147	u8 buf, int len)
		148	{
		149	int idx = 0;
		150	int i;
		151	u8 crc4;
		152	buf[idx++] = ((hdr->lct & 0xf) << 4) \| (hdr->lcr & 0xf);
		153	for (i = 0; i < (hdr->lct / 2); i++)
		154	buf[idx++] = hdr->rad[i];
		155	buf[idx++] = (hdr->broadcast << 7) \| (hdr->path_msg << 6) \|
		156	(hdr->msg_len & 0x3f);
		157	buf[idx++] = (hdr->somt << 7) \| (hdr->eomt << 6) \| (hdr->seqno << 4);
		158
		159	crc4 = drm_dp_msg_header_crc4(buf, (idx * 2) - 1);
		160	buf[idx - 1] \|= (crc4 & 0xf);
		161
		162	*len = idx;
		163	}
		164
		165	static bool drm_dp_decode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
		166	u8 buf, int buflen, u8 hdrlen)
		167	{
		168	u8 crc4;
		169	u8 len;
		170	int i;
		171	u8 idx;
		172	if (buf[0] == 0)
		173	return false;
		174	len = 3;
		175	len += ((buf[0] & 0xf0) >> 4) / 2;
		176	if (len > buflen)
		177	return false;
		178	crc4 = drm_dp_msg_header_crc4(buf, (len * 2) - 1);
		179
		180	if ((crc4 & 0xf) != (buf[len - 1] & 0xf)) {
		181	DRM_DEBUG_KMS("crc4 mismatch 0x%x 0x%x\n", crc4, buf[len - 1]);
		182	return false;
		183	}
		184
		185	hdr->lct = (buf[0] & 0xf0) >> 4;
		186	hdr->lcr = (buf[0] & 0xf);
		187	idx = 1;
		188	for (i = 0; i < (hdr->lct / 2); i++)
		189	hdr->rad[i] = buf[idx++];
		190	hdr->broadcast = (buf[idx] >> 7) & 0x1;
		191	hdr->path_msg = (buf[idx] >> 6) & 0x1;
		192	hdr->msg_len = buf[idx] & 0x3f;
		193	idx++;
		194	hdr->somt = (buf[idx] >> 7) & 0x1;
		195	hdr->eomt = (buf[idx] >> 6) & 0x1;
		196	hdr->seqno = (buf[idx] >> 4) & 0x1;
		197	idx++;
		198	*hdrlen = idx;
		199	return true;
		200	}
		201
		202	static void drm_dp_encode_sideband_req(struct drm_dp_sideband_msg_req_body *req,
		203	struct drm_dp_sideband_msg_tx *raw)
		204	{
		205	int idx = 0;
		206	int i;
		207	u8 *buf = raw->msg;
		208	buf[idx++] = req->req_type & 0x7f;
		209
		210	switch (req->req_type) {
		211	case DP_ENUM_PATH_RESOURCES:
		212	buf[idx] = (req->u.port_num.port_number & 0xf) << 4;
		213	idx++;
		214	break;
		215	case DP_ALLOCATE_PAYLOAD:
		216	buf[idx] = (req->u.allocate_payload.port_number & 0xf) << 4 \|
		217	(req->u.allocate_payload.number_sdp_streams & 0xf);
		218	idx++;
		219	buf[idx] = (req->u.allocate_payload.vcpi & 0x7f);
		220	idx++;
		221	buf[idx] = (req->u.allocate_payload.pbn >> 8);
		222	idx++;
		223	buf[idx] = (req->u.allocate_payload.pbn & 0xff);
		224	idx++;
		225	for (i = 0; i < req->u.allocate_payload.number_sdp_streams / 2; i++) {
		226	buf[idx] = ((req->u.allocate_payload.sdp_stream_sink[i * 2] & 0xf) << 4) \|
		227	(req->u.allocate_payload.sdp_stream_sink[i * 2 + 1] & 0xf);
		228	idx++;
		229	}
		230	if (req->u.allocate_payload.number_sdp_streams & 1) {
		231	i = req->u.allocate_payload.number_sdp_streams - 1;
		232	buf[idx] = (req->u.allocate_payload.sdp_stream_sink[i] & 0xf) << 4;
		233	idx++;
		234	}
		235	break;
		236	case DP_QUERY_PAYLOAD:
		237	buf[idx] = (req->u.query_payload.port_number & 0xf) << 4;
		238	idx++;
		239	buf[idx] = (req->u.query_payload.vcpi & 0x7f);
		240	idx++;
		241	break;
		242	case DP_REMOTE_DPCD_READ:
		243	buf[idx] = (req->u.dpcd_read.port_number & 0xf) << 4;
		244	buf[idx] \|= ((req->u.dpcd_read.dpcd_address & 0xf0000) >> 16) & 0xf;
		245	idx++;
		246	buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff00) >> 8;
		247	idx++;
		248	buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff);
		249	idx++;
		250	buf[idx] = (req->u.dpcd_read.num_bytes);
		251	idx++;
		252	break;
		253
		254	case DP_REMOTE_DPCD_WRITE:
		255	buf[idx] = (req->u.dpcd_write.port_number & 0xf) << 4;
		256	buf[idx] \|= ((req->u.dpcd_write.dpcd_address & 0xf0000) >> 16) & 0xf;
		257	idx++;
		258	buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff00) >> 8;
		259	idx++;
		260	buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff);
		261	idx++;
		262	buf[idx] = (req->u.dpcd_write.num_bytes);
		263	idx++;
		264	memcpy(&buf[idx], req->u.dpcd_write.bytes, req->u.dpcd_write.num_bytes);
		265	idx += req->u.dpcd_write.num_bytes;
		266	break;
		267	case DP_REMOTE_I2C_READ:
		268	buf[idx] = (req->u.i2c_read.port_number & 0xf) << 4;
		269	buf[idx] \|= (req->u.i2c_read.num_transactions & 0x3);
		270	idx++;
		271	for (i = 0; i < (req->u.i2c_read.num_transactions & 0x3); i++) {
		272	buf[idx] = req->u.i2c_read.transactions[i].i2c_dev_id & 0x7f;
		273	idx++;
		274	buf[idx] = req->u.i2c_read.transactions[i].num_bytes;
		275	idx++;
		276	memcpy(&buf[idx], req->u.i2c_read.transactions[i].bytes, req->u.i2c_read.transactions[i].num_bytes);
		277	idx += req->u.i2c_read.transactions[i].num_bytes;
		278
		279	buf[idx] = (req->u.i2c_read.transactions[i].no_stop_bit & 0x1) << 5;
		280	buf[idx] \|= (req->u.i2c_read.transactions[i].i2c_transaction_delay & 0xf);
		281	idx++;
		282	}
		283	buf[idx] = (req->u.i2c_read.read_i2c_device_id) & 0x7f;
		284	idx++;
		285	buf[idx] = (req->u.i2c_read.num_bytes_read);
		286	idx++;
		287	break;
		288
		289	case DP_REMOTE_I2C_WRITE:
		290	buf[idx] = (req->u.i2c_write.port_number & 0xf) << 4;
		291	idx++;
		292	buf[idx] = (req->u.i2c_write.write_i2c_device_id) & 0x7f;
		293	idx++;
		294	buf[idx] = (req->u.i2c_write.num_bytes);
		295	idx++;
		296	memcpy(&buf[idx], req->u.i2c_write.bytes, req->u.i2c_write.num_bytes);
		297	idx += req->u.i2c_write.num_bytes;
		298	break;
		299	}
		300	raw->cur_len = idx;
		301	}
		302
		303	static void drm_dp_crc_sideband_chunk_req(u8 *msg, u8 len)
		304	{
		305	u8 crc4;
		306	crc4 = drm_dp_msg_data_crc4(msg, len);
		307	msg[len] = crc4;
		308	}
		309
		310	static void drm_dp_encode_sideband_reply(struct drm_dp_sideband_msg_reply_body *rep,
		311	struct drm_dp_sideband_msg_tx *raw)
		312	{
		313	int idx = 0;
		314	u8 *buf = raw->msg;
		315
		316	buf[idx++] = (rep->reply_type & 0x1) << 7 \| (rep->req_type & 0x7f);
		317
		318	raw->cur_len = idx;
		319	}
		320
		321	/* this adds a chunk of msg to the builder to get the final msg */
		322	static bool drm_dp_sideband_msg_build(struct drm_dp_sideband_msg_rx *msg,
		323	u8 *replybuf, u8 replybuflen, bool hdr)
		324	{
		325	int ret;
		326	u8 crc4;
		327
		328	if (hdr) {
		329	u8 hdrlen;
		330	struct drm_dp_sideband_msg_hdr recv_hdr;
		331	ret = drm_dp_decode_sideband_msg_hdr(&recv_hdr, replybuf, replybuflen, &hdrlen);
		332	if (ret == false) {
		333	print_hex_dump(KERN_DEBUG, "failed hdr", DUMP_PREFIX_NONE, 16, 1, replybuf, replybuflen, false);
		334	return false;
		335	}
		336
		337	/* get length contained in this portion */
		338	msg->curchunk_len = recv_hdr.msg_len;
		339	msg->curchunk_hdrlen = hdrlen;
		340
		341	/* we have already gotten an somt - don't bother parsing */
		342	if (recv_hdr.somt && msg->have_somt)
		343	return false;
		344
		345	if (recv_hdr.somt) {
		346	memcpy(&msg->initial_hdr, &recv_hdr, sizeof(struct drm_dp_sideband_msg_hdr));
		347	msg->have_somt = true;
		348	}
		349	if (recv_hdr.eomt)
		350	msg->have_eomt = true;
		351
		352	/* copy the bytes for the remainder of this header chunk */
		353	msg->curchunk_idx = min(msg->curchunk_len, (u8)(replybuflen - hdrlen));
		354	memcpy(&msg->chunk[0], replybuf + hdrlen, msg->curchunk_idx);
		355	} else {
		356	memcpy(&msg->chunk[msg->curchunk_idx], replybuf, replybuflen);
		357	msg->curchunk_idx += replybuflen;
		358	}
		359
		360	if (msg->curchunk_idx >= msg->curchunk_len) {
		361	/* do CRC */
		362	crc4 = drm_dp_msg_data_crc4(msg->chunk, msg->curchunk_len - 1);
		363	/* copy chunk into bigger msg */
		364	memcpy(&msg->msg[msg->curlen], msg->chunk, msg->curchunk_len - 1);
		365	msg->curlen += msg->curchunk_len - 1;
		366	}
		367	return true;
		368	}
		369
		370	static bool drm_dp_sideband_parse_link_address(struct drm_dp_sideband_msg_rx *raw,
		371	struct drm_dp_sideband_msg_reply_body *repmsg)
		372	{
		373	int idx = 1;
		374	int i;
		375	memcpy(repmsg->u.link_addr.guid, &raw->msg[idx], 16);
		376	idx += 16;
		377	repmsg->u.link_addr.nports = raw->msg[idx] & 0xf;
		378	idx++;
		379	if (idx > raw->curlen)
		380	goto fail_len;
		381	for (i = 0; i < repmsg->u.link_addr.nports; i++) {
		382	if (raw->msg[idx] & 0x80)
		383	repmsg->u.link_addr.ports[i].input_port = 1;
		384
		385	repmsg->u.link_addr.ports[i].peer_device_type = (raw->msg[idx] >> 4) & 0x7;
		386	repmsg->u.link_addr.ports[i].port_number = (raw->msg[idx] & 0xf);
		387
		388	idx++;
		389	if (idx > raw->curlen)
		390	goto fail_len;
		391	repmsg->u.link_addr.ports[i].mcs = (raw->msg[idx] >> 7) & 0x1;
		392	repmsg->u.link_addr.ports[i].ddps = (raw->msg[idx] >> 6) & 0x1;
		393	if (repmsg->u.link_addr.ports[i].input_port == 0)
		394	repmsg->u.link_addr.ports[i].legacy_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
		395	idx++;
		396	if (idx > raw->curlen)
		397	goto fail_len;
		398	if (repmsg->u.link_addr.ports[i].input_port == 0) {
		399	repmsg->u.link_addr.ports[i].dpcd_revision = (raw->msg[idx]);
		400	idx++;
		401	if (idx > raw->curlen)
		402	goto fail_len;
		403	memcpy(repmsg->u.link_addr.ports[i].peer_guid, &raw->msg[idx], 16);
		404	idx += 16;
		405	if (idx > raw->curlen)
		406	goto fail_len;
		407	repmsg->u.link_addr.ports[i].num_sdp_streams = (raw->msg[idx] >> 4) & 0xf;
		408	repmsg->u.link_addr.ports[i].num_sdp_stream_sinks = (raw->msg[idx] & 0xf);
		409	idx++;
		410
		411	}
		412	if (idx > raw->curlen)
		413	goto fail_len;
		414	}
		415
		416	return true;
		417	fail_len:
		418	DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
		419	return false;
		420	}
		421
		422	static bool drm_dp_sideband_parse_remote_dpcd_read(struct drm_dp_sideband_msg_rx *raw,
		423	struct drm_dp_sideband_msg_reply_body *repmsg)
		424	{
		425	int idx = 1;
		426	repmsg->u.remote_dpcd_read_ack.port_number = raw->msg[idx] & 0xf;
		427	idx++;
		428	if (idx > raw->curlen)
		429	goto fail_len;
		430	repmsg->u.remote_dpcd_read_ack.num_bytes = raw->msg[idx];
		431	if (idx > raw->curlen)
		432	goto fail_len;
		433
		434	memcpy(repmsg->u.remote_dpcd_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_dpcd_read_ack.num_bytes);
		435	return true;
		436	fail_len:
		437	DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
		438	return false;
		439	}
		440
		441	static bool drm_dp_sideband_parse_remote_dpcd_write(struct drm_dp_sideband_msg_rx *raw,
		442	struct drm_dp_sideband_msg_reply_body *repmsg)
		443	{
		444	int idx = 1;
		445	repmsg->u.remote_dpcd_write_ack.port_number = raw->msg[idx] & 0xf;
		446	idx++;
		447	if (idx > raw->curlen)
		448	goto fail_len;
		449	return true;
		450	fail_len:
		451	DRM_DEBUG_KMS("parse length fail %d %d\n", idx, raw->curlen);
		452	return false;
		453	}
		454
		455	static bool drm_dp_sideband_parse_remote_i2c_read_ack(struct drm_dp_sideband_msg_rx *raw,
		456	struct drm_dp_sideband_msg_reply_body *repmsg)
		457	{
		458	int idx = 1;
		459
		460	repmsg->u.remote_i2c_read_ack.port_number = (raw->msg[idx] & 0xf);
		461	idx++;
		462	if (idx > raw->curlen)
		463	goto fail_len;
		464	repmsg->u.remote_i2c_read_ack.num_bytes = raw->msg[idx];
		465	idx++;
		466	/* TODO check */
		467	memcpy(repmsg->u.remote_i2c_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_i2c_read_ack.num_bytes);
		468	return true;
		469	fail_len:
		470	DRM_DEBUG_KMS("remote i2c reply parse length fail %d %d\n", idx, raw->curlen);
		471	return false;
		472	}
		473
		474	static bool drm_dp_sideband_parse_enum_path_resources_ack(struct drm_dp_sideband_msg_rx *raw,
		475	struct drm_dp_sideband_msg_reply_body *repmsg)
		476	{
		477	int idx = 1;
		478	repmsg->u.path_resources.port_number = (raw->msg[idx] >> 4) & 0xf;
		479	idx++;
		480	if (idx > raw->curlen)
		481	goto fail_len;
		482	repmsg->u.path_resources.full_payload_bw_number = (raw->msg[idx] << 8) \| (raw->msg[idx+1]);
		483	idx += 2;
		484	if (idx > raw->curlen)
		485	goto fail_len;
		486	repmsg->u.path_resources.avail_payload_bw_number = (raw->msg[idx] << 8) \| (raw->msg[idx+1]);
		487	idx += 2;
		488	if (idx > raw->curlen)
		489	goto fail_len;
		490	return true;
		491	fail_len:
		492	DRM_DEBUG_KMS("enum resource parse length fail %d %d\n", idx, raw->curlen);
		493	return false;
		494	}
		495
		496	static bool drm_dp_sideband_parse_allocate_payload_ack(struct drm_dp_sideband_msg_rx *raw,
		497	struct drm_dp_sideband_msg_reply_body *repmsg)
		498	{
		499	int idx = 1;
		500	repmsg->u.allocate_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
		501	idx++;
		502	if (idx > raw->curlen)
		503	goto fail_len;
		504	repmsg->u.allocate_payload.vcpi = raw->msg[idx];
		505	idx++;
		506	if (idx > raw->curlen)
		507	goto fail_len;
		508	repmsg->u.allocate_payload.allocated_pbn = (raw->msg[idx] << 8) \| (raw->msg[idx+1]);
		509	idx += 2;
		510	if (idx > raw->curlen)
		511	goto fail_len;
		512	return true;
		513	fail_len:
		514	DRM_DEBUG_KMS("allocate payload parse length fail %d %d\n", idx, raw->curlen);
		515	return false;
		516	}
		517
		518	static bool drm_dp_sideband_parse_query_payload_ack(struct drm_dp_sideband_msg_rx *raw,
		519	struct drm_dp_sideband_msg_reply_body *repmsg)
		520	{
		521	int idx = 1;
		522	repmsg->u.query_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
		523	idx++;
		524	if (idx > raw->curlen)
		525	goto fail_len;
		526	repmsg->u.query_payload.allocated_pbn = (raw->msg[idx] << 8) \| (raw->msg[idx + 1]);
		527	idx += 2;
		528	if (idx > raw->curlen)
		529	goto fail_len;
		530	return true;
		531	fail_len:
		532	DRM_DEBUG_KMS("query payload parse length fail %d %d\n", idx, raw->curlen);
		533	return false;
		534	}
		535
		536	static bool drm_dp_sideband_parse_reply(struct drm_dp_sideband_msg_rx *raw,
		537	struct drm_dp_sideband_msg_reply_body *msg)
		538	{
		539	memset(msg, 0, sizeof(*msg));
		540	msg->reply_type = (raw->msg[0] & 0x80) >> 7;
		541	msg->req_type = (raw->msg[0] & 0x7f);
		542
		543	if (msg->reply_type) {
		544	memcpy(msg->u.nak.guid, &raw->msg[1], 16);
		545	msg->u.nak.reason = raw->msg[17];
		546	msg->u.nak.nak_data = raw->msg[18];
		547	return false;
		548	}
		549
		550	switch (msg->req_type) {
		551	case DP_LINK_ADDRESS:
		552	return drm_dp_sideband_parse_link_address(raw, msg);
		553	case DP_QUERY_PAYLOAD:
		554	return drm_dp_sideband_parse_query_payload_ack(raw, msg);
		555	case DP_REMOTE_DPCD_READ:
		556	return drm_dp_sideband_parse_remote_dpcd_read(raw, msg);
		557	case DP_REMOTE_DPCD_WRITE:
		558	return drm_dp_sideband_parse_remote_dpcd_write(raw, msg);
		559	case DP_REMOTE_I2C_READ:
		560	return drm_dp_sideband_parse_remote_i2c_read_ack(raw, msg);
		561	case DP_ENUM_PATH_RESOURCES:
		562	return drm_dp_sideband_parse_enum_path_resources_ack(raw, msg);
		563	case DP_ALLOCATE_PAYLOAD:
		564	return drm_dp_sideband_parse_allocate_payload_ack(raw, msg);
		565	default:
		566	DRM_ERROR("Got unknown reply 0x%02x\n", msg->req_type);
		567	return false;
		568	}
		569	}
		570
		571	static bool drm_dp_sideband_parse_connection_status_notify(struct drm_dp_sideband_msg_rx *raw,
		572	struct drm_dp_sideband_msg_req_body *msg)
		573	{
		574	int idx = 1;
		575
		576	msg->u.conn_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
		577	idx++;
		578	if (idx > raw->curlen)
		579	goto fail_len;
		580
		581	memcpy(msg->u.conn_stat.guid, &raw->msg[idx], 16);
		582	idx += 16;
		583	if (idx > raw->curlen)
		584	goto fail_len;
		585
		586	msg->u.conn_stat.legacy_device_plug_status = (raw->msg[idx] >> 6) & 0x1;
		587	msg->u.conn_stat.displayport_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
		588	msg->u.conn_stat.message_capability_status = (raw->msg[idx] >> 4) & 0x1;
		589	msg->u.conn_stat.input_port = (raw->msg[idx] >> 3) & 0x1;
		590	msg->u.conn_stat.peer_device_type = (raw->msg[idx] & 0x7);
		591	idx++;
		592	return true;
		593	fail_len:
		594	DRM_DEBUG_KMS("connection status reply parse length fail %d %d\n", idx, raw->curlen);
		595	return false;
		596	}
		597
		598	static bool drm_dp_sideband_parse_resource_status_notify(struct drm_dp_sideband_msg_rx *raw,
		599	struct drm_dp_sideband_msg_req_body *msg)
		600	{
		601	int idx = 1;
		602
		603	msg->u.resource_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
		604	idx++;
		605	if (idx > raw->curlen)
		606	goto fail_len;
		607
		608	memcpy(msg->u.resource_stat.guid, &raw->msg[idx], 16);
		609	idx += 16;
		610	if (idx > raw->curlen)
		611	goto fail_len;
		612
		613	msg->u.resource_stat.available_pbn = (raw->msg[idx] << 8) \| (raw->msg[idx + 1]);
		614	idx++;
		615	return true;
		616	fail_len:
		617	DRM_DEBUG_KMS("resource status reply parse length fail %d %d\n", idx, raw->curlen);
		618	return false;
		619	}
		620
		621	static bool drm_dp_sideband_parse_req(struct drm_dp_sideband_msg_rx *raw,
		622	struct drm_dp_sideband_msg_req_body *msg)
		623	{
		624	memset(msg, 0, sizeof(*msg));
		625	msg->req_type = (raw->msg[0] & 0x7f);
		626
		627	switch (msg->req_type) {
		628	case DP_CONNECTION_STATUS_NOTIFY:
		629	return drm_dp_sideband_parse_connection_status_notify(raw, msg);
		630	case DP_RESOURCE_STATUS_NOTIFY:
		631	return drm_dp_sideband_parse_resource_status_notify(raw, msg);
		632	default:
		633	DRM_ERROR("Got unknown request 0x%02x\n", msg->req_type);
		634	return false;
		635	}
		636	}
		637
		638	static int build_dpcd_write(struct drm_dp_sideband_msg_tx msg, u8 port_num, u32 offset, u8 num_bytes, u8 bytes)
		639	{
		640	struct drm_dp_sideband_msg_req_body req;
		641
		642	req.req_type = DP_REMOTE_DPCD_WRITE;
		643	req.u.dpcd_write.port_number = port_num;
		644	req.u.dpcd_write.dpcd_address = offset;
		645	req.u.dpcd_write.num_bytes = num_bytes;
		646	req.u.dpcd_write.bytes = bytes;
		647	drm_dp_encode_sideband_req(&req, msg);
		648
		649	return 0;
		650	}
		651
		652	static int build_link_address(struct drm_dp_sideband_msg_tx *msg)
		653	{
		654	struct drm_dp_sideband_msg_req_body req;
		655
		656	req.req_type = DP_LINK_ADDRESS;
		657	drm_dp_encode_sideband_req(&req, msg);
		658	return 0;
		659	}
		660
		661	static int build_enum_path_resources(struct drm_dp_sideband_msg_tx *msg, int port_num)
		662	{
		663	struct drm_dp_sideband_msg_req_body req;
		664
		665	req.req_type = DP_ENUM_PATH_RESOURCES;
		666	req.u.port_num.port_number = port_num;
		667	drm_dp_encode_sideband_req(&req, msg);
		668	msg->path_msg = true;
		669	return 0;
		670	}
		671
		672	static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_num,
6937	serge	673	u8 vcpi, uint16_t pbn,
		674	u8 number_sdp_streams,
		675	u8 *sdp_stream_sink)
5060	serge	676	{
		677	struct drm_dp_sideband_msg_req_body req;
		678	memset(&req, 0, sizeof(req));
		679	req.req_type = DP_ALLOCATE_PAYLOAD;
		680	req.u.allocate_payload.port_number = port_num;
		681	req.u.allocate_payload.vcpi = vcpi;
		682	req.u.allocate_payload.pbn = pbn;
6937	serge	683	req.u.allocate_payload.number_sdp_streams = number_sdp_streams;
		684	memcpy(req.u.allocate_payload.sdp_stream_sink, sdp_stream_sink,
		685	number_sdp_streams);
5060	serge	686	drm_dp_encode_sideband_req(&req, msg);
		687	msg->path_msg = true;
		688	return 0;
		689	}
		690
		691	static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
		692	struct drm_dp_vcpi *vcpi)
		693	{
5271	serge	694	int ret, vcpi_ret;
5060	serge	695
		696	mutex_lock(&mgr->payload_lock);
		697	ret = find_first_zero_bit(&mgr->payload_mask, mgr->max_payloads + 1);
		698	if (ret > mgr->max_payloads) {
		699	ret = -EINVAL;
		700	DRM_DEBUG_KMS("out of payload ids %d\n", ret);
		701	goto out_unlock;
		702	}
		703
5271	serge	704	vcpi_ret = find_first_zero_bit(&mgr->vcpi_mask, mgr->max_payloads + 1);
		705	if (vcpi_ret > mgr->max_payloads) {
		706	ret = -EINVAL;
		707	DRM_DEBUG_KMS("out of vcpi ids %d\n", ret);
		708	goto out_unlock;
		709	}
		710
5060	serge	711	set_bit(ret, &mgr->payload_mask);
5271	serge	712	set_bit(vcpi_ret, &mgr->vcpi_mask);
		713	vcpi->vcpi = vcpi_ret + 1;
5060	serge	714	mgr->proposed_vcpis[ret - 1] = vcpi;
		715	out_unlock:
		716	mutex_unlock(&mgr->payload_lock);
		717	return ret;
		718	}
		719
		720	static void drm_dp_mst_put_payload_id(struct drm_dp_mst_topology_mgr *mgr,
5271	serge	721	int vcpi)
5060	serge	722	{
5271	serge	723	int i;
		724	if (vcpi == 0)
5060	serge	725	return;
		726
		727	mutex_lock(&mgr->payload_lock);
5271	serge	728	DRM_DEBUG_KMS("putting payload %d\n", vcpi);
		729	clear_bit(vcpi - 1, &mgr->vcpi_mask);
		730
		731	for (i = 0; i < mgr->max_payloads; i++) {
		732	if (mgr->proposed_vcpis[i])
		733	if (mgr->proposed_vcpis[i]->vcpi == vcpi) {
		734	mgr->proposed_vcpis[i] = NULL;
		735	clear_bit(i + 1, &mgr->payload_mask);
		736	}
		737	}
5060	serge	738	mutex_unlock(&mgr->payload_lock);
		739	}
		740
		741	static bool check_txmsg_state(struct drm_dp_mst_topology_mgr *mgr,
		742	struct drm_dp_sideband_msg_tx *txmsg)
		743	{
		744	bool ret;
6084	serge	745
		746	/*
		747	* All updates to txmsg->state are protected by mgr->qlock, and the two
		748	* cases we check here are terminal states. For those the barriers
		749	* provided by the wake_up/wait_event pair are enough.
		750	*/
5060	serge	751	ret = (txmsg->state == DRM_DP_SIDEBAND_TX_RX \|\|
		752	txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT);
		753	return ret;
		754	}
		755
		756	static int drm_dp_mst_wait_tx_reply(struct drm_dp_mst_branch *mstb,
		757	struct drm_dp_sideband_msg_tx *txmsg)
		758	{
		759	struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
		760	int ret;
		761
		762	ret = wait_event_timeout(mgr->tx_waitq,
		763	check_txmsg_state(mgr, txmsg),
		764	(4 * HZ));
		765	mutex_lock(&mstb->mgr->qlock);
		766	if (ret > 0) {
		767	if (txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT) {
		768	ret = -EIO;
		769	goto out;
		770	}
		771	} else {
		772	DRM_DEBUG_KMS("timedout msg send %p %d %d\n", txmsg, txmsg->state, txmsg->seqno);
		773
		774	/* dump some state */
		775	ret = -EIO;
		776
		777	/* remove from q */
		778	if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED \|\|
		779	txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND) {
		780	list_del(&txmsg->next);
		781	}
		782
		783	if (txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND \|\|
		784	txmsg->state == DRM_DP_SIDEBAND_TX_SENT) {
		785	mstb->tx_slots[txmsg->seqno] = NULL;
		786	}
		787	}
		788	out:
		789	mutex_unlock(&mgr->qlock);
		790
		791	return ret;
		792	}
		793
		794	static struct drm_dp_mst_branch drm_dp_add_mst_branch_device(u8 lct, u8 rad)
		795	{
		796	struct drm_dp_mst_branch *mstb;
		797
		798	mstb = kzalloc(sizeof(*mstb), GFP_KERNEL);
		799	if (!mstb)
		800	return NULL;
		801
		802	mstb->lct = lct;
		803	if (lct > 1)
		804	memcpy(mstb->rad, rad, lct / 2);
		805	INIT_LIST_HEAD(&mstb->ports);
		806	kref_init(&mstb->kref);
		807	return mstb;
		808	}
		809
6320	serge	810	static void drm_dp_free_mst_port(struct kref *kref);
		811
		812	static void drm_dp_free_mst_branch_device(struct kref *kref)
		813	{
		814	struct drm_dp_mst_branch *mstb = container_of(kref, struct drm_dp_mst_branch, kref);
		815	if (mstb->port_parent) {
		816	if (list_empty(&mstb->port_parent->next))
		817	kref_put(&mstb->port_parent->kref, drm_dp_free_mst_port);
		818	}
		819	kfree(mstb);
		820	}
		821
5060	serge	822	static void drm_dp_destroy_mst_branch_device(struct kref *kref)
		823	{
		824	struct drm_dp_mst_branch *mstb = container_of(kref, struct drm_dp_mst_branch, kref);
		825	struct drm_dp_mst_port port, tmp;
		826	bool wake_tx = false;
		827
		828	/*
6320	serge	829	* init kref again to be used by ports to remove mst branch when it is
		830	* not needed anymore
		831	*/
		832	kref_init(kref);
		833
		834	if (mstb->port_parent && list_empty(&mstb->port_parent->next))
		835	kref_get(&mstb->port_parent->kref);
		836
		837	/*
5060	serge	838	* destroy all ports - don't need lock
		839	* as there are no more references to the mst branch
		840	* device at this point.
		841	*/
		842	list_for_each_entry_safe(port, tmp, &mstb->ports, next) {
		843	list_del(&port->next);
		844	drm_dp_put_port(port);
		845	}
		846
		847	/* drop any tx slots msg */
		848	mutex_lock(&mstb->mgr->qlock);
		849	if (mstb->tx_slots[0]) {
		850	mstb->tx_slots[0]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
		851	mstb->tx_slots[0] = NULL;
		852	wake_tx = true;
		853	}
		854	if (mstb->tx_slots[1]) {
		855	mstb->tx_slots[1]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
		856	mstb->tx_slots[1] = NULL;
		857	wake_tx = true;
		858	}
		859	mutex_unlock(&mstb->mgr->qlock);
		860
		861	// if (wake_tx)
		862	// wake_up(&mstb->mgr->tx_waitq);
6320	serge	863	kref_put(kref, drm_dp_free_mst_branch_device);
5060	serge	864	}
		865
		866	static void drm_dp_put_mst_branch_device(struct drm_dp_mst_branch *mstb)
		867	{
		868	kref_put(&mstb->kref, drm_dp_destroy_mst_branch_device);
		869	}
		870
		871
		872	static void drm_dp_port_teardown_pdt(struct drm_dp_mst_port *port, int old_pdt)
		873	{
5271	serge	874	struct drm_dp_mst_branch *mstb;
		875
5060	serge	876	switch (old_pdt) {
		877	case DP_PEER_DEVICE_DP_LEGACY_CONV:
		878	case DP_PEER_DEVICE_SST_SINK:
		879	/* remove i2c over sideband */
		880	drm_dp_mst_unregister_i2c_bus(&port->aux);
		881	break;
		882	case DP_PEER_DEVICE_MST_BRANCHING:
5271	serge	883	mstb = port->mstb;
5060	serge	884	port->mstb = NULL;
5271	serge	885	drm_dp_put_mst_branch_device(mstb);
5060	serge	886	break;
		887	}
		888	}
		889
		890	static void drm_dp_destroy_port(struct kref *kref)
		891	{
		892	struct drm_dp_mst_port *port = container_of(kref, struct drm_dp_mst_port, kref);
		893	struct drm_dp_mst_topology_mgr *mgr = port->mgr;
6084	serge	894
5060	serge	895	if (!port->input) {
		896	port->vcpi.num_slots = 0;
5271	serge	897
		898	kfree(port->cached_edid);
6084	serge	899
		900	/*
		901	* The only time we don't have a connector
		902	* on an output port is if the connector init
		903	* fails.
		904	*/
		905	if (port->connector) {
		906	/* we can't destroy the connector here, as
		907	* we might be holding the mode_config.mutex
		908	* from an EDID retrieval */
		909
		910	mutex_lock(&mgr->destroy_connector_lock);
6320	serge	911	kref_get(&port->parent->kref);
6084	serge	912	list_add(&port->next, &mgr->destroy_connector_list);
		913	mutex_unlock(&mgr->destroy_connector_lock);
		914	// schedule_work(&mgr->destroy_connector_work);
		915	return;
		916	}
		917	/* no need to clean up vcpi
		918	* as if we have no connector we never setup a vcpi */
5060	serge	919	drm_dp_port_teardown_pdt(port, port->pdt);
		920	}
		921	kfree(port);
		922	}
		923
		924	static void drm_dp_put_port(struct drm_dp_mst_port *port)
		925	{
		926	kref_put(&port->kref, drm_dp_destroy_port);
		927	}
		928
		929	static struct drm_dp_mst_branch drm_dp_mst_get_validated_mstb_ref_locked(struct drm_dp_mst_branch mstb, struct drm_dp_mst_branch *to_find)
		930	{
		931	struct drm_dp_mst_port *port;
		932	struct drm_dp_mst_branch *rmstb;
		933	if (to_find == mstb) {
		934	kref_get(&mstb->kref);
		935	return mstb;
		936	}
		937	list_for_each_entry(port, &mstb->ports, next) {
		938	if (port->mstb) {
		939	rmstb = drm_dp_mst_get_validated_mstb_ref_locked(port->mstb, to_find);
		940	if (rmstb)
		941	return rmstb;
		942	}
		943	}
		944	return NULL;
		945	}
		946
		947	static struct drm_dp_mst_branch drm_dp_get_validated_mstb_ref(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_branch *mstb)
		948	{
		949	struct drm_dp_mst_branch *rmstb = NULL;
		950	mutex_lock(&mgr->lock);
		951	if (mgr->mst_primary)
		952	rmstb = drm_dp_mst_get_validated_mstb_ref_locked(mgr->mst_primary, mstb);
		953	mutex_unlock(&mgr->lock);
		954	return rmstb;
		955	}
		956
		957	static struct drm_dp_mst_port drm_dp_mst_get_port_ref_locked(struct drm_dp_mst_branch mstb, struct drm_dp_mst_port *to_find)
		958	{
		959	struct drm_dp_mst_port port, mport;
		960
		961	list_for_each_entry(port, &mstb->ports, next) {
		962	if (port == to_find) {
		963	kref_get(&port->kref);
		964	return port;
		965	}
		966	if (port->mstb) {
		967	mport = drm_dp_mst_get_port_ref_locked(port->mstb, to_find);
		968	if (mport)
		969	return mport;
		970	}
		971	}
		972	return NULL;
		973	}
		974
		975	static struct drm_dp_mst_port drm_dp_get_validated_port_ref(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port *port)
		976	{
		977	struct drm_dp_mst_port *rport = NULL;
		978	mutex_lock(&mgr->lock);
		979	if (mgr->mst_primary)
		980	rport = drm_dp_mst_get_port_ref_locked(mgr->mst_primary, port);
		981	mutex_unlock(&mgr->lock);
		982	return rport;
		983	}
		984
		985	static struct drm_dp_mst_port drm_dp_get_port(struct drm_dp_mst_branch mstb, u8 port_num)
		986	{
		987	struct drm_dp_mst_port *port;
		988
		989	list_for_each_entry(port, &mstb->ports, next) {
		990	if (port->port_num == port_num) {
		991	kref_get(&port->kref);
		992	return port;
		993	}
		994	}
		995
		996	return NULL;
		997	}
		998
		999	/*
		1000	* calculate a new RAD for this MST branch device
		1001	* if parent has an LCT of 2 then it has 1 nibble of RAD,
		1002	* if parent has an LCT of 3 then it has 2 nibbles of RAD,
		1003	*/
		1004	static u8 drm_dp_calculate_rad(struct drm_dp_mst_port *port,
		1005	u8 *rad)
		1006	{
6320	serge	1007	int parent_lct = port->parent->lct;
5060	serge	1008	int shift = 4;
6320	serge	1009	int idx = (parent_lct - 1) / 2;
		1010	if (parent_lct > 1) {
		1011	memcpy(rad, port->parent->rad, idx + 1);
		1012	shift = (parent_lct % 2) ? 4 : 0;
5060	serge	1013	} else
		1014	rad[0] = 0;
		1015
		1016	rad[idx] \|= port->port_num << shift;
6320	serge	1017	return parent_lct + 1;
5060	serge	1018	}
		1019
		1020	/*
		1021	* return sends link address for new mstb
		1022	*/
		1023	static bool drm_dp_port_setup_pdt(struct drm_dp_mst_port *port)
		1024	{
		1025	int ret;
		1026	u8 rad[6], lct;
		1027	bool send_link = false;
		1028	switch (port->pdt) {
		1029	case DP_PEER_DEVICE_DP_LEGACY_CONV:
		1030	case DP_PEER_DEVICE_SST_SINK:
		1031	/* add i2c over sideband */
		1032	ret = drm_dp_mst_register_i2c_bus(&port->aux);
		1033	break;
		1034	case DP_PEER_DEVICE_MST_BRANCHING:
		1035	lct = drm_dp_calculate_rad(port, rad);
		1036
		1037	port->mstb = drm_dp_add_mst_branch_device(lct, rad);
		1038	port->mstb->mgr = port->mgr;
		1039	port->mstb->port_parent = port;
		1040
		1041	send_link = true;
		1042	break;
		1043	}
		1044	return send_link;
		1045	}
		1046
6320	serge	1047	static void drm_dp_check_mstb_guid(struct drm_dp_mst_branch mstb, u8 guid)
5060	serge	1048	{
		1049	int ret;
6320	serge	1050
		1051	memcpy(mstb->guid, guid, 16);
		1052
		1053	if (!drm_dp_validate_guid(mstb->mgr, mstb->guid)) {
		1054	if (mstb->port_parent) {
		1055	ret = drm_dp_send_dpcd_write(
		1056	mstb->mgr,
		1057	mstb->port_parent,
		1058	DP_GUID,
		1059	16,
		1060	mstb->guid);
		1061	} else {
		1062
		1063	ret = drm_dp_dpcd_write(
		1064	mstb->mgr->aux,
7144	serge	1065	DP_GUID,
6320	serge	1066	mstb->guid,
		1067	16);
5060	serge	1068	}
		1069	}
		1070	}
		1071
6084	serge	1072	static void build_mst_prop_path(const struct drm_dp_mst_branch *mstb,
		1073	int pnum,
5271	serge	1074	char *proppath,
		1075	size_t proppath_size)
5060	serge	1076	{
		1077	int i;
		1078	char temp[8];
5271	serge	1079	snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
5060	serge	1080	for (i = 0; i < (mstb->lct - 1); i++) {
		1081	int shift = (i % 2) ? 0 : 4;
6320	serge	1082	int port_num = (mstb->rad[i / 2] >> shift) & 0xf;
5271	serge	1083	snprintf(temp, sizeof(temp), "-%d", port_num);
		1084	strlcat(proppath, temp, proppath_size);
5060	serge	1085	}
6084	serge	1086	snprintf(temp, sizeof(temp), "-%d", pnum);
5271	serge	1087	strlcat(proppath, temp, proppath_size);
5060	serge	1088	}
		1089
		1090	static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
		1091	struct device *dev,
		1092	struct drm_dp_link_addr_reply_port *port_msg)
		1093	{
		1094	struct drm_dp_mst_port *port;
		1095	bool ret;
		1096	bool created = false;
		1097	int old_pdt = 0;
		1098	int old_ddps = 0;
		1099	port = drm_dp_get_port(mstb, port_msg->port_number);
		1100	if (!port) {
		1101	port = kzalloc(sizeof(*port), GFP_KERNEL);
		1102	if (!port)
		1103	return;
		1104	kref_init(&port->kref);
		1105	port->parent = mstb;
		1106	port->port_num = port_msg->port_number;
		1107	port->mgr = mstb->mgr;
		1108	port->aux.name = "DPMST";
		1109	port->aux.dev = dev;
		1110	created = true;
		1111	} else {
		1112	old_pdt = port->pdt;
		1113	old_ddps = port->ddps;
		1114	}
		1115
		1116	port->pdt = port_msg->peer_device_type;
		1117	port->input = port_msg->input_port;
		1118	port->mcs = port_msg->mcs;
		1119	port->ddps = port_msg->ddps;
		1120	port->ldps = port_msg->legacy_device_plug_status;
		1121	port->dpcd_rev = port_msg->dpcd_revision;
		1122	port->num_sdp_streams = port_msg->num_sdp_streams;
		1123	port->num_sdp_stream_sinks = port_msg->num_sdp_stream_sinks;
		1124
		1125	/* manage mstb port lists with mgr lock - take a reference
		1126	for this list */
		1127	if (created) {
		1128	mutex_lock(&mstb->mgr->lock);
		1129	kref_get(&port->kref);
		1130	list_add(&port->next, &mstb->ports);
		1131	mutex_unlock(&mstb->mgr->lock);
		1132	}
		1133
		1134	if (old_ddps != port->ddps) {
		1135	if (port->ddps) {
		1136	if (!port->input)
		1137	drm_dp_send_enum_path_resources(mstb->mgr, mstb, port);
		1138	} else {
		1139	port->available_pbn = 0;
		1140	}
		1141	}
		1142
		1143	if (old_pdt != port->pdt && !port->input) {
		1144	drm_dp_port_teardown_pdt(port, old_pdt);
		1145
		1146	ret = drm_dp_port_setup_pdt(port);
6084	serge	1147	if (ret == true)
5060	serge	1148	drm_dp_send_link_address(mstb->mgr, port->mstb);
		1149	}
		1150
		1151	if (created && !port->input) {
		1152	char proppath[255];
6084	serge	1153
		1154	build_mst_prop_path(mstb, port->port_num, proppath, sizeof(proppath));
5060	serge	1155	port->connector = (*mstb->mgr->cbs->add_connector)(mstb->mgr, port, proppath);
6084	serge	1156	if (!port->connector) {
		1157	/* remove it from the port list */
		1158	mutex_lock(&mstb->mgr->lock);
		1159	list_del(&port->next);
		1160	mutex_unlock(&mstb->mgr->lock);
		1161	/* drop port list reference */
		1162	drm_dp_put_port(port);
		1163	goto out;
		1164	}
6937	serge	1165	if (port->port_num >= DP_MST_LOGICAL_PORT_0) {
5271	serge	1166	port->cached_edid = drm_get_edid(port->connector, &port->aux.ddc);
6084	serge	1167	drm_mode_connector_set_tile_property(port->connector);
5271	serge	1168	}
6084	serge	1169	(*mstb->mgr->cbs->register_connector)(port->connector);
5060	serge	1170	}
		1171
6084	serge	1172	out:
5060	serge	1173	/* put reference to this port */
		1174	drm_dp_put_port(port);
		1175	}
		1176
		1177	static void drm_dp_update_port(struct drm_dp_mst_branch *mstb,
		1178	struct drm_dp_connection_status_notify *conn_stat)
		1179	{
		1180	struct drm_dp_mst_port *port;
		1181	int old_pdt;
		1182	int old_ddps;
		1183	bool dowork = false;
		1184	port = drm_dp_get_port(mstb, conn_stat->port_number);
		1185	if (!port)
		1186	return;
		1187
		1188	old_ddps = port->ddps;
		1189	old_pdt = port->pdt;
		1190	port->pdt = conn_stat->peer_device_type;
		1191	port->mcs = conn_stat->message_capability_status;
		1192	port->ldps = conn_stat->legacy_device_plug_status;
		1193	port->ddps = conn_stat->displayport_device_plug_status;
		1194
		1195	if (old_ddps != port->ddps) {
		1196	if (port->ddps) {
		1197	dowork = true;
		1198	} else {
		1199	port->available_pbn = 0;
		1200	}
		1201	}
		1202	if (old_pdt != port->pdt && !port->input) {
		1203	drm_dp_port_teardown_pdt(port, old_pdt);
		1204
		1205	if (drm_dp_port_setup_pdt(port))
		1206	dowork = true;
		1207	}
		1208
		1209	drm_dp_put_port(port);
		1210	// if (dowork)
		1211	// queue_work(system_long_wq, &mstb->mgr->work);
		1212
		1213	}
		1214
		1215	static struct drm_dp_mst_branch drm_dp_get_mst_branch_device(struct drm_dp_mst_topology_mgr mgr,
		1216	u8 lct, u8 *rad)
		1217	{
		1218	struct drm_dp_mst_branch *mstb;
		1219	struct drm_dp_mst_port *port;
		1220	int i;
		1221	/* find the port by iterating down */
6084	serge	1222
		1223	mutex_lock(&mgr->lock);
5060	serge	1224	mstb = mgr->mst_primary;
		1225
		1226	for (i = 0; i < lct - 1; i++) {
		1227	int shift = (i % 2) ? 0 : 4;
6320	serge	1228	int port_num = (rad[i / 2] >> shift) & 0xf;
5060	serge	1229
		1230	list_for_each_entry(port, &mstb->ports, next) {
		1231	if (port->port_num == port_num) {
6084	serge	1232	mstb = port->mstb;
		1233	if (!mstb) {
5060	serge	1234	DRM_ERROR("failed to lookup MSTB with lct %d, rad %02x\n", lct, rad[0]);
6084	serge	1235	goto out;
5060	serge	1236	}
		1237
		1238	break;
		1239	}
		1240	}
		1241	}
		1242	kref_get(&mstb->kref);
6084	serge	1243	out:
		1244	mutex_unlock(&mgr->lock);
5060	serge	1245	return mstb;
		1246	}
		1247
6320	serge	1248	static struct drm_dp_mst_branch *get_mst_branch_device_by_guid_helper(
		1249	struct drm_dp_mst_branch *mstb,
		1250	uint8_t *guid)
		1251	{
		1252	struct drm_dp_mst_branch *found_mstb;
		1253	struct drm_dp_mst_port *port;
		1254
		1255	if (memcmp(mstb->guid, guid, 16) == 0)
		1256	return mstb;
		1257
		1258
		1259	list_for_each_entry(port, &mstb->ports, next) {
		1260	if (!port->mstb)
		1261	continue;
		1262
		1263	found_mstb = get_mst_branch_device_by_guid_helper(port->mstb, guid);
		1264
		1265	if (found_mstb)
		1266	return found_mstb;
		1267	}
		1268
		1269	return NULL;
		1270	}
		1271
		1272	static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device_by_guid(
		1273	struct drm_dp_mst_topology_mgr *mgr,
		1274	uint8_t *guid)
		1275	{
		1276	struct drm_dp_mst_branch *mstb;
		1277
		1278	/* find the port by iterating down */
		1279	mutex_lock(&mgr->lock);
		1280
		1281	mstb = get_mst_branch_device_by_guid_helper(mgr->mst_primary, guid);
		1282
		1283	if (mstb)
		1284	kref_get(&mstb->kref);
		1285
		1286	mutex_unlock(&mgr->lock);
		1287	return mstb;
		1288	}
		1289
5060	serge	1290	static void drm_dp_check_and_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
		1291	struct drm_dp_mst_branch *mstb)
		1292	{
		1293	struct drm_dp_mst_port *port;
6084	serge	1294	struct drm_dp_mst_branch *mstb_child;
		1295	if (!mstb->link_address_sent)
		1296	drm_dp_send_link_address(mgr, mstb);
5060	serge	1297
		1298	list_for_each_entry(port, &mstb->ports, next) {
		1299	if (port->input)
		1300	continue;
		1301
		1302	if (!port->ddps)
		1303	continue;
		1304
		1305	if (!port->available_pbn)
		1306	drm_dp_send_enum_path_resources(mgr, mstb, port);
		1307
6084	serge	1308	if (port->mstb) {
		1309	mstb_child = drm_dp_get_validated_mstb_ref(mgr, port->mstb);
		1310	if (mstb_child) {
		1311	drm_dp_check_and_send_link_address(mgr, mstb_child);
		1312	drm_dp_put_mst_branch_device(mstb_child);
		1313	}
		1314	}
5060	serge	1315	}
		1316	}
		1317
		1318	static void drm_dp_mst_link_probe_work(struct work_struct *work)
		1319	{
		1320	struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, work);
6084	serge	1321	struct drm_dp_mst_branch *mstb;
5060	serge	1322
6084	serge	1323	mutex_lock(&mgr->lock);
		1324	mstb = mgr->mst_primary;
		1325	if (mstb) {
		1326	kref_get(&mstb->kref);
		1327	}
		1328	mutex_unlock(&mgr->lock);
		1329	if (mstb) {
		1330	drm_dp_check_and_send_link_address(mgr, mstb);
		1331	drm_dp_put_mst_branch_device(mstb);
		1332	}
5060	serge	1333	}
		1334
		1335	static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
		1336	u8 *guid)
		1337	{
		1338	static u8 zero_guid[16];
		1339
		1340	if (!memcmp(guid, zero_guid, 16)) {
		1341	u64 salt = get_jiffies_64();
		1342	memcpy(&guid[0], &salt, sizeof(u64));
		1343	memcpy(&guid[8], &salt, sizeof(u64));
		1344	return false;
		1345	}
		1346	return true;
		1347	}
		1348
		1349	#if 0
		1350	static int build_dpcd_read(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes)
		1351	{
		1352	struct drm_dp_sideband_msg_req_body req;
		1353
		1354	req.req_type = DP_REMOTE_DPCD_READ;
		1355	req.u.dpcd_read.port_number = port_num;
		1356	req.u.dpcd_read.dpcd_address = offset;
		1357	req.u.dpcd_read.num_bytes = num_bytes;
		1358	drm_dp_encode_sideband_req(&req, msg);
		1359
		1360	return 0;
		1361	}
		1362	#endif
		1363
		1364	static int drm_dp_send_sideband_msg(struct drm_dp_mst_topology_mgr *mgr,
		1365	bool up, u8 *msg, int len)
		1366	{
		1367	int ret;
		1368	int regbase = up ? DP_SIDEBAND_MSG_UP_REP_BASE : DP_SIDEBAND_MSG_DOWN_REQ_BASE;
		1369	int tosend, total, offset;
		1370	int retries = 0;
		1371
		1372	retry:
		1373	total = len;
		1374	offset = 0;
		1375	do {
		1376	tosend = min3(mgr->max_dpcd_transaction_bytes, 16, total);
		1377
		1378	ret = drm_dp_dpcd_write(mgr->aux, regbase + offset,
		1379	&msg[offset],
		1380	tosend);
		1381	if (ret != tosend) {
		1382	if (ret == -EIO && retries < 5) {
		1383	retries++;
		1384	goto retry;
		1385	}
		1386	DRM_DEBUG_KMS("failed to dpcd write %d %d\n", tosend, ret);
		1387
		1388	return -EIO;
		1389	}
		1390	offset += tosend;
		1391	total -= tosend;
		1392	} while (total > 0);
		1393	return 0;
		1394	}
		1395
		1396	static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
		1397	struct drm_dp_sideband_msg_tx *txmsg)
		1398	{
		1399	struct drm_dp_mst_branch *mstb = txmsg->dst;
6320	serge	1400	u8 req_type;
5060	serge	1401
		1402	/* both msg slots are full */
		1403	if (txmsg->seqno == -1) {
		1404	if (mstb->tx_slots[0] && mstb->tx_slots[1]) {
		1405	DRM_DEBUG_KMS("%s: failed to find slot\n", __func__);
		1406	return -EAGAIN;
		1407	}
		1408	if (mstb->tx_slots[0] == NULL && mstb->tx_slots[1] == NULL) {
		1409	txmsg->seqno = mstb->last_seqno;
		1410	mstb->last_seqno ^= 1;
		1411	} else if (mstb->tx_slots[0] == NULL)
		1412	txmsg->seqno = 0;
		1413	else
		1414	txmsg->seqno = 1;
		1415	mstb->tx_slots[txmsg->seqno] = txmsg;
		1416	}
6320	serge	1417
		1418	req_type = txmsg->msg[0] & 0x7f;
		1419	if (req_type == DP_CONNECTION_STATUS_NOTIFY \|\|
		1420	req_type == DP_RESOURCE_STATUS_NOTIFY)
		1421	hdr->broadcast = 1;
		1422	else
7144	serge	1423	hdr->broadcast = 0;
5060	serge	1424	hdr->path_msg = txmsg->path_msg;
		1425	hdr->lct = mstb->lct;
		1426	hdr->lcr = mstb->lct - 1;
		1427	if (mstb->lct > 1)
		1428	memcpy(hdr->rad, mstb->rad, mstb->lct / 2);
		1429	hdr->seqno = txmsg->seqno;
		1430	return 0;
		1431	}
		1432	/*
		1433	* process a single block of the next message in the sideband queue
		1434	*/
		1435	static int process_single_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
		1436	struct drm_dp_sideband_msg_tx *txmsg,
		1437	bool up)
		1438	{
		1439	u8 chunk[48];
		1440	struct drm_dp_sideband_msg_hdr hdr;
		1441	int len, space, idx, tosend;
		1442	int ret;
		1443
		1444	memset(&hdr, 0, sizeof(struct drm_dp_sideband_msg_hdr));
		1445
		1446	if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED) {
		1447	txmsg->seqno = -1;
		1448	txmsg->state = DRM_DP_SIDEBAND_TX_START_SEND;
		1449	}
		1450
		1451	/* make hdr from dst mst - for replies use seqno
		1452	otherwise assign one */
		1453	ret = set_hdr_from_dst_qlock(&hdr, txmsg);
		1454	if (ret < 0)
		1455	return ret;
		1456
		1457	/* amount left to send in this message */
		1458	len = txmsg->cur_len - txmsg->cur_offset;
		1459
		1460	/* 48 - sideband msg size - 1 byte for data CRC, x header bytes */
		1461	space = 48 - 1 - drm_dp_calc_sb_hdr_size(&hdr);
		1462
		1463	tosend = min(len, space);
		1464	if (len == txmsg->cur_len)
		1465	hdr.somt = 1;
		1466	if (space >= len)
		1467	hdr.eomt = 1;
		1468
		1469
		1470	hdr.msg_len = tosend + 1;
		1471	drm_dp_encode_sideband_msg_hdr(&hdr, chunk, &idx);
		1472	memcpy(&chunk[idx], &txmsg->msg[txmsg->cur_offset], tosend);
		1473	/* add crc at end */
		1474	drm_dp_crc_sideband_chunk_req(&chunk[idx], tosend);
		1475	idx += tosend + 1;
		1476
		1477	ret = drm_dp_send_sideband_msg(mgr, up, chunk, idx);
		1478	if (ret) {
		1479	DRM_DEBUG_KMS("sideband msg failed to send\n");
		1480	return ret;
		1481	}
		1482
		1483	txmsg->cur_offset += tosend;
		1484	if (txmsg->cur_offset == txmsg->cur_len) {
		1485	txmsg->state = DRM_DP_SIDEBAND_TX_SENT;
		1486	return 1;
		1487	}
		1488	return 0;
		1489	}
		1490
		1491	static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
		1492	{
		1493	struct drm_dp_sideband_msg_tx *txmsg;
		1494	int ret;
		1495
6084	serge	1496	WARN_ON(!mutex_is_locked(&mgr->qlock));
		1497
5060	serge	1498	/* construct a chunk from the first msg in the tx_msg queue */
		1499	if (list_empty(&mgr->tx_msg_downq)) {
		1500	mgr->tx_down_in_progress = false;
		1501	return;
		1502	}
		1503	mgr->tx_down_in_progress = true;
		1504
		1505	txmsg = list_first_entry(&mgr->tx_msg_downq, struct drm_dp_sideband_msg_tx, next);
		1506	ret = process_single_tx_qlock(mgr, txmsg, false);
		1507	if (ret == 1) {
		1508	/* txmsg is sent it should be in the slots now */
		1509	list_del(&txmsg->next);
		1510	} else if (ret) {
		1511	DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
		1512	list_del(&txmsg->next);
		1513	if (txmsg->seqno != -1)
		1514	txmsg->dst->tx_slots[txmsg->seqno] = NULL;
		1515	txmsg->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
		1516	// wake_up(&mgr->tx_waitq);
		1517	}
		1518	if (list_empty(&mgr->tx_msg_downq)) {
		1519	mgr->tx_down_in_progress = false;
		1520	return;
		1521	}
		1522	}
		1523
		1524	/* called holding qlock */
6320	serge	1525	static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
		1526	struct drm_dp_sideband_msg_tx *txmsg)
5060	serge	1527	{
		1528	int ret;
		1529
		1530	/* construct a chunk from the first msg in the tx_msg queue */
6320	serge	1531	ret = process_single_tx_qlock(mgr, txmsg, true);
5060	serge	1532
6320	serge	1533	if (ret != 1)
5060	serge	1534	DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
6320	serge	1535
		1536	txmsg->dst->tx_slots[txmsg->seqno] = NULL;
5060	serge	1537	}
		1538
		1539	static void drm_dp_queue_down_tx(struct drm_dp_mst_topology_mgr *mgr,
		1540	struct drm_dp_sideband_msg_tx *txmsg)
		1541	{
		1542	mutex_lock(&mgr->qlock);
		1543	list_add_tail(&txmsg->next, &mgr->tx_msg_downq);
		1544	if (!mgr->tx_down_in_progress)
		1545	process_single_down_tx_qlock(mgr);
		1546	mutex_unlock(&mgr->qlock);
		1547	}
		1548
6084	serge	1549	static void drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
		1550	struct drm_dp_mst_branch *mstb)
5060	serge	1551	{
		1552	int len;
		1553	struct drm_dp_sideband_msg_tx *txmsg;
		1554	int ret;
		1555
		1556	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1557	if (!txmsg)
6084	serge	1558	return;
5060	serge	1559
		1560	txmsg->dst = mstb;
		1561	len = build_link_address(txmsg);
		1562
6084	serge	1563	mstb->link_address_sent = true;
5060	serge	1564	drm_dp_queue_down_tx(mgr, txmsg);
		1565
		1566	ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
		1567	if (ret > 0) {
		1568	int i;
		1569
		1570	if (txmsg->reply.reply_type == 1)
		1571	DRM_DEBUG_KMS("link address nak received\n");
		1572	else {
		1573	DRM_DEBUG_KMS("link address reply: %d\n", txmsg->reply.u.link_addr.nports);
		1574	for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
		1575	DRM_DEBUG_KMS("port %d: input %d, pdt: %d, pn: %d, dpcd_rev: %02x, mcs: %d, ddps: %d, ldps %d, sdp %d/%d\n", i,
		1576	txmsg->reply.u.link_addr.ports[i].input_port,
		1577	txmsg->reply.u.link_addr.ports[i].peer_device_type,
		1578	txmsg->reply.u.link_addr.ports[i].port_number,
		1579	txmsg->reply.u.link_addr.ports[i].dpcd_revision,
		1580	txmsg->reply.u.link_addr.ports[i].mcs,
		1581	txmsg->reply.u.link_addr.ports[i].ddps,
		1582	txmsg->reply.u.link_addr.ports[i].legacy_device_plug_status,
		1583	txmsg->reply.u.link_addr.ports[i].num_sdp_streams,
		1584	txmsg->reply.u.link_addr.ports[i].num_sdp_stream_sinks);
		1585	}
6320	serge	1586
		1587	drm_dp_check_mstb_guid(mstb, txmsg->reply.u.link_addr.guid);
		1588
5060	serge	1589	for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
		1590	drm_dp_add_port(mstb, mgr->dev, &txmsg->reply.u.link_addr.ports[i]);
		1591	}
		1592	(*mgr->cbs->hotplug)(mgr);
		1593	}
6084	serge	1594	} else {
		1595	mstb->link_address_sent = false;
5060	serge	1596	DRM_DEBUG_KMS("link address failed %d\n", ret);
6084	serge	1597	}
5060	serge	1598
		1599	kfree(txmsg);
		1600	}
		1601
		1602	static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
		1603	struct drm_dp_mst_branch *mstb,
		1604	struct drm_dp_mst_port *port)
		1605	{
		1606	int len;
		1607	struct drm_dp_sideband_msg_tx *txmsg;
		1608	int ret;
		1609
		1610	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1611	if (!txmsg)
		1612	return -ENOMEM;
		1613
		1614	txmsg->dst = mstb;
		1615	len = build_enum_path_resources(txmsg, port->port_num);
		1616
		1617	drm_dp_queue_down_tx(mgr, txmsg);
		1618
		1619	ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
		1620	if (ret > 0) {
		1621	if (txmsg->reply.reply_type == 1)
		1622	DRM_DEBUG_KMS("enum path resources nak received\n");
		1623	else {
		1624	if (port->port_num != txmsg->reply.u.path_resources.port_number)
		1625	DRM_ERROR("got incorrect port in response\n");
		1626	DRM_DEBUG_KMS("enum path resources %d: %d %d\n", txmsg->reply.u.path_resources.port_number, txmsg->reply.u.path_resources.full_payload_bw_number,
		1627	txmsg->reply.u.path_resources.avail_payload_bw_number);
		1628	port->available_pbn = txmsg->reply.u.path_resources.avail_payload_bw_number;
		1629	}
		1630	}
		1631
		1632	kfree(txmsg);
		1633	return 0;
		1634	}
		1635
6320	serge	1636	static struct drm_dp_mst_port drm_dp_get_last_connected_port_to_mstb(struct drm_dp_mst_branch mstb)
		1637	{
		1638	if (!mstb->port_parent)
		1639	return NULL;
		1640
		1641	if (mstb->port_parent->mstb != mstb)
		1642	return mstb->port_parent;
		1643
		1644	return drm_dp_get_last_connected_port_to_mstb(mstb->port_parent->parent);
		1645	}
		1646
		1647	static struct drm_dp_mst_branch drm_dp_get_last_connected_port_and_mstb(struct drm_dp_mst_topology_mgr mgr,
		1648	struct drm_dp_mst_branch *mstb,
		1649	int *port_num)
		1650	{
		1651	struct drm_dp_mst_branch *rmstb = NULL;
		1652	struct drm_dp_mst_port *found_port;
		1653	mutex_lock(&mgr->lock);
		1654	if (mgr->mst_primary) {
		1655	found_port = drm_dp_get_last_connected_port_to_mstb(mstb);
		1656
		1657	if (found_port) {
		1658	rmstb = found_port->parent;
		1659	kref_get(&rmstb->kref);
		1660	*port_num = found_port->port_num;
		1661	}
		1662	}
		1663	mutex_unlock(&mgr->lock);
		1664	return rmstb;
		1665	}
		1666
5060	serge	1667	static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
		1668	struct drm_dp_mst_port *port,
		1669	int id,
		1670	int pbn)
		1671	{
		1672	struct drm_dp_sideband_msg_tx *txmsg;
		1673	struct drm_dp_mst_branch *mstb;
6320	serge	1674	int len, ret, port_num;
6937	serge	1675	u8 sinks[DRM_DP_MAX_SDP_STREAMS];
		1676	int i;
5060	serge	1677
6660	serge	1678	port = drm_dp_get_validated_port_ref(mgr, port);
		1679	if (!port)
		1680	return -EINVAL;
		1681
6320	serge	1682	port_num = port->port_num;
5060	serge	1683	mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
6320	serge	1684	if (!mstb) {
		1685	mstb = drm_dp_get_last_connected_port_and_mstb(mgr, port->parent, &port_num);
		1686
6660	serge	1687	if (!mstb) {
		1688	drm_dp_put_port(port);
7144	serge	1689	return -EINVAL;
		1690	}
6320	serge	1691	}
5060	serge	1692
		1693	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1694	if (!txmsg) {
		1695	ret = -ENOMEM;
		1696	goto fail_put;
		1697	}
		1698
6937	serge	1699	for (i = 0; i < port->num_sdp_streams; i++)
		1700	sinks[i] = i;
		1701
5060	serge	1702	txmsg->dst = mstb;
6320	serge	1703	len = build_allocate_payload(txmsg, port_num,
5060	serge	1704	id,
6937	serge	1705	pbn, port->num_sdp_streams, sinks);
5060	serge	1706
		1707	drm_dp_queue_down_tx(mgr, txmsg);
		1708
		1709	ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
		1710	if (ret > 0) {
		1711	if (txmsg->reply.reply_type == 1) {
		1712	ret = -EINVAL;
		1713	} else
		1714	ret = 0;
		1715	}
		1716	kfree(txmsg);
		1717	fail_put:
		1718	drm_dp_put_mst_branch_device(mstb);
6660	serge	1719	drm_dp_put_port(port);
5060	serge	1720	return ret;
		1721	}
		1722
		1723	static int drm_dp_create_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
		1724	int id,
		1725	struct drm_dp_payload *payload)
		1726	{
		1727	int ret;
		1728
		1729	ret = drm_dp_dpcd_write_payload(mgr, id, payload);
		1730	if (ret < 0) {
		1731	payload->payload_state = 0;
		1732	return ret;
		1733	}
		1734	payload->payload_state = DP_PAYLOAD_LOCAL;
		1735	return 0;
		1736	}
		1737
		1738	static int drm_dp_create_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
		1739	struct drm_dp_mst_port *port,
		1740	int id,
		1741	struct drm_dp_payload *payload)
		1742	{
		1743	int ret;
		1744	ret = drm_dp_payload_send_msg(mgr, port, id, port->vcpi.pbn);
		1745	if (ret < 0)
		1746	return ret;
		1747	payload->payload_state = DP_PAYLOAD_REMOTE;
		1748	return ret;
		1749	}
		1750
		1751	static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
		1752	struct drm_dp_mst_port *port,
		1753	int id,
		1754	struct drm_dp_payload *payload)
		1755	{
		1756	DRM_DEBUG_KMS("\n");
		1757	/* its okay for these to fail */
		1758	if (port) {
		1759	drm_dp_payload_send_msg(mgr, port, id, 0);
		1760	}
		1761
		1762	drm_dp_dpcd_write_payload(mgr, id, payload);
5271	serge	1763	payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
5060	serge	1764	return 0;
		1765	}
		1766
		1767	static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
		1768	int id,
		1769	struct drm_dp_payload *payload)
		1770	{
		1771	payload->payload_state = 0;
		1772	return 0;
		1773	}
		1774
		1775	/**
		1776	* drm_dp_update_payload_part1() - Execute payload update part 1
		1777	* @mgr: manager to use.
		1778	*
		1779	* This iterates over all proposed virtual channels, and tries to
		1780	* allocate space in the link for them. For 0->slots transitions,
		1781	* this step just writes the VCPI to the MST device. For slots->0
		1782	* transitions, this writes the updated VCPIs and removes the
		1783	* remote VC payloads.
		1784	*
		1785	* after calling this the driver should generate ACT and payload
		1786	* packets.
		1787	*/
		1788	int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
		1789	{
5271	serge	1790	int i, j;
5060	serge	1791	int cur_slots = 1;
		1792	struct drm_dp_payload req_payload;
		1793	struct drm_dp_mst_port *port;
		1794
		1795	mutex_lock(&mgr->payload_lock);
		1796	for (i = 0; i < mgr->max_payloads; i++) {
		1797	/* solve the current payloads - compare to the hw ones
		1798	- update the hw view */
		1799	req_payload.start_slot = cur_slots;
		1800	if (mgr->proposed_vcpis[i]) {
		1801	port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
6660	serge	1802	port = drm_dp_get_validated_port_ref(mgr, port);
		1803	if (!port) {
		1804	mutex_unlock(&mgr->payload_lock);
		1805	return -EINVAL;
		1806	}
5060	serge	1807	req_payload.num_slots = mgr->proposed_vcpis[i]->num_slots;
6937	serge	1808	req_payload.vcpi = mgr->proposed_vcpis[i]->vcpi;
5060	serge	1809	} else {
		1810	port = NULL;
		1811	req_payload.num_slots = 0;
		1812	}
5271	serge	1813
		1814	if (mgr->payloads[i].start_slot != req_payload.start_slot) {
		1815	mgr->payloads[i].start_slot = req_payload.start_slot;
		1816	}
5060	serge	1817	/* work out what is required to happen with this payload */
5271	serge	1818	if (mgr->payloads[i].num_slots != req_payload.num_slots) {
5060	serge	1819
		1820	/* need to push an update for this payload */
		1821	if (req_payload.num_slots) {
5271	serge	1822	drm_dp_create_payload_step1(mgr, mgr->proposed_vcpis[i]->vcpi, &req_payload);
5060	serge	1823	mgr->payloads[i].num_slots = req_payload.num_slots;
6937	serge	1824	mgr->payloads[i].vcpi = req_payload.vcpi;
5060	serge	1825	} else if (mgr->payloads[i].num_slots) {
		1826	mgr->payloads[i].num_slots = 0;
6937	serge	1827	drm_dp_destroy_payload_step1(mgr, port, port->vcpi.vcpi, &mgr->payloads[i]);
5060	serge	1828	req_payload.payload_state = mgr->payloads[i].payload_state;
5271	serge	1829	mgr->payloads[i].start_slot = 0;
		1830	}
5060	serge	1831	mgr->payloads[i].payload_state = req_payload.payload_state;
		1832	}
		1833	cur_slots += req_payload.num_slots;
6660	serge	1834
		1835	if (port)
		1836	drm_dp_put_port(port);
5060	serge	1837	}
5271	serge	1838
		1839	for (i = 0; i < mgr->max_payloads; i++) {
		1840	if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
		1841	DRM_DEBUG_KMS("removing payload %d\n", i);
		1842	for (j = i; j < mgr->max_payloads - 1; j++) {
		1843	memcpy(&mgr->payloads[j], &mgr->payloads[j + 1], sizeof(struct drm_dp_payload));
		1844	mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
		1845	if (mgr->proposed_vcpis[j] && mgr->proposed_vcpis[j]->num_slots) {
		1846	set_bit(j + 1, &mgr->payload_mask);
		1847	} else {
		1848	clear_bit(j + 1, &mgr->payload_mask);
		1849	}
		1850	}
		1851	memset(&mgr->payloads[mgr->max_payloads - 1], 0, sizeof(struct drm_dp_payload));
		1852	mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
		1853	clear_bit(mgr->max_payloads, &mgr->payload_mask);
		1854
		1855	}
		1856	}
5060	serge	1857	mutex_unlock(&mgr->payload_lock);
		1858
		1859	return 0;
		1860	}
		1861	EXPORT_SYMBOL(drm_dp_update_payload_part1);
		1862
		1863	/**
		1864	* drm_dp_update_payload_part2() - Execute payload update part 2
		1865	* @mgr: manager to use.
		1866	*
		1867	* This iterates over all proposed virtual channels, and tries to
		1868	* allocate space in the link for them. For 0->slots transitions,
		1869	* this step writes the remote VC payload commands. For slots->0
		1870	* this just resets some internal state.
		1871	*/
		1872	int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
		1873	{
		1874	struct drm_dp_mst_port *port;
		1875	int i;
		1876	int ret = 0;
		1877	mutex_lock(&mgr->payload_lock);
		1878	for (i = 0; i < mgr->max_payloads; i++) {
		1879
		1880	if (!mgr->proposed_vcpis[i])
		1881	continue;
		1882
		1883	port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
		1884
		1885	DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
		1886	if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
5271	serge	1887	ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
5060	serge	1888	} else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
5271	serge	1889	ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
5060	serge	1890	}
		1891	if (ret) {
		1892	mutex_unlock(&mgr->payload_lock);
		1893	return ret;
		1894	}
		1895	}
		1896	mutex_unlock(&mgr->payload_lock);
		1897	return 0;
		1898	}
		1899	EXPORT_SYMBOL(drm_dp_update_payload_part2);
		1900
		1901	#if 0 /* unused as of yet */
		1902	static int drm_dp_send_dpcd_read(struct drm_dp_mst_topology_mgr *mgr,
		1903	struct drm_dp_mst_port *port,
		1904	int offset, int size)
		1905	{
		1906	int len;
		1907	struct drm_dp_sideband_msg_tx *txmsg;
		1908
		1909	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1910	if (!txmsg)
		1911	return -ENOMEM;
		1912
		1913	len = build_dpcd_read(txmsg, port->port_num, 0, 8);
		1914	txmsg->dst = port->parent;
		1915
		1916	drm_dp_queue_down_tx(mgr, txmsg);
		1917
		1918	return 0;
		1919	}
		1920	#endif
		1921
		1922	static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
		1923	struct drm_dp_mst_port *port,
		1924	int offset, int size, u8 *bytes)
		1925	{
		1926	int len;
		1927	int ret;
		1928	struct drm_dp_sideband_msg_tx *txmsg;
		1929	struct drm_dp_mst_branch *mstb;
		1930
		1931	mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
		1932	if (!mstb)
		1933	return -EINVAL;
		1934
		1935	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1936	if (!txmsg) {
		1937	ret = -ENOMEM;
		1938	goto fail_put;
		1939	}
		1940
		1941	len = build_dpcd_write(txmsg, port->port_num, offset, size, bytes);
		1942	txmsg->dst = mstb;
		1943
		1944	drm_dp_queue_down_tx(mgr, txmsg);
		1945
		1946	ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
		1947	if (ret > 0) {
		1948	if (txmsg->reply.reply_type == 1) {
		1949	ret = -EINVAL;
		1950	} else
		1951	ret = 0;
		1952	}
		1953	kfree(txmsg);
		1954	fail_put:
		1955	drm_dp_put_mst_branch_device(mstb);
		1956	return ret;
		1957	}
		1958
		1959	static int drm_dp_encode_up_ack_reply(struct drm_dp_sideband_msg_tx *msg, u8 req_type)
		1960	{
		1961	struct drm_dp_sideband_msg_reply_body reply;
		1962
6937	serge	1963	reply.reply_type = 0;
5060	serge	1964	reply.req_type = req_type;
		1965	drm_dp_encode_sideband_reply(&reply, msg);
		1966	return 0;
		1967	}
		1968
		1969	static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
		1970	struct drm_dp_mst_branch *mstb,
		1971	int req_type, int seqno, bool broadcast)
		1972	{
		1973	struct drm_dp_sideband_msg_tx *txmsg;
		1974
		1975	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		1976	if (!txmsg)
		1977	return -ENOMEM;
		1978
		1979	txmsg->dst = mstb;
		1980	txmsg->seqno = seqno;
		1981	drm_dp_encode_up_ack_reply(txmsg, req_type);
		1982
		1983	mutex_lock(&mgr->qlock);
6320	serge	1984
		1985	process_single_up_tx_qlock(mgr, txmsg);
		1986
5060	serge	1987	mutex_unlock(&mgr->qlock);
6320	serge	1988
		1989	kfree(txmsg);
5060	serge	1990	return 0;
		1991	}
		1992
5271	serge	1993	static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
		1994	int dp_link_count,
		1995	int *out)
5060	serge	1996	{
		1997	switch (dp_link_bw) {
5271	serge	1998	default:
		1999	DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
		2000	dp_link_bw, dp_link_count);
		2001	return false;
		2002
5060	serge	2003	case DP_LINK_BW_1_62:
5271	serge	2004	out = 3 dp_link_count;
		2005	break;
5060	serge	2006	case DP_LINK_BW_2_7:
5271	serge	2007	out = 5 dp_link_count;
		2008	break;
5060	serge	2009	case DP_LINK_BW_5_4:
5271	serge	2010	out = 10 dp_link_count;
		2011	break;
5060	serge	2012	}
5271	serge	2013	return true;
5060	serge	2014	}
		2015
		2016	/**
		2017	* drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
		2018	* @mgr: manager to set state for
		2019	* @mst_state: true to enable MST on this connector - false to disable.
		2020	*
		2021	* This is called by the driver when it detects an MST capable device plugged
		2022	* into a DP MST capable port, or when a DP MST capable device is unplugged.
		2023	*/
		2024	int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool mst_state)
		2025	{
		2026	int ret = 0;
		2027	struct drm_dp_mst_branch *mstb = NULL;
		2028
		2029	mutex_lock(&mgr->lock);
		2030	if (mst_state == mgr->mst_state)
		2031	goto out_unlock;
		2032
		2033	mgr->mst_state = mst_state;
		2034	/* set the device into MST mode */
		2035	if (mst_state) {
		2036	WARN_ON(mgr->mst_primary);
		2037
		2038	/* get dpcd info */
		2039	ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
		2040	if (ret != DP_RECEIVER_CAP_SIZE) {
		2041	DRM_DEBUG_KMS("failed to read DPCD\n");
		2042	goto out_unlock;
		2043	}
		2044
5271	serge	2045	if (!drm_dp_get_vc_payload_bw(mgr->dpcd[1],
		2046	mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK,
		2047	&mgr->pbn_div)) {
		2048	ret = -EINVAL;
		2049	goto out_unlock;
		2050	}
		2051
5060	serge	2052	mgr->total_pbn = 2560;
		2053	mgr->total_slots = DIV_ROUND_UP(mgr->total_pbn, mgr->pbn_div);
		2054	mgr->avail_slots = mgr->total_slots;
		2055
		2056	/* add initial branch device at LCT 1 */
		2057	mstb = drm_dp_add_mst_branch_device(1, NULL);
		2058	if (mstb == NULL) {
		2059	ret = -ENOMEM;
		2060	goto out_unlock;
		2061	}
		2062	mstb->mgr = mgr;
		2063
		2064	/* give this the main reference */
		2065	mgr->mst_primary = mstb;
		2066	kref_get(&mgr->mst_primary->kref);
		2067
		2068	ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
7144	serge	2069	DP_MST_EN \| DP_UP_REQ_EN \| DP_UPSTREAM_IS_SRC);
5060	serge	2070	if (ret < 0) {
		2071	goto out_unlock;
		2072	}
		2073
6320	serge	2074	{
		2075	struct drm_dp_payload reset_pay;
		2076	reset_pay.start_slot = 0;
		2077	reset_pay.num_slots = 0x3f;
		2078	drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
5060	serge	2079	}
		2080
		2081	// queue_work(system_long_wq, &mgr->work);
		2082
		2083	ret = 0;
		2084	} else {
		2085	/* disable MST on the device */
		2086	mstb = mgr->mst_primary;
		2087	mgr->mst_primary = NULL;
		2088	/* this can fail if the device is gone */
		2089	drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL, 0);
		2090	ret = 0;
		2091	memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
		2092	mgr->payload_mask = 0;
		2093	set_bit(0, &mgr->payload_mask);
5271	serge	2094	mgr->vcpi_mask = 0;
5060	serge	2095	}
		2096
		2097	out_unlock:
		2098	mutex_unlock(&mgr->lock);
		2099	if (mstb)
		2100	drm_dp_put_mst_branch_device(mstb);
		2101	return ret;
		2102
		2103	}
		2104	EXPORT_SYMBOL(drm_dp_mst_topology_mgr_set_mst);
		2105
		2106	/**
		2107	* drm_dp_mst_topology_mgr_suspend() - suspend the MST manager
		2108	* @mgr: manager to suspend
		2109	*
		2110	* This function tells the MST device that we can't handle UP messages
		2111	* anymore. This should stop it from sending any since we are suspended.
		2112	*/
		2113	void drm_dp_mst_topology_mgr_suspend(struct drm_dp_mst_topology_mgr *mgr)
		2114	{
		2115	mutex_lock(&mgr->lock);
		2116	drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
		2117	DP_MST_EN \| DP_UPSTREAM_IS_SRC);
		2118	mutex_unlock(&mgr->lock);
		2119	}
		2120	EXPORT_SYMBOL(drm_dp_mst_topology_mgr_suspend);
		2121
		2122	/**
		2123	* drm_dp_mst_topology_mgr_resume() - resume the MST manager
		2124	* @mgr: manager to resume
		2125	*
		2126	* This will fetch DPCD and see if the device is still there,
		2127	* if it is, it will rewrite the MSTM control bits, and return.
		2128	*
		2129	* if the device fails this returns -1, and the driver should do
		2130	* a full MST reprobe, in case we were undocked.
		2131	*/
		2132	int drm_dp_mst_topology_mgr_resume(struct drm_dp_mst_topology_mgr *mgr)
		2133	{
		2134	int ret = 0;
		2135
		2136	mutex_lock(&mgr->lock);
		2137
		2138	if (mgr->mst_primary) {
		2139	int sret;
6660	serge	2140	u8 guid[16];
		2141
5060	serge	2142	sret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
		2143	if (sret != DP_RECEIVER_CAP_SIZE) {
		2144	DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
		2145	ret = -1;
		2146	goto out_unlock;
		2147	}
		2148
		2149	ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
		2150	DP_MST_EN \| DP_UP_REQ_EN \| DP_UPSTREAM_IS_SRC);
		2151	if (ret < 0) {
		2152	DRM_DEBUG_KMS("mst write failed - undocked during suspend?\n");
		2153	ret = -1;
		2154	goto out_unlock;
		2155	}
6660	serge	2156
		2157	/* Some hubs forget their guids after they resume */
		2158	sret = drm_dp_dpcd_read(mgr->aux, DP_GUID, guid, 16);
		2159	if (sret != 16) {
		2160	DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
		2161	ret = -1;
		2162	goto out_unlock;
		2163	}
		2164	drm_dp_check_mstb_guid(mgr->mst_primary, guid);
		2165
5060	serge	2166	ret = 0;
		2167	} else
		2168	ret = -1;
		2169
		2170	out_unlock:
		2171	mutex_unlock(&mgr->lock);
		2172	return ret;
		2173	}
		2174	EXPORT_SYMBOL(drm_dp_mst_topology_mgr_resume);
		2175
		2176	static void drm_dp_get_one_sb_msg(struct drm_dp_mst_topology_mgr *mgr, bool up)
		2177	{
		2178	int len;
		2179	u8 replyblock[32];
		2180	int replylen, origlen, curreply;
		2181	int ret;
		2182	struct drm_dp_sideband_msg_rx *msg;
		2183	int basereg = up ? DP_SIDEBAND_MSG_UP_REQ_BASE : DP_SIDEBAND_MSG_DOWN_REP_BASE;
		2184	msg = up ? &mgr->up_req_recv : &mgr->down_rep_recv;
		2185
		2186	len = min(mgr->max_dpcd_transaction_bytes, 16);
		2187	ret = drm_dp_dpcd_read(mgr->aux, basereg,
		2188	replyblock, len);
		2189	if (ret != len) {
		2190	DRM_DEBUG_KMS("failed to read DPCD down rep %d %d\n", len, ret);
		2191	return;
		2192	}
		2193	ret = drm_dp_sideband_msg_build(msg, replyblock, len, true);
		2194	if (!ret) {
		2195	DRM_DEBUG_KMS("sideband msg build failed %d\n", replyblock[0]);
		2196	return;
		2197	}
		2198	replylen = msg->curchunk_len + msg->curchunk_hdrlen;
		2199
		2200	origlen = replylen;
		2201	replylen -= len;
		2202	curreply = len;
		2203	while (replylen > 0) {
		2204	len = min3(replylen, mgr->max_dpcd_transaction_bytes, 16);
		2205	ret = drm_dp_dpcd_read(mgr->aux, basereg + curreply,
		2206	replyblock, len);
		2207	if (ret != len) {
		2208	DRM_DEBUG_KMS("failed to read a chunk\n");
		2209	}
		2210	ret = drm_dp_sideband_msg_build(msg, replyblock, len, false);
		2211	if (ret == false)
		2212	DRM_DEBUG_KMS("failed to build sideband msg\n");
		2213	curreply += len;
		2214	replylen -= len;
		2215	}
		2216	}
		2217
		2218	static int drm_dp_mst_handle_down_rep(struct drm_dp_mst_topology_mgr *mgr)
		2219	{
		2220	int ret = 0;
		2221
		2222	drm_dp_get_one_sb_msg(mgr, false);
		2223
		2224	if (mgr->down_rep_recv.have_eomt) {
		2225	struct drm_dp_sideband_msg_tx *txmsg;
		2226	struct drm_dp_mst_branch *mstb;
		2227	int slot = -1;
		2228	mstb = drm_dp_get_mst_branch_device(mgr,
		2229	mgr->down_rep_recv.initial_hdr.lct,
		2230	mgr->down_rep_recv.initial_hdr.rad);
		2231
		2232	if (!mstb) {
		2233	DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->down_rep_recv.initial_hdr.lct);
		2234	memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2235	return 0;
		2236	}
		2237
		2238	/* find the message */
		2239	slot = mgr->down_rep_recv.initial_hdr.seqno;
		2240	mutex_lock(&mgr->qlock);
		2241	txmsg = mstb->tx_slots[slot];
		2242	/* remove from slots */
		2243	mutex_unlock(&mgr->qlock);
		2244
		2245	if (!txmsg) {
		2246	DRM_DEBUG_KMS("Got MST reply with no msg %p %d %d %02x %02x\n",
		2247	mstb,
		2248	mgr->down_rep_recv.initial_hdr.seqno,
		2249	mgr->down_rep_recv.initial_hdr.lct,
		2250	mgr->down_rep_recv.initial_hdr.rad[0],
		2251	mgr->down_rep_recv.msg[0]);
		2252	drm_dp_put_mst_branch_device(mstb);
		2253	memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2254	return 0;
		2255	}
		2256
		2257	drm_dp_sideband_parse_reply(&mgr->down_rep_recv, &txmsg->reply);
		2258	if (txmsg->reply.reply_type == 1) {
		2259	DRM_DEBUG_KMS("Got NAK reply: req 0x%02x, reason 0x%02x, nak data 0x%02x\n", txmsg->reply.req_type, txmsg->reply.u.nak.reason, txmsg->reply.u.nak.nak_data);
		2260	}
		2261
		2262	memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2263	drm_dp_put_mst_branch_device(mstb);
		2264
		2265	mutex_lock(&mgr->qlock);
		2266	txmsg->state = DRM_DP_SIDEBAND_TX_RX;
		2267	mstb->tx_slots[slot] = NULL;
		2268	mutex_unlock(&mgr->qlock);
		2269
		2270	// wake_up(&mgr->tx_waitq);
		2271	}
		2272	return ret;
		2273	}
		2274
		2275	static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
		2276	{
		2277	int ret = 0;
		2278	drm_dp_get_one_sb_msg(mgr, true);
		2279
		2280	if (mgr->up_req_recv.have_eomt) {
		2281	struct drm_dp_sideband_msg_req_body msg;
6320	serge	2282	struct drm_dp_mst_branch *mstb = NULL;
5060	serge	2283	bool seqno;
6320	serge	2284
		2285	if (!mgr->up_req_recv.initial_hdr.broadcast) {
7144	serge	2286	mstb = drm_dp_get_mst_branch_device(mgr,
		2287	mgr->up_req_recv.initial_hdr.lct,
		2288	mgr->up_req_recv.initial_hdr.rad);
		2289	if (!mstb) {
		2290	DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
		2291	memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2292	return 0;
		2293	}
5060	serge	2294	}
		2295
		2296	seqno = mgr->up_req_recv.initial_hdr.seqno;
		2297	drm_dp_sideband_parse_req(&mgr->up_req_recv, &msg);
		2298
		2299	if (msg.req_type == DP_CONNECTION_STATUS_NOTIFY) {
6320	serge	2300	drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
		2301
		2302	if (!mstb)
		2303	mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.conn_stat.guid);
		2304
		2305	if (!mstb) {
		2306	DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
		2307	memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2308	return 0;
		2309	}
		2310
5060	serge	2311	drm_dp_update_port(mstb, &msg.u.conn_stat);
6320	serge	2312
5060	serge	2313	DRM_DEBUG_KMS("Got CSN: pn: %d ldps:%d ddps: %d mcs: %d ip: %d pdt: %d\n", msg.u.conn_stat.port_number, msg.u.conn_stat.legacy_device_plug_status, msg.u.conn_stat.displayport_device_plug_status, msg.u.conn_stat.message_capability_status, msg.u.conn_stat.input_port, msg.u.conn_stat.peer_device_type);
		2314	(*mgr->cbs->hotplug)(mgr);
		2315
		2316	} else if (msg.req_type == DP_RESOURCE_STATUS_NOTIFY) {
6320	serge	2317	drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
		2318	if (!mstb)
		2319	mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.resource_stat.guid);
		2320
		2321	if (!mstb) {
		2322	DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
		2323	memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2324	return 0;
		2325	}
		2326
5060	serge	2327	DRM_DEBUG_KMS("Got RSN: pn: %d avail_pbn %d\n", msg.u.resource_stat.port_number, msg.u.resource_stat.available_pbn);
		2328	}
		2329
		2330	drm_dp_put_mst_branch_device(mstb);
		2331	memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
		2332	}
		2333	return ret;
		2334	}
		2335
		2336	/**
		2337	* drm_dp_mst_hpd_irq() - MST hotplug IRQ notify
		2338	* @mgr: manager to notify irq for.
		2339	* @esi: 4 bytes from SINK_COUNT_ESI
5271	serge	2340	* @handled: whether the hpd interrupt was consumed or not
5060	serge	2341	*
		2342	* This should be called from the driver when it detects a short IRQ,
		2343	* along with the value of the DEVICE_SERVICE_IRQ_VECTOR_ESI0. The
		2344	* topology manager will process the sideband messages received as a result
		2345	* of this.
		2346	*/
		2347	int drm_dp_mst_hpd_irq(struct drm_dp_mst_topology_mgr mgr, u8 esi, bool *handled)
		2348	{
		2349	int ret = 0;
		2350	int sc;
		2351	*handled = false;
		2352	sc = esi[0] & 0x3f;
		2353
		2354	if (sc != mgr->sink_count) {
		2355	mgr->sink_count = sc;
		2356	*handled = true;
		2357	}
		2358
		2359	if (esi[1] & DP_DOWN_REP_MSG_RDY) {
		2360	ret = drm_dp_mst_handle_down_rep(mgr);
		2361	*handled = true;
		2362	}
		2363
		2364	if (esi[1] & DP_UP_REQ_MSG_RDY) {
		2365	ret \|= drm_dp_mst_handle_up_req(mgr);
		2366	*handled = true;
		2367	}
		2368
		2369	drm_dp_mst_kick_tx(mgr);
		2370	return ret;
		2371	}
		2372	EXPORT_SYMBOL(drm_dp_mst_hpd_irq);
		2373
		2374	/**
		2375	* drm_dp_mst_detect_port() - get connection status for an MST port
		2376	* @mgr: manager for this port
		2377	* @port: unverified pointer to a port
		2378	*
		2379	* This returns the current connection state for a port. It validates the
		2380	* port pointer still exists so the caller doesn't require a reference
		2381	*/
5271	serge	2382	enum drm_connector_status drm_dp_mst_detect_port(struct drm_connector *connector,
		2383	struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port)
5060	serge	2384	{
		2385	enum drm_connector_status status = connector_status_disconnected;
		2386
		2387	/* we need to search for the port in the mgr in case its gone */
		2388	port = drm_dp_get_validated_port_ref(mgr, port);
		2389	if (!port)
		2390	return connector_status_disconnected;
		2391
		2392	if (!port->ddps)
		2393	goto out;
		2394
		2395	switch (port->pdt) {
		2396	case DP_PEER_DEVICE_NONE:
		2397	case DP_PEER_DEVICE_MST_BRANCHING:
		2398	break;
		2399
		2400	case DP_PEER_DEVICE_SST_SINK:
		2401	status = connector_status_connected;
5271	serge	2402	/* for logical ports - cache the EDID */
		2403	if (port->port_num >= 8 && !port->cached_edid) {
		2404	port->cached_edid = drm_get_edid(connector, &port->aux.ddc);
		2405	}
5060	serge	2406	break;
		2407	case DP_PEER_DEVICE_DP_LEGACY_CONV:
		2408	if (port->ldps)
		2409	status = connector_status_connected;
		2410	break;
		2411	}
		2412	out:
		2413	drm_dp_put_port(port);
		2414	return status;
		2415	}
		2416	EXPORT_SYMBOL(drm_dp_mst_detect_port);
		2417
		2418	/**
6937	serge	2419	* drm_dp_mst_port_has_audio() - Check whether port has audio capability or not
		2420	* @mgr: manager for this port
		2421	* @port: unverified pointer to a port.
		2422	*
		2423	* This returns whether the port supports audio or not.
		2424	*/
		2425	bool drm_dp_mst_port_has_audio(struct drm_dp_mst_topology_mgr *mgr,
		2426	struct drm_dp_mst_port *port)
		2427	{
		2428	bool ret = false;
		2429
		2430	port = drm_dp_get_validated_port_ref(mgr, port);
		2431	if (!port)
		2432	return ret;
		2433	ret = port->has_audio;
		2434	drm_dp_put_port(port);
		2435	return ret;
		2436	}
		2437	EXPORT_SYMBOL(drm_dp_mst_port_has_audio);
		2438
		2439	/**
5060	serge	2440	* drm_dp_mst_get_edid() - get EDID for an MST port
		2441	* @connector: toplevel connector to get EDID for
		2442	* @mgr: manager for this port
		2443	* @port: unverified pointer to a port.
		2444	*
		2445	* This returns an EDID for the port connected to a connector,
		2446	* It validates the pointer still exists so the caller doesn't require a
		2447	* reference.
		2448	*/
		2449	struct edid drm_dp_mst_get_edid(struct drm_connector connector, struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port)
		2450	{
		2451	struct edid *edid = NULL;
		2452
		2453	/* we need to search for the port in the mgr in case its gone */
		2454	port = drm_dp_get_validated_port_ref(mgr, port);
		2455	if (!port)
		2456	return NULL;
		2457
5271	serge	2458	if (port->cached_edid)
		2459	edid = drm_edid_duplicate(port->cached_edid);
6084	serge	2460	else {
		2461	edid = drm_get_edid(connector, &port->aux.ddc);
		2462	drm_mode_connector_set_tile_property(connector);
		2463	}
6937	serge	2464	port->has_audio = drm_detect_monitor_audio(edid);
5060	serge	2465	drm_dp_put_port(port);
		2466	return edid;
		2467	}
		2468	EXPORT_SYMBOL(drm_dp_mst_get_edid);
		2469
		2470	/**
		2471	* drm_dp_find_vcpi_slots() - find slots for this PBN value
		2472	* @mgr: manager to use
		2473	* @pbn: payload bandwidth to convert into slots.
		2474	*/
		2475	int drm_dp_find_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr,
		2476	int pbn)
		2477	{
		2478	int num_slots;
		2479
		2480	num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
		2481
		2482	if (num_slots > mgr->avail_slots)
		2483	return -ENOSPC;
		2484	return num_slots;
		2485	}
		2486	EXPORT_SYMBOL(drm_dp_find_vcpi_slots);
		2487
		2488	static int drm_dp_init_vcpi(struct drm_dp_mst_topology_mgr *mgr,
		2489	struct drm_dp_vcpi *vcpi, int pbn)
		2490	{
		2491	int num_slots;
		2492	int ret;
		2493
		2494	num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
		2495
		2496	if (num_slots > mgr->avail_slots)
		2497	return -ENOSPC;
		2498
		2499	vcpi->pbn = pbn;
		2500	vcpi->aligned_pbn = num_slots * mgr->pbn_div;
		2501	vcpi->num_slots = num_slots;
		2502
		2503	ret = drm_dp_mst_assign_payload_id(mgr, vcpi);
		2504	if (ret < 0)
		2505	return ret;
		2506	return 0;
		2507	}
		2508
		2509	/**
		2510	* drm_dp_mst_allocate_vcpi() - Allocate a virtual channel
		2511	* @mgr: manager for this port
		2512	* @port: port to allocate a virtual channel for.
		2513	* @pbn: payload bandwidth number to request
		2514	* @slots: returned number of slots for this PBN.
		2515	*/
		2516	bool drm_dp_mst_allocate_vcpi(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port, int pbn, int *slots)
		2517	{
		2518	int ret;
		2519
		2520	port = drm_dp_get_validated_port_ref(mgr, port);
		2521	if (!port)
		2522	return false;
		2523
		2524	if (port->vcpi.vcpi > 0) {
		2525	DRM_DEBUG_KMS("payload: vcpi %d already allocated for pbn %d - requested pbn %d\n", port->vcpi.vcpi, port->vcpi.pbn, pbn);
		2526	if (pbn == port->vcpi.pbn) {
		2527	*slots = port->vcpi.num_slots;
6320	serge	2528	drm_dp_put_port(port);
5060	serge	2529	return true;
		2530	}
		2531	}
		2532
		2533	ret = drm_dp_init_vcpi(mgr, &port->vcpi, pbn);
		2534	if (ret) {
		2535	DRM_DEBUG_KMS("failed to init vcpi %d %d %d\n", DIV_ROUND_UP(pbn, mgr->pbn_div), mgr->avail_slots, ret);
		2536	goto out;
		2537	}
		2538	DRM_DEBUG_KMS("initing vcpi for %d %d\n", pbn, port->vcpi.num_slots);
		2539	*slots = port->vcpi.num_slots;
		2540
		2541	drm_dp_put_port(port);
		2542	return true;
		2543	out:
		2544	return false;
		2545	}
		2546	EXPORT_SYMBOL(drm_dp_mst_allocate_vcpi);
		2547
6084	serge	2548	int drm_dp_mst_get_vcpi_slots(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port)
		2549	{
		2550	int slots = 0;
		2551	port = drm_dp_get_validated_port_ref(mgr, port);
		2552	if (!port)
		2553	return slots;
		2554
		2555	slots = port->vcpi.num_slots;
		2556	drm_dp_put_port(port);
		2557	return slots;
		2558	}
		2559	EXPORT_SYMBOL(drm_dp_mst_get_vcpi_slots);
		2560
5060	serge	2561	/**
		2562	* drm_dp_mst_reset_vcpi_slots() - Reset number of slots to 0 for VCPI
		2563	* @mgr: manager for this port
		2564	* @port: unverified pointer to a port.
		2565	*
		2566	* This just resets the number of slots for the ports VCPI for later programming.
		2567	*/
		2568	void drm_dp_mst_reset_vcpi_slots(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port)
		2569	{
		2570	port = drm_dp_get_validated_port_ref(mgr, port);
		2571	if (!port)
		2572	return;
		2573	port->vcpi.num_slots = 0;
		2574	drm_dp_put_port(port);
		2575	}
		2576	EXPORT_SYMBOL(drm_dp_mst_reset_vcpi_slots);
		2577
		2578	/**
		2579	* drm_dp_mst_deallocate_vcpi() - deallocate a VCPI
		2580	* @mgr: manager for this port
		2581	* @port: unverified port to deallocate vcpi for
		2582	*/
		2583	void drm_dp_mst_deallocate_vcpi(struct drm_dp_mst_topology_mgr mgr, struct drm_dp_mst_port port)
		2584	{
		2585	port = drm_dp_get_validated_port_ref(mgr, port);
		2586	if (!port)
		2587	return;
		2588
		2589	drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
		2590	port->vcpi.num_slots = 0;
		2591	port->vcpi.pbn = 0;
		2592	port->vcpi.aligned_pbn = 0;
		2593	port->vcpi.vcpi = 0;
		2594	drm_dp_put_port(port);
		2595	}
		2596	EXPORT_SYMBOL(drm_dp_mst_deallocate_vcpi);
		2597
		2598	static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
		2599	int id, struct drm_dp_payload *payload)
		2600	{
		2601	u8 payload_alloc[3], status;
		2602	int ret;
		2603	int retries = 0;
		2604
		2605	drm_dp_dpcd_writeb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS,
		2606	DP_PAYLOAD_TABLE_UPDATED);
		2607
		2608	payload_alloc[0] = id;
		2609	payload_alloc[1] = payload->start_slot;
		2610	payload_alloc[2] = payload->num_slots;
		2611
		2612	ret = drm_dp_dpcd_write(mgr->aux, DP_PAYLOAD_ALLOCATE_SET, payload_alloc, 3);
		2613	if (ret != 3) {
		2614	DRM_DEBUG_KMS("failed to write payload allocation %d\n", ret);
		2615	goto fail;
		2616	}
		2617
		2618	retry:
		2619	ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
		2620	if (ret < 0) {
		2621	DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
		2622	goto fail;
		2623	}
		2624
		2625	if (!(status & DP_PAYLOAD_TABLE_UPDATED)) {
		2626	retries++;
		2627	if (retries < 20) {
		2628	usleep_range(10000, 20000);
		2629	goto retry;
		2630	}
		2631	DRM_DEBUG_KMS("status not set after read payload table status %d\n", status);
		2632	ret = -EINVAL;
		2633	goto fail;
		2634	}
		2635	ret = 0;
		2636	fail:
		2637	return ret;
		2638	}
		2639
		2640
		2641	/**
		2642	* drm_dp_check_act_status() - Check ACT handled status.
		2643	* @mgr: manager to use
		2644	*
		2645	* Check the payload status bits in the DPCD for ACT handled completion.
		2646	*/
		2647	int drm_dp_check_act_status(struct drm_dp_mst_topology_mgr *mgr)
		2648	{
		2649	u8 status;
		2650	int ret;
		2651	int count = 0;
		2652
		2653	do {
		2654	ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
		2655
		2656	if (ret < 0) {
		2657	DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
		2658	goto fail;
		2659	}
		2660
		2661	if (status & DP_PAYLOAD_ACT_HANDLED)
		2662	break;
		2663	count++;
		2664	udelay(100);
		2665
		2666	} while (count < 30);
		2667
		2668	if (!(status & DP_PAYLOAD_ACT_HANDLED)) {
		2669	DRM_DEBUG_KMS("failed to get ACT bit %d after %d retries\n", status, count);
		2670	ret = -EINVAL;
		2671	goto fail;
		2672	}
		2673	return 0;
		2674	fail:
		2675	return ret;
		2676	}
		2677	EXPORT_SYMBOL(drm_dp_check_act_status);
		2678
		2679	/**
		2680	* drm_dp_calc_pbn_mode() - Calculate the PBN for a mode.
		2681	* @clock: dot clock for the mode
		2682	* @bpp: bpp for the mode.
		2683	*
		2684	* This uses the formula in the spec to calculate the PBN value for a mode.
		2685	*/
		2686	int drm_dp_calc_pbn_mode(int clock, int bpp)
		2687	{
6320	serge	2688	u64 kbps;
		2689	s64 peak_kbps;
		2690	u32 numerator;
		2691	u32 denominator;
5060	serge	2692
6320	serge	2693	kbps = clock * bpp;
5060	serge	2694
6320	serge	2695	/*
		2696	* margin 5300ppm + 300ppm ~ 0.6% as per spec, factor is 1.006
		2697	* The unit of 54/64Mbytes/sec is an arbitrary unit chosen based on
		2698	* common multiplier to render an integer PBN for all link rate/lane
		2699	* counts combinations
		2700	* calculate
		2701	* peak_kbps *= (1006/1000)
		2702	* peak_kbps *= (64/54)
		2703	* peak_kbps *= 8 convert to bytes
		2704	*/
5060	serge	2705
6320	serge	2706	numerator = 64 * 1006;
		2707	denominator = 54 * 8 * 1000 * 1000;
5060	serge	2708
6320	serge	2709	kbps *= numerator;
		2710	peak_kbps = drm_fixp_from_fraction(kbps, denominator);
		2711
		2712	return drm_fixp2int_ceil(peak_kbps);
5060	serge	2713	}
		2714	EXPORT_SYMBOL(drm_dp_calc_pbn_mode);
		2715
		2716	static int test_calc_pbn_mode(void)
		2717	{
		2718	int ret;
		2719	ret = drm_dp_calc_pbn_mode(154000, 30);
6320	serge	2720	if (ret != 689) {
		2721	DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
		2722	154000, 30, 689, ret);
5060	serge	2723	return -EINVAL;
6320	serge	2724	}
5060	serge	2725	ret = drm_dp_calc_pbn_mode(234000, 30);
6320	serge	2726	if (ret != 1047) {
		2727	DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
		2728	234000, 30, 1047, ret);
5060	serge	2729	return -EINVAL;
6320	serge	2730	}
		2731	ret = drm_dp_calc_pbn_mode(297000, 24);
		2732	if (ret != 1063) {
		2733	DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
		2734	297000, 24, 1063, ret);
		2735	return -EINVAL;
		2736	}
5060	serge	2737	return 0;
		2738	}
		2739
		2740	/* we want to kick the TX after we've ack the up/down IRQs. */
		2741	static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr)
		2742	{
		2743	// queue_work(system_long_wq, &mgr->tx_work);
		2744	}
		2745
		2746	static void drm_dp_mst_dump_mstb(struct seq_file *m,
		2747	struct drm_dp_mst_branch *mstb)
		2748	{
		2749	struct drm_dp_mst_port *port;
		2750	int tabs = mstb->lct;
		2751	char prefix[10];
		2752	int i;
		2753
		2754	for (i = 0; i < tabs; i++)
		2755	prefix[i] = '\t';
		2756	prefix[i] = '\0';
		2757
		2758	// seq_printf(m, "%smst: %p, %d\n", prefix, mstb, mstb->num_ports);
		2759	// list_for_each_entry(port, &mstb->ports, next) {
		2760	// seq_printf(m, "%sport: %d: ddps: %d ldps: %d, %p, conn: %p\n", prefix, port->port_num, port->ddps, port->ldps, port, port->connector);
		2761	// if (port->mstb)
		2762	// drm_dp_mst_dump_mstb(m, port->mstb);
		2763	// }
		2764	}
		2765
		2766	static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
		2767	char *buf)
		2768	{
		2769	int ret;
		2770	int i;
		2771	for (i = 0; i < 4; i++) {
		2772	ret = drm_dp_dpcd_read(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS + (i * 16), &buf[i * 16], 16);
		2773	if (ret != 16)
		2774	break;
		2775	}
		2776	if (i == 4)
		2777	return true;
		2778	return false;
		2779	}
		2780
		2781	/**
		2782	* drm_dp_mst_dump_topology(): dump topology to seq file.
		2783	* @m: seq_file to dump output to
		2784	* @mgr: manager to dump current topology for.
		2785	*
		2786	* helper to dump MST topology to a seq file for debugfs.
		2787	*/
		2788	void drm_dp_mst_dump_topology(struct seq_file *m,
		2789	struct drm_dp_mst_topology_mgr *mgr)
		2790	{
		2791	int i;
		2792	struct drm_dp_mst_port *port;
		2793	mutex_lock(&mgr->lock);
		2794	if (mgr->mst_primary)
		2795	drm_dp_mst_dump_mstb(m, mgr->mst_primary);
		2796
		2797	/* dump VCPIs */
		2798	mutex_unlock(&mgr->lock);
		2799
		2800
		2801
		2802	}
		2803	EXPORT_SYMBOL(drm_dp_mst_dump_topology);
		2804
		2805	static void drm_dp_tx_work(struct work_struct *work)
		2806	{
		2807	struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, tx_work);
		2808
		2809	mutex_lock(&mgr->qlock);
		2810	if (mgr->tx_down_in_progress)
		2811	process_single_down_tx_qlock(mgr);
		2812	mutex_unlock(&mgr->qlock);
		2813	}
		2814
6320	serge	2815	static void drm_dp_free_mst_port(struct kref *kref)
		2816	{
		2817	struct drm_dp_mst_port *port = container_of(kref, struct drm_dp_mst_port, kref);
		2818	kref_put(&port->parent->kref, drm_dp_free_mst_branch_device);
		2819	kfree(port);
		2820	}
5060	serge	2821	/**
		2822	* drm_dp_mst_topology_mgr_init - initialise a topology manager
		2823	* @mgr: manager struct to initialise
		2824	* @dev: device providing this structure - for i2c addition.
		2825	* @aux: DP helper aux channel to talk to this device
		2826	* @max_dpcd_transaction_bytes: hw specific DPCD transaction limit
		2827	* @max_payloads: maximum number of payloads this GPU can source
		2828	* @conn_base_id: the connector object ID the MST device is connected to.
		2829	*
		2830	* Return 0 for success, or negative error code on failure
		2831	*/
		2832	int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
		2833	struct device dev, struct drm_dp_aux aux,
		2834	int max_dpcd_transaction_bytes,
		2835	int max_payloads, int conn_base_id)
		2836	{
		2837	mutex_init(&mgr->lock);
		2838	mutex_init(&mgr->qlock);
		2839	mutex_init(&mgr->payload_lock);
6084	serge	2840	mutex_init(&mgr->destroy_connector_lock);
5060	serge	2841	INIT_LIST_HEAD(&mgr->tx_msg_downq);
6084	serge	2842	INIT_LIST_HEAD(&mgr->destroy_connector_list);
5060	serge	2843	INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
		2844	INIT_WORK(&mgr->tx_work, drm_dp_tx_work);
6088	serge	2845	init_waitqueue_head(&mgr->tx_waitq);
5060	serge	2846	mgr->dev = dev;
		2847	mgr->aux = aux;
		2848	mgr->max_dpcd_transaction_bytes = max_dpcd_transaction_bytes;
		2849	mgr->max_payloads = max_payloads;
		2850	mgr->conn_base_id = conn_base_id;
6937	serge	2851	if (max_payloads + 1 > sizeof(mgr->payload_mask) * 8 \|\|
		2852	max_payloads + 1 > sizeof(mgr->vcpi_mask) * 8)
		2853	return -EINVAL;
5060	serge	2854	mgr->payloads = kcalloc(max_payloads, sizeof(struct drm_dp_payload), GFP_KERNEL);
		2855	if (!mgr->payloads)
		2856	return -ENOMEM;
		2857	mgr->proposed_vcpis = kcalloc(max_payloads, sizeof(struct drm_dp_vcpi *), GFP_KERNEL);
		2858	if (!mgr->proposed_vcpis)
		2859	return -ENOMEM;
		2860	set_bit(0, &mgr->payload_mask);
6937	serge	2861	if (test_calc_pbn_mode() < 0)
		2862	DRM_ERROR("MST PBN self-test failed\n");
		2863
5060	serge	2864	return 0;
		2865	}
		2866	EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
		2867
		2868	/**
		2869	* drm_dp_mst_topology_mgr_destroy() - destroy topology manager.
		2870	* @mgr: manager to destroy
		2871	*/
		2872	void drm_dp_mst_topology_mgr_destroy(struct drm_dp_mst_topology_mgr *mgr)
		2873	{
		2874	mutex_lock(&mgr->payload_lock);
		2875	kfree(mgr->payloads);
		2876	mgr->payloads = NULL;
		2877	kfree(mgr->proposed_vcpis);
		2878	mgr->proposed_vcpis = NULL;
		2879	mutex_unlock(&mgr->payload_lock);
		2880	mgr->dev = NULL;
		2881	mgr->aux = NULL;
		2882	}
		2883	EXPORT_SYMBOL(drm_dp_mst_topology_mgr_destroy);
		2884
		2885	/* I2C device */
		2886	static int drm_dp_mst_i2c_xfer(struct i2c_adapter adapter, struct i2c_msg msgs,
		2887	int num)
		2888	{
		2889	struct drm_dp_aux *aux = adapter->algo_data;
		2890	struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port, aux);
		2891	struct drm_dp_mst_branch *mstb;
		2892	struct drm_dp_mst_topology_mgr *mgr = port->mgr;
		2893	unsigned int i;
		2894	bool reading = false;
		2895	struct drm_dp_sideband_msg_req_body msg;
		2896	struct drm_dp_sideband_msg_tx *txmsg = NULL;
		2897	int ret;
		2898
		2899	mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
		2900	if (!mstb)
		2901	return -EREMOTEIO;
		2902
		2903	/* construct i2c msg */
		2904	/* see if last msg is a read */
		2905	if (msgs[num - 1].flags & I2C_M_RD)
		2906	reading = true;
		2907
6084	serge	2908	if (!reading \|\| (num - 1 > DP_REMOTE_I2C_READ_MAX_TRANSACTIONS)) {
5060	serge	2909	DRM_DEBUG_KMS("Unsupported I2C transaction for MST device\n");
		2910	ret = -EIO;
		2911	goto out;
		2912	}
		2913
6084	serge	2914	memset(&msg, 0, sizeof(msg));
5060	serge	2915	msg.req_type = DP_REMOTE_I2C_READ;
		2916	msg.u.i2c_read.num_transactions = num - 1;
		2917	msg.u.i2c_read.port_number = port->port_num;
		2918	for (i = 0; i < num - 1; i++) {
		2919	msg.u.i2c_read.transactions[i].i2c_dev_id = msgs[i].addr;
		2920	msg.u.i2c_read.transactions[i].num_bytes = msgs[i].len;
		2921	msg.u.i2c_read.transactions[i].bytes = msgs[i].buf;
		2922	}
		2923	msg.u.i2c_read.read_i2c_device_id = msgs[num - 1].addr;
		2924	msg.u.i2c_read.num_bytes_read = msgs[num - 1].len;
		2925
		2926	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
		2927	if (!txmsg) {
		2928	ret = -ENOMEM;
		2929	goto out;
		2930	}
		2931
		2932	txmsg->dst = mstb;
		2933	drm_dp_encode_sideband_req(&msg, txmsg);
		2934
		2935	drm_dp_queue_down_tx(mgr, txmsg);
		2936
		2937	ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
		2938	if (ret > 0) {
		2939
		2940	if (txmsg->reply.reply_type == 1) { /* got a NAK back */
		2941	ret = -EREMOTEIO;
		2942	goto out;
		2943	}
		2944	if (txmsg->reply.u.remote_i2c_read_ack.num_bytes != msgs[num - 1].len) {
		2945	ret = -EIO;
		2946	goto out;
		2947	}
		2948	memcpy(msgs[num - 1].buf, txmsg->reply.u.remote_i2c_read_ack.bytes, msgs[num - 1].len);
		2949	ret = num;
		2950	}
		2951	out:
		2952	kfree(txmsg);
		2953	drm_dp_put_mst_branch_device(mstb);
		2954	return ret;
		2955	}
		2956
		2957	static u32 drm_dp_mst_i2c_functionality(struct i2c_adapter *adapter)
		2958	{
		2959	return I2C_FUNC_I2C \| I2C_FUNC_SMBUS_EMUL \|
		2960	I2C_FUNC_SMBUS_READ_BLOCK_DATA \|
		2961	I2C_FUNC_SMBUS_BLOCK_PROC_CALL \|
		2962	I2C_FUNC_10BIT_ADDR;
		2963	}
		2964
		2965	static const struct i2c_algorithm drm_dp_mst_i2c_algo = {
		2966	.functionality = drm_dp_mst_i2c_functionality,
		2967	.master_xfer = drm_dp_mst_i2c_xfer,
		2968	};
		2969
		2970	/**
		2971	* drm_dp_mst_register_i2c_bus() - register an I2C adapter for I2C-over-AUX
		2972	* @aux: DisplayPort AUX channel
		2973	*
		2974	* Returns 0 on success or a negative error code on failure.
		2975	*/
		2976	static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux)
		2977	{
		2978	aux->ddc.algo = &drm_dp_mst_i2c_algo;
		2979	aux->ddc.algo_data = aux;
		2980	aux->ddc.retries = 3;
		2981
		2982	aux->ddc.class = I2C_CLASS_DDC;
		2983	aux->ddc.owner = THIS_MODULE;
		2984	aux->ddc.dev.parent = aux->dev;
		2985
		2986	return i2c_add_adapter(&aux->ddc);
		2987	}
		2988
		2989	/**
		2990	* drm_dp_mst_unregister_i2c_bus() - unregister an I2C-over-AUX adapter
		2991	* @aux: DisplayPort AUX channel
		2992	*/
		2993	static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux)
		2994	{
		2995	i2c_del_adapter(&aux->ddc);
		2996	}

Subversion Repositories Kolibri OS

(root)/drivers/video/drm/drm_dp_mst_topology.c – Rev 7144