From 72069d67a0519bff6216f3d4c31db5fc72949216 Mon Sep 17 00:00:00 2001
From: Andrei-Liviu Simion <andrei.simion@xilinx.com>
Date: Thu, 23 Apr 2015 13:27:00 -0700
Subject: [PATCH] dp: rx: mst: Format down reply message for REMOTE_DPCD_READ
 down requests.

Signed-off-by: Andrei-Liviu Simion <andrei.simion@xilinx.com>
---
 XilinxProcessorIPLib/drivers/dp/src/xdp.h     | 16 +++++
 XilinxProcessorIPLib/drivers/dp/src/xdp_mst.c | 69 +++++++++++++++++++
 2 files changed, 85 insertions(+)

diff --git a/XilinxProcessorIPLib/drivers/dp/src/xdp.h b/XilinxProcessorIPLib/drivers/dp/src/xdp.h
index 564ca812..723276e0 100644
--- a/XilinxProcessorIPLib/drivers/dp/src/xdp.h
+++ b/XilinxProcessorIPLib/drivers/dp/src/xdp.h
@@ -592,6 +592,17 @@ typedef struct {
 						application. */
 } XDp_RxIicMapEntry;
 
+/**
+ * This typedef represents the DPCD address map for a device. This is used to
+ * allow the user to define a DPCD map for any sink connected to one of the RX's
+ * ports.
+ */
+typedef struct {
+	u8 *DataPtr;
+	u32 NumBytes;
+	u32 StartAddr;
+} XDp_RxDpcdMap;
+
 /**
  * This typedef contains information on the directly connected ports to the RX
  * branch. Information contained in XDp_SbMsgLinkAddressReplyDeviceInfo is not
@@ -606,6 +617,11 @@ typedef struct {
 						allows the user to define up to
 						XDP_RX_NUM_I2C_ENTRIES_PER_PORT
 						I2C addresses per port. */
+	XDp_RxDpcdMap DpcdMap;		/**< When the RX replies to a
+						REMOTE_DPCD_READ sideband
+						message, it responds with the
+						associated DPCD map for the
+						requested port. */
 	u8 Exposed;			/**< When set to 1, the RX branch device
 						will expose the port in the
 						LINK_ADDRESS reply. */
diff --git a/XilinxProcessorIPLib/drivers/dp/src/xdp_mst.c b/XilinxProcessorIPLib/drivers/dp/src/xdp_mst.c
index fd3279d6..d5cbf166 100644
--- a/XilinxProcessorIPLib/drivers/dp/src/xdp_mst.c
+++ b/XilinxProcessorIPLib/drivers/dp/src/xdp_mst.c
@@ -150,6 +150,7 @@ typedef struct
 
 static void XDp_RxSetClearPayloadIdReply(XDp_SidebandMsg *Msg);
 static void XDp_RxSetAllocPayloadReply(XDp_SidebandMsg *Msg);
+static u32 XDp_RxSetRemoteDpcdReadReply(XDp *InstancePtr, XDp_SidebandMsg *Msg);
 static void XDp_TxIssueGuid(XDp *InstancePtr, u8 LinkCountTotal,
 			u8 *RelativeAddress, XDp_TxTopology *Topology,
 			u32 *Guid);
@@ -2570,6 +2571,74 @@ static void XDp_RxSetAllocPayloadReply(XDp_SidebandMsg *Msg)
 	Msg->Body.MsgDataLength = ReplyIndex;
 }
 
+/******************************************************************************/
+/**
+ * This function will set and format a sideband message structure for replying
+ * to a REMOTE_DPCD_READ down request.
+ *
+ * @param	Msg is a pointer to the message to be formatted.
+ *
+ * @return	None.
+ *
+ * @note	None.
+ *
+*******************************************************************************/
+static u32 XDp_RxSetRemoteDpcdReadReply(XDp *InstancePtr, XDp_SidebandMsg *Msg)
+{
+	u8 NumReadBytes;
+	u8 ReplyIndex = 0;
+	u8 Index;
+	u32 DpcdReadIndex;
+	u8 PortNum;
+	u32 DpcdReadAddress;
+	XDp_RxDpcdMap *DpcdMap;
+	u32 DpcdMapEndAddr;
+
+	PortNum = Msg->Body.MsgData[1] >> 4;
+	DpcdReadAddress = ((Msg->Body.MsgData[1] & 0xF) << 16) |
+			(Msg->Body.MsgData[2] << 8) | Msg->Body.MsgData[3];
+	NumReadBytes = Msg->Body.MsgData[4];
+
+	Msg->Header.LinkCountTotal = 1;
+	Msg->Header.LinkCountRemaining = 0;
+	Msg->Header.BroadcastMsg = 0;
+	Msg->Header.PathMsg = 0;
+	Msg->Header.MsgHeaderLength = 3;
+
+	Msg->Body.MsgData[ReplyIndex++] = XDP_TX_SBMSG_REMOTE_DPCD_READ;
+	Msg->Body.MsgData[ReplyIndex++] = PortNum;
+	Msg->Body.MsgData[ReplyIndex++] = NumReadBytes;
+
+	DpcdMap = &InstancePtr->RxInstance.Topology.Ports[PortNum].DpcdMap;
+
+	if (!DpcdMap->DataPtr) {
+		/* Supply garbage data if the targeted port has no associated
+		 * DPCD map. */
+		memset(&Msg->Body.MsgData[ReplyIndex], 0x00, NumReadBytes);
+			Msg->Body.MsgDataLength = ReplyIndex + NumReadBytes;
+		return XST_FAILURE;
+	}
+	DpcdMapEndAddr = (DpcdMap->StartAddr + DpcdMap->NumBytes - 1);
+
+	for (Index = 0; Index < NumReadBytes; Index++) {
+		DpcdReadIndex = DpcdReadAddress + Index;
+
+		if ((DpcdReadIndex >= DpcdMap->StartAddr) &&
+					(DpcdReadIndex <= DpcdMapEndAddr)) {
+			Msg->Body.MsgData[ReplyIndex++] = DpcdMap->DataPtr[
+					DpcdReadIndex - DpcdMap->StartAddr];
+		}
+		else {
+			/* Supply garbage data if the read is out of range. */
+			Msg->Body.MsgData[ReplyIndex++] = 0x00;
+		}
+	}
+
+	Msg->Body.MsgDataLength = ReplyIndex;
+
+	return XST_SUCCESS;
+}
+
 /******************************************************************************/
 /**
  * This function will check whether or not a DisplayPort device has a global