msm: pcie: calculate EP's capability register offsets

The start address of the capability register varies
depending on the endpoint. This change calculates the
endpoint's capability register offset instead of using
a fixed one.

Change-Id: I28a97d316aee8c34afe313838b91fcc06af0847f
Signed-off-by: Tony Truong <truong@codeaurora.org>

1 files changed, 86 insertions, 36 deletions

diff --git a/drivers/pci/host/pci-msm.c b/drivers/pci/host/pci-msm.c
index 01ff8b8fca1f..0ca874fbc763 100644
--- a/drivers/pci/host/pci-msm.c
+++ b/drivers/pci/host/pci-msm.c
@@ -1190,6 +1190,29 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 	u32 current_offset = 0;
 	u32 ep_l1sub_ctrl1_offset = 0;
 	u32 ep_l1sub_cap_reg1_offset = 0;
+	u32 ep_link_ctrlstts_offset = 0;
+	u32 ep_dev_ctrl2stts2_offset = 0;
+
+	current_offset = readl_relaxed(dev->conf + PCIE_CAP_PTR_OFFSET) & 0xff;
+
+	while (current_offset) {
+		val = readl_relaxed(dev->conf + current_offset);
+		if ((val & 0xff) == PCIE20_CAP_ID) {
+			ep_link_ctrlstts_offset = current_offset + 0x10;
+			ep_dev_ctrl2stts2_offset = current_offset + 0x28;
+			break;
+		}
+		current_offset = (val >> 8) & 0xff;
+	}
+
+	if (!ep_link_ctrlstts_offset)
+		PCIE_DBG(dev,
+			"RC%d endpoint does not support PCIe capability registers\n",
+			dev->rc_idx);
+	else
+		PCIE_DBG(dev,
+			"RC%d: ep_link_ctrlstts_offset: 0x%x\n",
+			dev->rc_idx, ep_link_ctrlstts_offset);
 
 	switch (testcase) {
 	case 0: /* output status */
@@ -1264,22 +1287,22 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 				PCIE20_CAP_LINKCTRLSTATUS,
 				BIT(0), 0);
 		msm_pcie_write_mask(dev->conf +
-				PCIE20_CAP_LINKCTRLSTATUS,
+				ep_link_ctrlstts_offset,
 				BIT(0), 0);
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 					readl_relaxed(dev->dm_core +
 					PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 					readl_relaxed(dev->conf +
-					PCIE20_CAP_LINKCTRLSTATUS);
+					ep_link_ctrlstts_offset);
 		}
 		pr_alert("PCIe: RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		pr_alert("PCIe: EP's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_CAP_LINKCTRLSTATUS));
+			ep_link_ctrlstts_offset));
 		break;
 	case 6: /* enable L0s */
 		pr_alert("\n\nPCIe: RC%d: enable L0s\n\n",
@@ -1288,22 +1311,22 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 				PCIE20_CAP_LINKCTRLSTATUS,
 				0, BIT(0));
 		msm_pcie_write_mask(dev->conf +
-				PCIE20_CAP_LINKCTRLSTATUS,
+				ep_link_ctrlstts_offset,
 				0, BIT(0));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 					readl_relaxed(dev->dm_core +
 					PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 					readl_relaxed(dev->conf +
-					PCIE20_CAP_LINKCTRLSTATUS);
+					ep_link_ctrlstts_offset);
 		}
 		pr_alert("PCIe: RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		pr_alert("PCIe: EP's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_CAP_LINKCTRLSTATUS));
+			ep_link_ctrlstts_offset));
 		break;
 	case 7: /* disable L1 */
 		pr_alert("\n\nPCIe: RC%d: disable L1\n\n",
@@ -1312,22 +1335,22 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 				PCIE20_CAP_LINKCTRLSTATUS,
 				BIT(1), 0);
 		msm_pcie_write_mask(dev->conf +
-				PCIE20_CAP_LINKCTRLSTATUS,
+				ep_link_ctrlstts_offset,
 				BIT(1), 0);
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 					readl_relaxed(dev->dm_core +
 					PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 					readl_relaxed(dev->conf +
-					PCIE20_CAP_LINKCTRLSTATUS);
+					ep_link_ctrlstts_offset);
 		}
 		pr_alert("PCIe: RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		pr_alert("PCIe: EP's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_CAP_LINKCTRLSTATUS));
+			ep_link_ctrlstts_offset));
 		break;
 	case 8: /* enable L1 */
 		pr_alert("\n\nPCIe: RC%d: enable L1\n\n",
@@ -1336,22 +1359,22 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 				PCIE20_CAP_LINKCTRLSTATUS,
 				0, BIT(1));
 		msm_pcie_write_mask(dev->conf +
-				PCIE20_CAP_LINKCTRLSTATUS,
+				ep_link_ctrlstts_offset,
 				0, BIT(1));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 					readl_relaxed(dev->dm_core +
 					PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 					readl_relaxed(dev->conf +
-					PCIE20_CAP_LINKCTRLSTATUS);
+					ep_link_ctrlstts_offset);
 		}
 		pr_alert("PCIe: RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		pr_alert("PCIe: EP's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_CAP_LINKCTRLSTATUS));
+			ep_link_ctrlstts_offset));
 		break;
 	case 9: /* disable L1ss */
 		pr_alert("\n\nPCIe: RC%d: disable L1ss\n\n",
@@ -1385,7 +1408,7 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 					ep_l1sub_ctrl1_offset,
 					0xf, 0);
 		msm_pcie_write_mask(dev->conf +
-					PCIE20_DEVICE_CONTROL2_STATUS2,
+					ep_dev_ctrl2stts2_offset,
 					BIT(10), 0);
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_L1SUB_CONTROL1 / 4] =
@@ -1397,9 +1420,9 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 			dev->ep_shadow[0][ep_l1sub_ctrl1_offset / 4] =
 				readl_relaxed(dev->conf +
 				ep_l1sub_ctrl1_offset);
