[PATCH v4 01/22] xen/arm: smmuv3: Maintain a SID->device structure

[Milan Djokic <milan_djokic@xxxxxxxx>]

From: Jean-Philippe Brucker <jean-philippe@xxxxxxxxxx>

Backport Linux commits cdf315f907d4 and b00d24997a11, adapted to the Xen
SMMUv3 implementation.

When handling faults from the event or PRI queue, we need to find the
struct device associated with a SID. Add an rb_tree to keep track of
SIDs.

Also handle duplicate SIDs, as bridged PCI devices may have multiple
streams with the same SID.

The backport is adapted to Xen's SMMUv3 driver structure and types.
Fixes are also included in the master insertion rollback
and teardown paths to correctly handle partially
inserted stream entries and avoid leaving stale rb_tree nodes.

Linux commits:

  cdf315f907d4 ("iommu/arm-smmu-v3: Maintain a SID->device structure")
  b00d24997a11 ("iommu/arm-smmu-v3: Fix iommu_device_probe bug due to 
duplicated stream ids")

Link: https://lore.kernel.org/r/20210401154718.307519-8-jean-philippe@xxxxxxxxxx
Link: https://lore.kernel.org/r/20250415185620.504299-1-nicolinc@xxxxxxxxxx
Origin: git://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git 
cdf315f907d4
Origin: git://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git 
b00d24997a11

Signed-off-by: Jean-Philippe Brucker <jean-philippe@xxxxxxxxxx>
Signed-off-by: Joerg Roedel <jroedel@xxxxxxx>
Signed-off-by: Nicolin Chen <nicolinc@xxxxxxxxxx>
Signed-off-by: Will Deacon <will@xxxxxxxxxx>
Signed-off-by: Rahul Singh <rahul.singh@xxxxxxx>
Signed-off-by: Milan Djokic <milan_djokic@xxxxxxxx>
---
 xen/drivers/passthrough/arm/smmu-v3.c | 176 ++++++++++++++++++++++----
 xen/drivers/passthrough/arm/smmu-v3.h |  13 +-
 2 files changed, 163 insertions(+), 26 deletions(-)

diff --git a/xen/drivers/passthrough/arm/smmu-v3.c 
b/xen/drivers/passthrough/arm/smmu-v3.c
index bf153227db..7162473d62 100644
--- a/xen/drivers/passthrough/arm/smmu-v3.c
+++ b/xen/drivers/passthrough/arm/smmu-v3.c
@@ -809,6 +809,27 @@ static int arm_smmu_init_l2_strtab(struct arm_smmu_device 
*smmu, u32 sid)
        return 0;
 }
 
