[PATCH v1 3/3] vpci/msi: Remove registers when init_msi() fails

[Jiqian Chen <Jiqian.Chen@xxxxxxx>]

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

To do that, use a vpci_register array to record all MSI
registers and call vpci_remove_register() to remove registers.

Signed-off-by: Jiqian Chen <Jiqian.Chen@xxxxxxx>
---
 xen/drivers/vpci/msi.c | 57 +++++++++++++++++++++++++++++-------------
 1 file changed, 39 insertions(+), 18 deletions(-)

diff --git a/xen/drivers/vpci/msi.c b/xen/drivers/vpci/msi.c
index 9d7a9fd8dba1..30ef97efb7b0 100644
--- a/xen/drivers/vpci/msi.c
+++ b/xen/drivers/vpci/msi.c
@@ -195,6 +195,9 @@ static void cf_check mask_write(
 
 static int cf_check init_msi(struct pci_dev *pdev)
 {
+    unsigned int offset;
+    unsigned int reg_index = 0;
+    struct vpci_register registers[VPCI_CAP_MAX_REGISTER];
     unsigned int pos = pdev->msi_pos;
     uint16_t control;
     int ret;
@@ -206,15 +209,13 @@ static int cf_check init_msi(struct pci_dev *pdev)
     if ( !pdev->vpci->msi )
         return -ENOMEM;
 
+    offset = msi_control_reg(pos);
     ret = vpci_add_register(pdev->vpci, control_read, control_write,
-                            msi_control_reg(pos), 2, pdev->vpci->msi);
+                            offset, 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;
+    registers[reg_index].offset = offset;
+    registers[reg_index++].size = 2;
 
     /* Get the maximum number of vectors the device supports. */
     control = pci_conf_read16(pdev->sbdf, msi_control_reg(pos));
@@ -234,33 +235,42 @@ static int cf_check init_msi(struct pci_dev *pdev)
     pdev->vpci->msi->address64 = is_64bit_address(control);
     pdev->vpci->msi->masking = is_mask_bit_support(control);
 
+    offset = msi_lower_address_reg(pos);
     ret = vpci_add_register(pdev->vpci, address_read, address_write,
-                            msi_lower_address_reg(pos), 4, pdev->vpci->msi);
+                            offset, 4, pdev->vpci->msi);
     if ( ret )
-        return ret;
+        goto fail;
+    registers[reg_index].offset = offset;
+    registers[reg_index++].size = 4;
 
+    offset = msi_data_reg(pos, pdev->vpci->msi->address64);
     ret = vpci_add_register(pdev->vpci, data_read, data_write,
-                            msi_data_reg(pos, pdev->vpci->msi->address64), 2,
-                            pdev->vpci->msi);
+                            offset, 2, pdev->vpci->msi);
     if ( ret )
-        return ret;
+        goto fail;
+    registers[reg_index].offset = offset;
+    registers[reg_index++].size = 2;
 
     if ( pdev->vpci->msi->address64 )
     {
+        offset = msi_upper_address_reg(pos);
         ret = vpci_add_register(pdev->vpci, address_hi_read, address_hi_write,
-                                msi_upper_address_reg(pos), 4, 
pdev->vpci->msi);
+                                offset, 4, pdev->vpci->msi);
         if ( ret )
-            return ret;
+            goto fail;
+        registers[reg_index].offset = offset;
+        registers[reg_index++].size = 4;
     }
 
     if ( pdev->vpci->msi->masking )
     {
+        offset = msi_mask_bits_reg(pos, pdev->vpci->msi->address64);
         ret = vpci_add_register(pdev->vpci, mask_read, mask_write,
-                                msi_mask_bits_reg(pos,
-                                                  pdev->vpci->msi->address64),
-                                4, pdev->vpci->msi);
+                                offset, 4, pdev->vpci->msi);
         if ( ret )
-            return ret;
+            goto fail;
+        registers[reg_index].offset = offset;
+        registers[reg_index++].size = 4;
         /*
          * 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 +279,17 @@ static int cf_check init_msi(struct pci_dev *pdev)
     }
 
     return 0;
+
+ fail:
+    for ( unsigned int i = 0; i < reg_index; i++ )
+        if ( vpci_remove_register(pdev->vpci,
+                                  registers[i].offset,
+                                  registers[i].size) )
+            continue;
+    xfree(pdev->vpci->msi);
+    pdev->vpci->msi = NULL;
+
+    return ret;
 }
 REGISTER_VPCI_LEGACY_CAP(PCI_CAP_ID_MSI, init_msi, VPCI_PRIORITY_LOW);
 
-- 
2.34.1