diff --git a/xen/drivers/vpci/vpci.c b/xen/drivers/vpci/vpci.c index 7778acee0d..da67226e4f 100644 --- a/xen/drivers/vpci/vpci.c +++ b/xen/drivers/vpci/vpci.c @@ -83,6 +83,88 @@ static int assign_virtual_sbdf(struct pci_dev *pdev) #endif /* CONFIG_HAS_VPCI_GUEST_SUPPORT */ +static struct vpci_register *vpci_get_register(const struct vpci *vpci, + unsigned int offset, + unsigned int size) +{ + struct vpci_register *r; + + ASSERT(spin_is_locked(&vpci->lock)); + + list_for_each_entry ( r, &vpci->handlers, node ) + { + if ( r->offset == offset && r->size == size ) + return r; + + if ( offset <= r->offset ) + break; + } + + return NULL; +} + +static struct vpci_register *vpci_get_previous_cap_register( + const struct vpci *vpci, unsigned int offset) +{ + unsigned int next; + struct vpci_register *r; + + if ( offset < 0x40 ) + { + ASSERT_UNREACHABLE(); + return NULL; + } + + for ( r = vpci_get_register(vpci, PCI_CAPABILITY_LIST, 1); r; + r = next >= 0x40 ? vpci_get_register(vpci, + next + PCI_CAP_LIST_NEXT, 1) + : NULL ) + { + next = (unsigned int)(uintptr_t)r->private; + ASSERT(next == (uintptr_t)r->private); + if ( next == offset ) + break; + } + + return r; +} + +static int vpci_capability_hide(const struct pci_dev *pdev, unsigned int cap) +{ + const unsigned int offset = pci_find_cap_offset(pdev->sbdf, cap); + struct vpci_register *prev_r, *next_r; + struct vpci *vpci = pdev->vpci; + + if ( !offset ) + { + ASSERT_UNREACHABLE(); + return 0; + } + + spin_lock(&vpci->lock); + prev_r = vpci_get_previous_cap_register(vpci, offset); + next_r = vpci_get_register(vpci, offset + PCI_CAP_LIST_NEXT, 1); + if ( !prev_r || !next_r ) + { + spin_unlock(&vpci->lock); + return -ENODEV; + } + + prev_r->private = next_r->private; + /* + * Not calling vpci_remove_register() here is to avoid redoing + * the register search. + */ + list_del(&next_r->node); + spin_unlock(&vpci->lock); + xfree(next_r); + + if ( !is_hardware_domain(pdev->domain) ) + return vpci_remove_register(vpci, offset + PCI_CAP_LIST_ID, 1); + + return 0; +} + static int vpci_init_capabilities(struct pci_dev *pdev) { for ( unsigned int i = 0; i < NUM_VPCI_INIT; i++ ) @@ -103,7 +185,34 @@ static int vpci_init_capabilities(struct pci_dev *pdev) rc = capability->init(pdev); if ( rc ) - return rc; + { + const char *type = is_ext ? "extended" : "legacy"; + + printk(XENLOG_WARNING + "%pd %pp: init %s cap %u fail rc=%d, mask it\n", + pdev->domain, &pdev->sbdf, type, cap, rc); + + if ( capability->cleanup ) + { + rc = capability->cleanup(pdev); + if ( rc ) + { + printk(XENLOG_ERR "%pd %pp: clean %s cap %u fail rc=%d\n", + pdev->domain, &pdev->sbdf, type, cap, rc); + if ( !is_hardware_domain(pdev->domain) ) + return rc; + } + } + + if ( !is_ext ) + rc = vpci_capability_hide(pdev, cap); + if ( rc ) + { + printk(XENLOG_ERR "%pd %pp: hide %s cap %u fail rc=%d\n", + pdev->domain, &pdev->sbdf, type, cap, rc); + return rc; + } + } } return 0;