+__maybe_unused
+static struct arm_smmu_master *
+arm_smmu_find_master(struct arm_smmu_device *smmu, u32 sid)
+{
+       struct rb_node *node;
+       struct arm_smmu_stream *stream;
+
+       node = smmu->streams.rb_node;
+       while (node) {
+               stream = rb_entry(node, struct arm_smmu_stream, node);
+               if (stream->id < sid)
+                       node = node->rb_right;
+               else if (stream->id > sid)
+                       node = node->rb_left;
+               else
+                       return stream->master;
+       }
+
+       return NULL;
+}
+
 /* IRQ and event handlers */
 static void arm_smmu_evtq_tasklet(void *dev)
 {
@@ -1042,8 +1063,8 @@ static int arm_smmu_atc_inv_master(struct arm_smmu_master 
*master,
        if (!master->ats_enabled)
                return 0;
 
-       for (i = 0; i < master->num_sids; i++) {
-               cmd->atc.sid = master->sids[i];
+       for (i = 0; i < master->num_streams; i++) {
+               cmd->atc.sid = master->streams[i].id;
                arm_smmu_cmdq_issue_cmd(master->smmu, cmd);
        }
 
@@ -1271,13 +1292,13 @@ static void arm_smmu_install_ste_for_dev(struct 
arm_smmu_master *master)
        int i, j;
        struct arm_smmu_device *smmu = master->smmu;
 
-       for (i = 0; i < master->num_sids; ++i) {
-               u32 sid = master->sids[i];
+       for (i = 0; i < master->num_streams; ++i) {
+               u32 sid = master->streams[i].id;
                __le64 *step = arm_smmu_get_step_for_sid(smmu, sid);
 
                /* Bridged PCI devices may end up with duplicated IDs */
                for (j = 0; j < i; j++)
-                       if (master->sids[j] == sid)
+                       if (master->streams[j].id == sid)
                                break;
                if (j < i)
                        continue;
@@ -1486,6 +1507,123 @@ static bool arm_smmu_sid_in_range(struct 
arm_smmu_device *smmu, u32 sid)
 
        return sid < limit;
 }
+
+static int arm_smmu_insert_master(struct arm_smmu_device *smmu,
+                                 struct arm_smmu_master *master)
+{
+       int i;
+       int ret = 0;
+       struct arm_smmu_stream *new_stream, *cur_stream;
+       struct rb_node **new_node, *parent_node = NULL;
+       struct iommu_fwspec *fwspec = dev_iommu_fwspec_get(master->dev);
+       bool *insert;
+
+       master->streams = _xzalloc_array(sizeof(*master->streams), sizeof(void 
*),
+                                       fwspec->num_ids);
+       if (!master->streams)
+               return -ENOMEM;
+
+       insert = _xzalloc_array(sizeof(bool), sizeof(bool *),
+                                       fwspec->num_ids);
+       if (!insert) {
+               xfree(master->streams);
+               master->streams = NULL;
+               return -ENOMEM;
+       }
+
+       master->num_streams = fwspec->num_ids;
+
+       mutex_lock(&smmu->streams_mutex);
+       for (i = 0; i < fwspec->num_ids; i++) {
+               u32 sid = fwspec->ids[i];
+               insert[i] = false;
+
+               new_stream = &master->streams[i];
+               new_stream->id = sid;
+               new_stream->master = master;
+               RB_CLEAR_NODE(&new_stream->node);
+
+               /*
+                * Check the SIDs are in range of the SMMU and our stream table
+                */
+               if (!arm_smmu_sid_in_range(smmu, sid)) {
+                       ret = -ERANGE;
+                       break;
+               }
+
+               /* Ensure l2 strtab is initialised */
+               if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
+                       ret = arm_smmu_init_l2_strtab(smmu, sid);
+                       if (ret)
+                               break;
+               }
+
+               /* Insert into SID tree */
+               insert[i] = true;
+               new_node = &(smmu->streams.rb_node);
+               while (*new_node) {
+                       cur_stream = rb_entry(*new_node, struct arm_smmu_stream,
+                                             node);
+                       parent_node = *new_node;
+                       if (cur_stream->id > new_stream->id) {
+                               new_node = &((*new_node)->rb_left);
+                       } else if (cur_stream->id < new_stream->id) {
+                               new_node = &((*new_node)->rb_right);
+                       } else if (cur_stream->master == master) {
+                               /* Bridged PCI devices may end up with 
duplicated IDs */
+                               insert[i] = false;
+                               break;
+                       }
+                       else {
+                               dev_warn(master->dev,
+                                        "stream %u already in tree\n",
+                                        cur_stream->id);
+                               ret = -EINVAL;
+                               insert[i] = false;
+                               break;
+                       }
+               }
+               if (ret)
+                       break;
+
+               if(insert[i]) {
+                       rb_link_node(&new_stream->node, parent_node, new_node);
+                       rb_insert_color(&new_stream->node, &smmu->streams);
+               }
+       }
+
+       if (ret) {
+               for (i--; i >= 0; i--)
+                       if (insert[i])
+                               rb_erase(&master->streams[i].node, 
&smmu->streams);
+               xfree(master->streams);
+               master->streams = NULL;
+       }
+       mutex_unlock(&smmu->streams_mutex);
+
+       xfree(insert);
+
+       return ret;
+}
+
+static void arm_smmu_remove_master(struct arm_smmu_master *master)
+{
+       int i;
+       struct arm_smmu_device *smmu = master->smmu;
+       struct iommu_fwspec *fwspec = dev_iommu_fwspec_get(master->dev);
+
+       if (!smmu || !master->streams)
+               return;
+
+       mutex_lock(&smmu->streams_mutex);
+       for (i = 0; i < fwspec->num_ids; i++)
+               if(!RB_EMPTY_NODE(&master->streams[i].node))
+                       rb_erase(&master->streams[i].node, &smmu->streams);
+       mutex_unlock(&smmu->streams_mutex);
+
+       xfree(master->streams);
+}
+
 /* Forward declaration */
 static struct arm_smmu_device *arm_smmu_get_by_dev(const struct device *dev);
 static int arm_smmu_assign_dev(struct domain *d, u8 devfn, struct device *dev,
@@ -1495,7 +1633,7 @@ static int arm_smmu_deassign_dev(struct domain *d, 
uint8_t devfn,
 
 static int arm_smmu_add_device(u8 devfn, struct device *dev)
 {
-       int i, ret;
+       int ret;
        struct arm_smmu_device *smmu;
        struct arm_smmu_master *master;
        struct iommu_fwspec *fwspec;
@@ -1532,26 +1670,11 @@ static int arm_smmu_add_device(u8 devfn, struct device 
*dev)
 
        master->dev = dev;
        master->smmu = smmu;
-       master->sids = fwspec->ids;
-       master->num_sids = fwspec->num_ids;
        dev_iommu_priv_set(dev, master);
 
-       /* Check the SIDs are in range of the SMMU and our stream table */
-       for (i = 0; i < master->num_sids; i++) {
-               u32 sid = master->sids[i];
-
-               if (!arm_smmu_sid_in_range(smmu, sid)) {
-                       ret = -ERANGE;
-                       goto err_free_master;
-               }
-
-               /* Ensure l2 strtab is initialised */
-               if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
-                       ret = arm_smmu_init_l2_strtab(smmu, sid);
-                       if (ret)
-                               goto err_free_master;
-               }
-       }
+       ret = arm_smmu_insert_master(smmu, master);
+       if (ret)
+               goto err_free_master;
 
        /*
         * Note that PASID must be enabled before, and disabled after ATS:
@@ -1598,6 +1721,8 @@ static int arm_smmu_add_device(u8 devfn, struct device 
*dev)
        return 0;
 
 err_free_master:
+       arm_smmu_disable_pasid(master);
+       arm_smmu_remove_master(master);
        xfree(master);
        dev_iommu_priv_set(dev, NULL);
        return ret;
@@ -1796,6 +1921,9 @@ static int __init arm_smmu_init_structures(struct 
arm_smmu_device *smmu)
 {
        int ret;
 
+       mutex_init(&smmu->streams_mutex);
+       smmu->streams = RB_ROOT;
+
        ret = arm_smmu_init_queues(smmu);
        if (ret)
                return ret;
diff --git a/xen/drivers/passthrough/arm/smmu-v3.h 
b/xen/drivers/passthrough/arm/smmu-v3.h
index ab07366294..ab1f29f6c7 100644
--- a/xen/drivers/passthrough/arm/smmu-v3.h
+++ b/xen/drivers/passthrough/arm/smmu-v3.h
@@ -639,6 +639,15 @@ struct arm_smmu_device {
        struct tasklet          evtq_irq_tasklet;
        struct tasklet          priq_irq_tasklet;
        struct tasklet          combined_irq_tasklet;
+
+       struct rb_root          streams;
+       struct mutex            streams_mutex;
+};
+
+struct arm_smmu_stream {
+       u32                                                     id;
+       struct arm_smmu_master          *master;
+       struct rb_node                          node;
 };
 
 /* SMMU private data for each master */
@@ -647,8 +656,8 @@ struct arm_smmu_master {
        struct device                   *dev;
        struct arm_smmu_domain          *domain;
        struct list_head                domain_head;
-       u32                             *sids;
-       unsigned int                    num_sids;
+       struct arm_smmu_stream          *streams;
+       unsigned int                            num_streams;
        bool                            ats_enabled;
 };
 
-- 
2.43.0