-			dev->ep_shadow[0][PCIE20_DEVICE_CONTROL2_STATUS2 / 4] =
+			dev->ep_shadow[0][ep_dev_ctrl2stts2_offset / 4] =
 				readl_relaxed(dev->conf +
-				PCIE20_DEVICE_CONTROL2_STATUS2);
+				ep_dev_ctrl2stts2_offset);
 		}
 		pr_alert("PCIe: RC's L1SUB_CONTROL1:0x%x\n",
 			readl_relaxed(dev->dm_core +
@@ -1412,7 +1435,7 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 			ep_l1sub_ctrl1_offset));
 		pr_alert("PCIe: EP's DEVICE_CONTROL2_STATUS2:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_DEVICE_CONTROL2_STATUS2));
+			ep_dev_ctrl2stts2_offset));
 		break;
 	case 10: /* enable L1ss */
 		pr_alert("\n\nPCIe: RC%d: enable L1ss\n\n",
@@ -1455,7 +1478,7 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 					ep_l1sub_ctrl1_offset,
 					0xf, val);
 		msm_pcie_write_mask(dev->conf +
-					PCIE20_DEVICE_CONTROL2_STATUS2,
+					ep_dev_ctrl2stts2_offset,
 					0, BIT(10));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_L1SUB_CONTROL1 / 4] =
@@ -1467,9 +1490,9 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 			dev->ep_shadow[0][ep_l1sub_ctrl1_offset / 4] =
 				readl_relaxed(dev->conf +
 				ep_l1sub_ctrl1_offset);
