From f0729cd91d0e1574ecf07341a98a85596d25cfb2 Mon Sep 17 00:00:00 2001 From: Jian Jun Chen Date: Fri, 18 May 2018 13:11:44 +0800 Subject: [PATCH] dm: add pci_emul_find_capability pci_emul_find_capability can be used to get the offset of a PCI capability in PCI configuration space. Signed-off-by: Jian Jun Chen Reviewed-by: Hao Li Reviewed-by: Zhao Yakui Acked-by: Kevin Tian --- devicemodel/hw/pci/core.c | 39 ++++++++++++++++++++++++++++++++++ devicemodel/include/pci_core.h | 2 ++ 2 files changed, 41 insertions(+) diff --git a/devicemodel/hw/pci/core.c b/devicemodel/hw/pci/core.c index 3b3287d05..49e8da80e 100644 --- a/devicemodel/hw/pci/core.c +++ b/devicemodel/hw/pci/core.c @@ -724,6 +724,45 @@ pci_emul_add_capability(struct pci_vdev *dev, u_char *capdata, int caplen) return 0; } +/* + * p_capoff is used as both input and output. Set *p_capoff to 0 when this + * function is called for the first time, it will return offset of the first + * matched one in p_capoff. To find the next matched one, please use the + * returned *p_capoff from last call as the input, in this case the offset of + * the next matched one will be returned in *p_capoff. + * Please check the returned value first before touch p_capoff. + */ +int +pci_emul_find_capability(struct pci_vdev *dev, uint8_t capid, int *p_capoff) +{ + int coff; + uint16_t sts; + + sts = pci_get_cfgdata16(dev, PCIR_STATUS); + if ((sts & PCIM_STATUS_CAPPRESENT) == 0) + return -1; + + if (!p_capoff) + return -1; + + if (*p_capoff == 0) + coff = pci_get_cfgdata8(dev, PCIR_CAP_PTR); + else if (*p_capoff >= CAP_START_OFFSET && *p_capoff <= dev->prevcap) + coff = pci_get_cfgdata8(dev, *p_capoff + 1); + else + return -1; + + while (coff >= CAP_START_OFFSET && coff <= dev->prevcap) { + if (pci_get_cfgdata8(dev, coff) == capid) { + *p_capoff = coff; + return 0; + } + coff = pci_get_cfgdata8(dev, coff + 1); + } + + return -1; +} + static struct pci_vdev_ops * pci_emul_finddev(char *name) { diff --git a/devicemodel/include/pci_core.h b/devicemodel/include/pci_core.h index 3d8088e16..88f5c0c21 100644 --- a/devicemodel/include/pci_core.h +++ b/devicemodel/include/pci_core.h @@ -233,6 +233,8 @@ int pci_emul_alloc_pbar(struct pci_vdev *pdi, int idx, void pci_emul_free_bars(struct pci_vdev *pdi); int pci_emul_add_capability(struct pci_vdev *dev, u_char *capdata, int caplen); +int pci_emul_find_capability(struct pci_vdev *dev, uint8_t capid, + int *p_capoff); int pci_emul_add_msicap(struct pci_vdev *pi, int msgnum); int pci_emul_add_pciecap(struct pci_vdev *pi, int pcie_device_type); void pci_generate_msi(struct pci_vdev *pi, int msgnum);