[PATCH 24/24] iommu/amd: Relocate vIOMMU translate-device-id on PCI reserve

When PCI attach reserves a BDF already allocated to a vIOMMU, relocate
that vIOMMU to a newly allocated translate-device-id before marking the
BDF reserved for the device.  Reprogram translation DTE and VFctrl under
trans_devid_lock; roll back the pool on failure.

Signed-off-by: Suravee Suthikulpanit <[email protected]>
---
 drivers/iommu/amd/trans_devid.c | 116 +++++++++++++++++++++++++++++++-
 1 file changed, 115 insertions(+), 1 deletion(-)

diff --git a/drivers/iommu/amd/trans_devid.c b/drivers/iommu/amd/trans_devid.c
index 9d7d93c5dcdb..6fb1ba3c9444 100644
--- a/drivers/iommu/amd/trans_devid.c
+++ b/drivers/iommu/amd/trans_devid.c
@@ -82,16 +82,120 @@ void amd_iommu_pci_seg_trans_devid_fini(struct amd_iommu_pci_seg *pci_seg)
 	xa_destroy(&pci_seg->trans_devid_xa);
 }
 
+/**
+ * trans_devid_do_relocate - move vIOMMU translation DTE from @old_id to @new_id
+ *
+ * Caller holds @aviommu->trans_devid_lock.  Pool xarray already records @new_id
+ * as allocated to @aviommu and @old_id as reserved.
+ */
+static int trans_devid_do_relocate(struct amd_iommu_viommu *aviommu,
+				   u16 old_id, u16 new_id)
+{
+	struct iommufd_viommu *viommu = &aviommu->core;
+	struct amd_iommu *iommu =
+		container_of(viommu->iommu_dev, struct amd_iommu, iommu);
+	int ret;
+
+	aviommu->trans_devid = new_id;
+
+	ret = amd_iommu_set_translate_dte(viommu);
+	if (ret)
+		goto err_restore_id;
+
+	amd_iommu_update_vfctrl_mmio_translate_devid(iommu, aviommu->gid, new_id);
+
+	if (search_dev_data(iommu, old_id))
+		amd_iommu_clear_translate_dte(iommu, old_id);
+
+	return 0;
+
+err_restore_id:
+	aviommu->trans_devid = old_id;
+	return ret;
+}
+
+/**
+ * trans_devid_relocate - move an allocated id to a new slot and reserve @from_id
+ *
+ * Called when PCI attach needs a BDF that a vIOMMU already owns.  Updates the
+ * per-segment pool, then reprograms DTE and VFctrl on the owning vIOMMU.
+ *
+ * Locking: takes @aviommu->trans_devid_lock, then pci_seg->trans_devid_mutex
+ * (same order as destroy).  Hardware steps run with the viommu lock held and
+ * the segment mutex dropped.
+ */
+static int trans_devid_relocate(struct amd_iommu_pci_seg *pci_seg, u16 from_id,
+				struct amd_iommu_viommu *aviommu)
+{
+	u16 new_id;
+	int ret;
+
+	mutex_lock(&aviommu->trans_devid_lock);
+
+	mutex_lock(&pci_seg->trans_devid_mutex);
+	if (trans_devid_xa_owner(xa_load(&pci_seg->trans_devid_xa, from_id)) !=
+	    aviommu) {
+		ret = -ENOENT;
+		goto unlock_seg;
+	}
+
+	if (aviommu->trans_devid != from_id) {
+		ret = -EINVAL;
+		goto unlock_seg;
+	}
+
+	new_id = trans_devid_find_free_locked(pci_seg);
+	if (new_id < 0) {
+		ret = new_id;
+		goto unlock_seg;
+	}
+
+	ret = trans_devid_xa_install_allocated_locked(pci_seg, new_id, aviommu);
+	if (ret)
+		goto unlock_seg;
+
+	ret = trans_devid_xa_install_reserved_locked(pci_seg, from_id);
+	if (ret) {
+		xa_erase(&pci_seg->trans_devid_xa, new_id);
+		goto unlock_seg;
+	}
+
+	mutex_unlock(&pci_seg->trans_devid_mutex);
+
+	ret = trans_devid_do_relocate(aviommu, from_id, new_id);
+	if (ret) {
+		mutex_lock(&pci_seg->trans_devid_mutex);
+		xa_erase(&pci_seg->trans_devid_xa, new_id);
+		trans_devid_xa_install_allocated_locked(pci_seg, from_id,
+							aviommu);
+		mutex_unlock(&pci_seg->trans_devid_mutex);
+	}
+
+	mutex_unlock(&aviommu->trans_devid_lock);
+	return ret;
+
+unlock_seg:
+	mutex_unlock(&pci_seg->trans_devid_mutex);
+	mutex_unlock(&aviommu->trans_devid_lock);
+	return ret;
+}
+
 /**
  * amd_iommu_trans_devid_reserve - occupy @id so it is never returned by alloc
  *
  * Reservation is done when attaching device to a domain (see amd_iommu_attach_device()).
  *
- * Return: 0 on success.  A second reserve of an already-reserved @id succeeds.
+ * When @id is allocated to a vIOMMU (e.g. after PCI hot-plug), the driver relocates
+ * that vIOMMU to a newly allocated translate-device-id and reserves @id for the PCI
+ * function.
+ *
+ * Return: 0 on success, %-ENOSPC if relocation cannot find a free id, or another
+ * errno from relocation.  A second reserve of an already-reserved @id succeeds.
  */
 int amd_iommu_trans_devid_reserve(struct amd_iommu_pci_seg *pci_seg, u16 id)
 {
 	void *entry;
+	struct amd_iommu_viommu *aviommu;
 	int ret = 0;
 
 	mutex_lock(&pci_seg->trans_devid_mutex);
@@ -99,6 +203,16 @@ int amd_iommu_trans_devid_reserve(struct amd_iommu_pci_seg *pci_seg, u16 id)
 	if (trans_devid_xa_is_reserved(entry))
 		goto unlock;
 
+	aviommu = trans_devid_xa_owner(entry);
+	if (aviommu) {
+		mutex_unlock(&pci_seg->trans_devid_mutex);
+		ret = trans_devid_relocate(pci_seg, id, aviommu);
+		if (!ret)
+			pr_debug("%s: Reserved trans_devid %#x after relocation (seg %#x)\n",
+				 __func__, id, pci_seg->id);
+		return ret;
+	}
+
 	ret = trans_devid_xa_install_reserved_locked(pci_seg, id);
 unlock:
 	mutex_unlock(&pci_seg->trans_devid_mutex);
-- 
2.34.1