-			dev->ep_shadow[0][PCIE20_DEVICE_CONTROL2_STATUS2 / 4] =
+			dev->ep_shadow[0][ep_dev_ctrl2stts2_offset / 4] =
 				readl_relaxed(dev->conf +
-				PCIE20_DEVICE_CONTROL2_STATUS2);
+				ep_dev_ctrl2stts2_offset);
 		}
 		pr_alert("PCIe: RC's L1SUB_CONTROL1:0x%x\n",
 			readl_relaxed(dev->dm_core +
@@ -1482,7 +1505,7 @@ static void msm_pcie_sel_debug_testcase(struct msm_pcie_dev_t *dev,
 			ep_l1sub_ctrl1_offset));
 		pr_alert("PCIe: EP's DEVICE_CONTROL2_STATUS2:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_DEVICE_CONTROL2_STATUS2));
+			ep_dev_ctrl2stts2_offset));
 		break;
 	case 11: /* enumerate PCIe  */
 		pr_alert("\n\nPCIe: attempting to enumerate RC%d\n\n",
@@ -2597,50 +2620,77 @@ static void msm_pcie_config_link_state(struct msm_pcie_dev_t *dev)
 	u32 current_offset;
 	u32 ep_l1sub_ctrl1_offset = 0;
 	u32 ep_l1sub_cap_reg1_offset = 0;
+	u32 ep_link_cap_offset = 0;
+	u32 ep_link_ctrlstts_offset = 0;
+	u32 ep_dev_ctrl2stts2_offset = 0;
 
 	/* Enable the AUX Clock and the Core Clk to be synchronous for L1SS*/
 	if (!dev->aux_clk_sync && dev->l1ss_supported)
 		msm_pcie_write_mask(dev->parf +
 				PCIE20_PARF_SYS_CTRL, BIT(3), 0);
 
+	current_offset = readl_relaxed(dev->conf + PCIE_CAP_PTR_OFFSET) & 0xff;
+
+	while (current_offset) {
+		val = readl_relaxed(dev->conf + current_offset);
+		if ((val & 0xff) == PCIE20_CAP_ID) {
+			ep_link_cap_offset = current_offset + 0x0c;
+			ep_link_ctrlstts_offset = current_offset + 0x10;
+			ep_dev_ctrl2stts2_offset = current_offset + 0x28;
+			break;
+		}
+		current_offset = (val >> 8) & 0xff;
+	}
+
+	if (!ep_link_cap_offset) {
+		PCIE_DBG(dev,
+			"RC%d endpoint does not support PCIe capability registers\n",
+			dev->rc_idx);
+		return;
+	} else {
+		PCIE_DBG(dev,
+			"RC%d: ep_link_cap_offset: 0x%x\n",
+			dev->rc_idx, ep_link_cap_offset);
+	}
+
 	if (dev->l0s_supported) {
 		msm_pcie_write_mask(dev->dm_core + PCIE20_CAP_LINKCTRLSTATUS,
 					0, BIT(0));
-		msm_pcie_write_mask(dev->conf + PCIE20_CAP_LINKCTRLSTATUS,
+		msm_pcie_write_mask(dev->conf + ep_link_ctrlstts_offset,
 					0, BIT(0));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 						readl_relaxed(dev->dm_core +
 						PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 						readl_relaxed(dev->conf +
-						PCIE20_CAP_LINKCTRLSTATUS);
+						ep_link_ctrlstts_offset);
 		}
 		PCIE_DBG2(dev, "RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		PCIE_DBG2(dev, "EP's CAP_LINKCTRLSTATUS:0x%x\n",
-			readl_relaxed(dev->conf + PCIE20_CAP_LINKCTRLSTATUS));
+			readl_relaxed(dev->conf + ep_link_ctrlstts_offset));
 	}
 
 	if (dev->l1_supported) {
 		msm_pcie_write_mask(dev->dm_core + PCIE20_CAP_LINKCTRLSTATUS,
 					0, BIT(1));
-		msm_pcie_write_mask(dev->conf + PCIE20_CAP_LINKCTRLSTATUS,
+		msm_pcie_write_mask(dev->conf + ep_link_ctrlstts_offset,
 					0, BIT(1));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_CAP_LINKCTRLSTATUS / 4] =
 						readl_relaxed(dev->conf +
 						PCIE20_CAP_LINKCTRLSTATUS);
-			dev->ep_shadow[0][PCIE20_CAP_LINKCTRLSTATUS / 4] =
+			dev->ep_shadow[0][ep_link_ctrlstts_offset / 4] =
 						readl_relaxed(dev->conf +
-						PCIE20_CAP_LINKCTRLSTATUS);
+						ep_link_ctrlstts_offset);
 		}
 		PCIE_DBG2(dev, "RC's CAP_LINKCTRLSTATUS:0x%x\n",
 			readl_relaxed(dev->dm_core +
 			PCIE20_CAP_LINKCTRLSTATUS));
 		PCIE_DBG2(dev, "EP's CAP_LINKCTRLSTATUS:0x%x\n",
-			readl_relaxed(dev->conf + PCIE20_CAP_LINKCTRLSTATUS));
+			readl_relaxed(dev->conf + ep_link_ctrlstts_offset));
 	}
 
 	if (dev->l1ss_supported) {
@@ -2676,7 +2726,7 @@ static void msm_pcie_config_link_state(struct msm_pcie_dev_t *dev)
 					0, BIT(10));
 		msm_pcie_write_reg_field(dev->conf, ep_l1sub_ctrl1_offset,
 					0xf, val);
-		msm_pcie_write_mask(dev->conf + PCIE20_DEVICE_CONTROL2_STATUS2,
+		msm_pcie_write_mask(dev->conf + ep_dev_ctrl2stts2_offset,
 					0, BIT(10));
 		if (dev->shadow_en) {
 			dev->rc_shadow[PCIE20_L1SUB_CONTROL1 / 4] =
@@ -2688,9 +2738,9 @@ static void msm_pcie_config_link_state(struct msm_pcie_dev_t *dev)
 			dev->ep_shadow[0][ep_l1sub_ctrl1_offset / 4] =
 					readl_relaxed(dev->conf +
 					ep_l1sub_ctrl1_offset);
-			dev->ep_shadow[0][PCIE20_DEVICE_CONTROL2_STATUS2 / 4] =
+			dev->ep_shadow[0][ep_dev_ctrl2stts2_offset / 4] =
 					readl_relaxed(dev->conf +
-					PCIE20_DEVICE_CONTROL2_STATUS2);
+					ep_dev_ctrl2stts2_offset);
 		}
 		PCIE_DBG2(dev, "RC's L1SUB_CONTROL1:0x%x\n",
 			readl_relaxed(dev->dm_core + PCIE20_L1SUB_CONTROL1));
@@ -2701,7 +2751,7 @@ static void msm_pcie_config_link_state(struct msm_pcie_dev_t *dev)
 			readl_relaxed(dev->conf + ep_l1sub_ctrl1_offset));
 		PCIE_DBG2(dev, "EP's DEVICE_CONTROL2_STATUS2:0x%x\n",
 			readl_relaxed(dev->conf +
-			PCIE20_DEVICE_CONTROL2_STATUS2));
+			ep_dev_ctrl2stts2_offset));
 	}
 }