[PATCH v3 07/11] vpci: Hide extended capability when it fails to initialize

[Jiqian Chen <Jiqian.Chen@xxxxxxx>]

When vpci fails to initialize a extended capability of device for dom0,
it just return error instead of catching and processing exception. That
makes the entire device unusable.

So, add new a function to hide extended capability when initialization
fails. And remove the failed extended capability handler from vpci
extended capability list.

Signed-off-by: Jiqian Chen <Jiqian.Chen@xxxxxxx>
---
cc: "Roger Pau Monné" <roger.pau@xxxxxxxxxx>
---
v2->v3 changes:
* Separated from the last version patch "vpci: Hide capability when it fails to 
initialize".
* Whole implementation changed because last version is wrong.
  This version gets target handler and previous handler from vpci->handlers, 
then remove the target.
* Note: a case in function vpci_ext_capability_mask() needs to be discussed,
  because it may change the offset of next capability when the offset of target
  capability is 0x100U(the first extended capability), my implementation is 
just to
  ignore and let hardware to handle the target capability.

v1->v2 changes:
* Removed the "priorities" of initializing capabilities since it isn't used 
anymore.
* Added new function vpci_capability_mask() and vpci_ext_capability_mask() to
  remove failed capability from list.
* Called vpci_make_msix_hole() in the end of init_msix().

Best regards,
Jiqian Chen.
---
 xen/drivers/vpci/vpci.c    | 79 ++++++++++++++++++++++++++++++++++++++
 xen/include/xen/pci_regs.h |  1 +
 2 files changed, 80 insertions(+)

diff --git a/xen/drivers/vpci/vpci.c b/xen/drivers/vpci/vpci.c
index f97c7cc460a0..8ff5169bdd18 100644
--- a/xen/drivers/vpci/vpci.c
+++ b/xen/drivers/vpci/vpci.c
@@ -183,6 +183,83 @@ static void vpci_capability_mask(struct pci_dev *pdev,
     xfree(next_r);
 }
 
+static struct vpci_register *vpci_get_previous_ext_cap_register
+                (struct vpci *vpci, const unsigned int offset)
+{
+    uint32_t header;
+    unsigned int pos = PCI_CFG_SPACE_SIZE;
+    struct vpci_register *r;
+
+    if ( offset <= PCI_CFG_SPACE_SIZE )
+        return NULL;
+
+    r = vpci_get_register(vpci, pos, 4);
+    ASSERT(r);
+
+    header = (uint32_t)(uintptr_t)r->private;
+    pos = PCI_EXT_CAP_NEXT(header);
+    while ( pos > PCI_CFG_SPACE_SIZE && pos != offset )
+    {
+        r = vpci_get_register(vpci, pos, 4);
+        ASSERT(r);
+        header = (uint32_t)(uintptr_t)r->private;
+        pos = PCI_EXT_CAP_NEXT(header);
+    }
+
+    if ( pos <= PCI_CFG_SPACE_SIZE )
+        return NULL;
+
+    return r;
+}
+
+static void vpci_ext_capability_mask(struct pci_dev *pdev,
+                                     const unsigned int cap)
+{
+    const unsigned int offset = pci_find_ext_capability(pdev->sbdf, cap);
+    struct vpci_register *rm, *prev_r;
+    struct vpci *vpci = pdev->vpci;
+    uint32_t header, pre_header;
+
+    spin_lock(&vpci->lock);
+    rm = vpci_get_register(vpci, offset, 4);
+    if ( !rm )
+    {
+        spin_unlock(&vpci->lock);
+        return;
+    }
+
+    header = (uint32_t)(uintptr_t)rm->private;
+    if ( offset == PCI_CFG_SPACE_SIZE )
+    {
+        if ( PCI_EXT_CAP_NEXT(header) <= PCI_CFG_SPACE_SIZE )
+            rm->private = (void *)(uintptr_t)0;
+        else
+            /*
+             * Else case needs to remove the capability in position 0x100U and
+             * moves the next capability to be in position 0x100U, that would
+             * cause the offset of next capability in vpci different from the
+             * hardware, then cause error accesses, so just ignore it here and
+             * hope hardware would handle the capability well.
+            */
+            printk(XENLOG_ERR "%pd %pp: ext cap %u is first cap, can't mask 
it\n",
+                   pdev->domain, &pdev->sbdf, cap);
+        spin_unlock(&vpci->lock);
+        return;
+    }
+
+    prev_r = vpci_get_previous_ext_cap_register(vpci, offset);
+    ASSERT(prev_r);
+
+    pre_header = (uint32_t)(uintptr_t)prev_r->private;
+    prev_r->private = (void *)(uintptr_t)((pre_header &
+                                           ~PCI_EXT_CAP_NEXT_MASK) |
+                                          (header & PCI_EXT_CAP_NEXT_MASK));
+
+    list_del(&rm->node);
+    spin_unlock(&vpci->lock);
+    xfree(rm);
+}
+
 static void vpci_init_capabilities(struct pci_dev *pdev)
 {
     for ( unsigned int i = 0; i < NUM_VPCI_INIT; i++ )
@@ -216,6 +293,8 @@ static void vpci_init_capabilities(struct pci_dev *pdev)
                    is_ext ? "extended" : "legacy", cap, rc);
             if ( !is_ext )
                 vpci_capability_mask(pdev, cap);
+            else
+                vpci_ext_capability_mask(pdev, cap);
         }
     }
 }
diff --git a/xen/include/xen/pci_regs.h b/xen/include/xen/pci_regs.h
index 27b4f44eedf3..5fe6653fded4 100644
--- a/xen/include/xen/pci_regs.h
+++ b/xen/include/xen/pci_regs.h
@@ -449,6 +449,7 @@
 #define PCI_EXT_CAP_ID(header)         ((header) & 0x0000ffff)
 #define PCI_EXT_CAP_VER(header)                (((header) >> 16) & 0xf)
 #define PCI_EXT_CAP_NEXT(header)       (((header) >> 20) & 0xffc)
+#define PCI_EXT_CAP_NEXT_MASK          0xFFC00000U
 
 #define PCI_EXT_CAP_ID_ERR     1
 #define PCI_EXT_CAP_ID_VC      2
-- 
2.34.1