From: Jean-Philippe Brucker <jean-philippe.bruc...@arm.com>

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

Signed-off-by: Eric Auger <eric.au...@redhat.com>
Signed-off-by: Jean-Philippe Brucker <jean-philippe.bruc...@arm.com>
---
 drivers/iommu/arm-smmu-v3.c | 112 +++++++++++++++++++++++++++++++++++-
 1 file changed, 111 insertions(+), 1 deletion(-)

diff --git a/drivers/iommu/arm-smmu-v3.c b/drivers/iommu/arm-smmu-v3.c
index 82508730feb7..ac7009348749 100644
--- a/drivers/iommu/arm-smmu-v3.c
+++ b/drivers/iommu/arm-smmu-v3.c
@@ -677,6 +677,16 @@ struct arm_smmu_device {
 
        /* IOMMU core code handle */
        struct iommu_device             iommu;
+
+       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 */
@@ -687,6 +697,7 @@ struct arm_smmu_master {
        struct list_head                domain_head;
        u32                             *sids;
        unsigned int                    num_sids;
+       struct arm_smmu_stream          *streams;
        bool                            ats_enabled;
        unsigned int                    ssid_bits;
 };
@@ -1967,6 +1978,32 @@ 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;
+       struct arm_smmu_master *master = NULL;
+
+       mutex_lock(&smmu->streams_mutex);
+       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 {
+                       master = stream->master;
+                       break;
+               }
+       }
+       mutex_unlock(&smmu->streams_mutex);
+
+       return master;
+}
+
 /* IRQ and event handlers */
 static irqreturn_t arm_smmu_evtq_thread(int irq, void *dev)
 {
@@ -2912,6 +2949,69 @@ 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;
+
+       master->streams = kcalloc(master->num_sids,
+                                 sizeof(struct arm_smmu_stream), GFP_KERNEL);
+       if (!master->streams)
+               return -ENOMEM;
+
+       mutex_lock(&smmu->streams_mutex);
+       for (i = 0; i < master->num_sids && !ret; i++) {
+               new_stream = &master->streams[i];
+               new_stream->id = master->sids[i];
+               new_stream->master = master;
+
+               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 {
+                               dev_warn(master->dev,
+                                        "stream %u already in tree\n",
+                                        cur_stream->id);
+                               ret = -EINVAL;
+                               break;
+                       }
+               }
+
+               if (!ret) {
+                       rb_link_node(&new_stream->node, parent_node, new_node);
+                       rb_insert_color(&new_stream->node, &smmu->streams);
+               }
+       }
+       mutex_unlock(&smmu->streams_mutex);
+
+       return ret;
+}
+
+static void arm_smmu_remove_master(struct arm_smmu_device *smmu,
+                                  struct arm_smmu_master *master)
+{
+       int i;
+
+       if (!master->streams)
+               return;
+
+       mutex_lock(&smmu->streams_mutex);
+       for (i = 0; i < master->num_sids; i++)
+               rb_erase(&master->streams[i].node, &smmu->streams);
+       mutex_unlock(&smmu->streams_mutex);
+
+       kfree(master->streams);
+}
+
 static struct iommu_ops arm_smmu_ops;
 
 static int arm_smmu_add_device(struct device *dev)
@@ -2979,15 +3079,21 @@ static int arm_smmu_add_device(struct device *dev)
        if (ret)
                goto err_disable_pasid;
 
+       ret = arm_smmu_insert_master(smmu, master);
+       if (ret)
+               goto err_unlink;
+
        group = iommu_group_get_for_dev(dev);
        if (IS_ERR(group)) {
                ret = PTR_ERR(group);
-               goto err_unlink;
+               goto err_remove_master;
        }
 
        iommu_group_put(group);
        return 0;
 
+err_remove_master:
+       arm_smmu_remove_master(smmu, master);
 err_unlink:
        iommu_device_unlink(&smmu->iommu, dev);
 err_disable_pasid:
@@ -3011,6 +3117,7 @@ static void arm_smmu_remove_device(struct device *dev)
        smmu = master->smmu;
        arm_smmu_detach_dev(master);
        iommu_group_remove_device(dev);
+       arm_smmu_remove_master(smmu, master);
        iommu_device_unlink(&smmu->iommu, dev);
        arm_smmu_disable_pasid(master);
        kfree(master);
@@ -3365,6 +3472,9 @@ static int 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;
-- 
2.20.1

_______________________________________________
iommu mailing list
iommu@lists.linux-foundation.org
https://lists.linuxfoundation.org/mailman/listinfo/iommu

Reply via email to