[PATCH v2 7/8] vpci/msi: Free MSI resources when init_msi() fails

[Jiqian Chen <Jiqian.Chen@xxxxxxx>]

When init_msi() fails, the previous new changes will hide MSI
capability, it can't rely on vpci_deassign_device() to remove
all MSI related resources anymore, those resources must be
cleaned up in failure path of init_msi.

To do that, add a new function to free MSI resources.

Signed-off-by: Jiqian Chen <Jiqian.Chen@xxxxxxx>
---
cc: "Roger Pau Monné" <roger.pau@xxxxxxxxxx>
---
v1->v2 changes:
* Added a new function fini_msi to free all MSI resources instead of using an 
array to record registered registers.
---
 xen/drivers/vpci/msi.c | 47 +++++++++++++++++++++++++++++++++---------
 1 file changed, 37 insertions(+), 10 deletions(-)

diff --git a/xen/drivers/vpci/msi.c b/xen/drivers/vpci/msi.c
index ca89ae9b9c22..48a466dba0ef 100644
--- a/xen/drivers/vpci/msi.c
+++ b/xen/drivers/vpci/msi.c
@@ -193,6 +193,33 @@ static void cf_check mask_write(
     msi->mask = val;
 }
 
+/* 12 is size of MSI structure for 32-bit Message Address without PVM */
+#define MSI_STRUCTURE_SIZE_32 12
+
+static void fini_msi(struct pci_dev *pdev)
+{
+    unsigned int size = MSI_STRUCTURE_SIZE_32;
+
+    if ( !pdev->vpci->msi )
+        return;
+
+    if ( pdev->vpci->msi->address64 )
+        size += 4;
+    if ( pdev->vpci->msi->masking )
+        size += 4;
+
+    /*
+     * Remove all possible registered registers except capability ID
+     * register and next capability pointer register, which will be
+     * removed in mask function.
+     */
+    vpci_remove_registers(pdev->vpci,
+                          msi_control_reg(pdev->msi_pos),
+                          size - PCI_MSI_FLAGS);
+    xfree(pdev->vpci->msi);
+    pdev->vpci->msi = NULL;
+}
+
 static int cf_check init_msi(struct pci_dev *pdev)
 {
     unsigned int pos = pdev->msi_pos;
@@ -209,12 +236,7 @@ static int cf_check init_msi(struct pci_dev *pdev)
     ret = vpci_add_register(pdev->vpci, control_read, control_write,
                             msi_control_reg(pos), 2, pdev->vpci->msi);
     if ( ret )
-        /*
-         * NB: there's no need to free the msi struct or remove the register
-         * handlers form the config space, the caller will take care of the
-         * cleanup.
-         */
-        return ret;
+        goto fail;
 
     /* Get the maximum number of vectors the device supports. */
     control = pci_conf_read16(pdev->sbdf, msi_control_reg(pos));
@@ -237,20 +259,20 @@ static int cf_check init_msi(struct pci_dev *pdev)
     ret = vpci_add_register(pdev->vpci, address_read, address_write,
                             msi_lower_address_reg(pos), 4, pdev->vpci->msi);
     if ( ret )
-        return ret;
+        goto fail;
 
     ret = vpci_add_register(pdev->vpci, data_read, data_write,
                             msi_data_reg(pos, pdev->vpci->msi->address64), 2,
                             pdev->vpci->msi);
     if ( ret )
-        return ret;
+        goto fail;
 
     if ( pdev->vpci->msi->address64 )
     {
         ret = vpci_add_register(pdev->vpci, address_hi_read, address_hi_write,
                                 msi_upper_address_reg(pos), 4, 
pdev->vpci->msi);
         if ( ret )
-            return ret;
+            goto fail;
     }
 
     if ( pdev->vpci->msi->masking )
@@ -260,7 +282,7 @@ static int cf_check init_msi(struct pci_dev *pdev)
                                                   pdev->vpci->msi->address64),
                                 4, pdev->vpci->msi);
         if ( ret )
-            return ret;
+            goto fail;
         /*
          * FIXME: do not add any handler for the pending bits for the hardware
          * domain, which means direct access. This will be revisited when
@@ -269,6 +291,11 @@ static int cf_check init_msi(struct pci_dev *pdev)
     }
 
     return 0;
+
+ fail:
+    fini_msi(pdev);
+
+    return ret;
 }
 REGISTER_VPCI_LEGACY_CAP(PCI_CAP_ID_MSI, init_msi);
 
-- 
2.34.1