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

Subversion Repositories Kolibri OS

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