From ea0b4d4093cf0e87a31372c37dfda2e509a01b7a Mon Sep 17 00:00:00 2001
From: Hemant Kumar <hemantk@codeaurora.org>
Date: Thu, 17 Sep 2020 20:06:49 -0700
Subject: [PATCH] pci/of: expand match PCI devices to OF nodes

Expand match criteria so that multiple OF nodes with
the same BDF will match with the correct PCI device.
OF node will have an optional compatible entry with
device and vendor ID, which will be matched to a PCI
device and vendor ID.

Change-Id: I6e654d5dbf55a05b4e1b6a60d5d87addc2cb14b9
Signed-off-by: Tony Truong <truong@codeaurora.org>
Signed-off-by: Hemant Kumar <hemantk@codeaurora.org>
---
 drivers/pci/of.c       | 49 ++++++++++++++++++++++++++++++++++++++++++
 include/linux/of_pci.h | 11 ++++++++++
 2 files changed, 60 insertions(+)

diff --git a/drivers/pci/of.c b/drivers/pci/of.c
index 36891e7deee3..2578db1f7e4c 100644
--- a/drivers/pci/of.c
+++ b/drivers/pci/of.c
@@ -20,8 +20,12 @@ void pci_set_of_node(struct pci_dev *dev)
 {
 	if (!dev->bus->dev.of_node)
 		return;
+#ifdef CONFIG_PCI_QTI
+	dev->dev.of_node = of_pci_find_child_device(dev);
+#else
 	dev->dev.of_node = of_pci_find_child_device(dev->bus->dev.of_node,
 						    dev->devfn);
+#endif
 	if (dev->dev.of_node)
 		dev->dev.fwnode = &dev->dev.of_node->fwnode;
 }
@@ -115,6 +119,50 @@ static inline int __of_pci_pci_compare(struct device_node *node,
 	return devfn == data;
 }
 
+#ifdef CONFIG_PCI_QTI
+static inline bool __of_pci_pci_compare_id(struct device_node *node,
+					   struct pci_dev *dev)
+{
+	char dev_id_str[10];
+	int index;
+
+	scnprintf(dev_id_str, sizeof(dev_id_str), "%04x:%04x", dev->vendor,
+			dev->device);
+
+	index = of_property_match_string(node, "pci-ids", dev_id_str);
+	if (index < 0 && index != -EINVAL)
+		return false;
+
+	return true;
+}
+
+struct device_node *of_pci_find_child_device(struct pci_dev *dev)
+{
+	struct device_node *node, *node2;
+	struct device_node *parent = dev->bus->dev.of_node;
+	unsigned int devfn = dev->devfn;
+
+	for_each_child_of_node(parent, node) {
+		if (__of_pci_pci_compare(node, devfn))
+			if (__of_pci_pci_compare_id(node, dev))
+				return node;
+		/*
+		 * Some OFs create a parent node "multifunc-device" as
+		 * a fake root for all functions of a multi-function
+		 * device we go down them as well.
+		 */
+		if (of_node_name_eq(node, "multifunc-device")) {
+			for_each_child_of_node(node, node2) {
+				if (__of_pci_pci_compare(node2, devfn)) {
+					of_node_put(node);
+					return node2;
+				}
+			}
+		}
+	}
+	return NULL;
+}
+#else
 struct device_node *of_pci_find_child_device(struct device_node *parent,
 					     unsigned int devfn)
 {
@@ -139,6 +187,7 @@ struct device_node *of_pci_find_child_device(struct device_node *parent,
 	}
 	return NULL;
 }
+#endif
 EXPORT_SYMBOL_GPL(of_pci_find_child_device);
 
 /**
diff --git a/include/linux/of_pci.h b/include/linux/of_pci.h
index 21a89c4880fa..3ccdd246f1d5 100644
--- a/include/linux/of_pci.h
+++ b/include/linux/of_pci.h
@@ -10,16 +10,27 @@ struct of_phandle_args;
 struct device_node;
 
 #if IS_ENABLED(CONFIG_OF) && IS_ENABLED(CONFIG_PCI)
+#ifdef CONFIG_PCI_QTI
+struct device_node *of_pci_find_child_device(struct pci_dev *dev);
+#else
 struct device_node *of_pci_find_child_device(struct device_node *parent,
 					     unsigned int devfn);
+#endif
 int of_pci_get_devfn(struct device_node *np);
 void of_pci_check_probe_only(void);
 #else
+#ifdef CONFIG_PCI_QTI
+static inline struct device_node *of_pci_find_child_device(struct pci_dev *dev)
+{
+	return NULL;
+}
+#else
 static inline struct device_node *of_pci_find_child_device(struct device_node *parent,
 					     unsigned int devfn)
 {
 	return NULL;
 }
+#endif
 
 static inline int of_pci_get_devfn(struct device_node *np)
 {