pci: add capability helpers

Add a way to iterate over all capabilities in a config space. Add a search
function for getting a specific capability.

Reviewed-by: Punit Agrawal <punit.agrawal@arm.com>
Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Will Deacon <will.deacon@arm.com>

2 files changed, 27 insertions, 0 deletions

diff --git a/include/kvm/pci.h b/include/kvm/pci.h
index 5d9c0f3b..01c244bc 100644
--- a/include/kvm/pci.h
+++ b/include/kvm/pci.h
@@ -58,6 +58,11 @@ struct msix_cap {
 	u32 pba_offset;
 };
 
+struct pci_cap_hdr {
+	u8	type;
+	u8	next;
+};
+
 #define PCI_BAR_OFFSET(b)	(offsetof(struct pci_device_header, bar[b]))
 #define PCI_DEV_CFG_SIZE	256
 #define PCI_DEV_CFG_MASK	(PCI_DEV_CFG_SIZE - 1)
@@ -113,6 +118,13 @@ struct pci_device_header {
 	enum irq_type	irq_type;
 };
 
+#define PCI_CAP(pci_hdr, pos) ((void *)(pci_hdr) + (pos))
+
+#define pci_for_each_cap(pos, cap, hdr)				\
+	for ((pos) = (hdr)->capabilities & ~3;			\
+	     (cap) = PCI_CAP(hdr, pos), (pos) != 0;		\
+	     (pos) = ((struct pci_cap_hdr *)(cap))->next & ~3)
+
 int pci__init(struct kvm *kvm);
 int pci__exit(struct kvm *kvm);
 struct pci_device_header *pci__find_dev(u8 dev_num);
@@ -121,4 +133,6 @@ void pci__assign_irq(struct device_header *dev_hdr);
 void pci__config_wr(struct kvm *kvm, union pci_config_address addr, void *data, int size);
 void pci__config_rd(struct kvm *kvm, union pci_config_address addr, void *data, int size);
 
+void *pci_find_cap(struct pci_device_header *hdr, u8 cap_type);
+
 #endif /* KVM__PCI_H */
diff --git a/pci.c b/pci.c
index 5a8c2ef4..689869cb 100644
--- a/pci.c
+++ b/pci.c
@@ -27,6 +27,19 @@ u32 pci_get_io_space_block(u32 size)
 	return block;
 }
 
+void *pci_find_cap(struct pci_device_header *hdr, u8 cap_type)
+{
+	u8 pos;
+	struct pci_cap_hdr *cap;
+
+	pci_for_each_cap(pos, cap, hdr) {
+		if (cap->type == cap_type)
+			return cap;
+	}
+
+	return NULL;
+}
+
 void pci__assign_irq(struct device_header *dev_hdr)
 {
 	struct pci_device_header *pci_hdr = dev_hdr->data;