1 files changed, 29 insertions, 2 deletions

diff --git a/drivers/pci/pcie/ptm.c b/drivers/pci/pcie/ptm.c
index a14ac94b96dc..bab8ac63c4f3 100644
--- a/drivers/pci/pcie/ptm.c
+++ b/drivers/pci/pcie/ptm.c
@@ -19,13 +19,29 @@
 
 static void pci_ptm_info(struct pci_dev *dev)
 {
-	dev_info(&dev->dev, "PTM enabled%s\n", dev->ptm_root ? " (root)" : "");
+	char clock_desc[8];
+
+	switch (dev->ptm_granularity) {
+	case 0:
+		snprintf(clock_desc, sizeof(clock_desc), "unknown");
+		break;
+	case 255:
+		snprintf(clock_desc, sizeof(clock_desc), ">254ns");
+		break;
+	default:
+		snprintf(clock_desc, sizeof(clock_desc), "%udns",
+			 dev->ptm_granularity);
+		break;
+	}
+	dev_info(&dev->dev, "PTM enabled%s, %s granularity\n",
+		 dev->ptm_root ? " (root)" : "", clock_desc);
 }
 
 void pci_ptm_init(struct pci_dev *dev)
 {
 	int pos;
 	u32 cap, ctrl;
+	u8 local_clock;
 	struct pci_dev *ups;
 
 	if (!pci_is_pcie(dev))
@@ -45,6 +61,7 @@ void pci_ptm_init(struct pci_dev *dev)
 		return;
 
 	pci_read_config_dword(dev, pos + PCI_PTM_CAP, &cap);
+	local_clock = (cap & PCI_PTM_GRANULARITY_MASK) >> 8;
 
 	/*
 	 * There's no point in enabling PTM unless it's enabled in the
@@ -55,14 +72,20 @@ void pci_ptm_init(struct pci_dev *dev)
 	ups = pci_upstream_bridge(dev);
 	if (ups && ups->ptm_enabled) {
 		ctrl = PCI_PTM_CTRL_ENABLE;
+		if (ups->ptm_granularity == 0)
+			dev->ptm_granularity = 0;
+		else if (ups->ptm_granularity > local_clock)
+			dev->ptm_granularity = ups->ptm_granularity;
 	} else {
 		if (cap & PCI_PTM_CAP_ROOT) {
 			ctrl = PCI_PTM_CTRL_ENABLE | PCI_PTM_CTRL_ROOT;
 			dev->ptm_root = 1;
+			dev->ptm_granularity = local_clock;
 		} else
 			return;
 	}
 
+	ctrl |= dev->ptm_granularity << 8;
 	pci_write_config_dword(dev, pos + PCI_PTM_CTRL, ctrl);
 	dev->ptm_enabled = 1;
 
@@ -98,18 +121,22 @@ int pci_enable_ptm(struct pci_dev *dev, u8 *granularity)
 		ups = pci_upstream_bridge(dev);
 		if (!ups || !ups->ptm_enabled)
 			return -EINVAL;
+
+		dev->ptm_granularity = ups->ptm_granularity;
 	} else if (pci_pcie_type(dev) == PCI_EXP_TYPE_RC_END) {
+		dev->ptm_granularity = 0;
 	} else
 		return -EINVAL;
 
 	ctrl = PCI_PTM_CTRL_ENABLE;
+	ctrl |= dev->ptm_granularity << 8;
 	pci_write_config_dword(dev, pos + PCI_PTM_CTRL, ctrl);
 	dev->ptm_enabled = 1;
 
 	pci_ptm_info(dev);
 
 	if (granularity)
-		*granularity = 0;
+		*granularity = dev->ptm_granularity;
 	return 0;
 }
 EXPORT_SYMBOL(pci_enable_ptm);