[RFC PATCHES 16/17] iommufd/selftest: Add IOPF feature for mock devices

[Lu Baolu <baolu.lu@xxxxxxxxxxxxxxx>]

So that we can test the delilvery of IO page faults through IOMMU with
the selftest infrastructure.

Signed-off-by: Lu Baolu <baolu.lu@xxxxxxxxxxxxxxx>
---
 drivers/iommu/iommufd/selftest.c | 71 ++++++++++++++++++++++++++++++++
 1 file changed, 71 insertions(+)

diff --git a/drivers/iommu/iommufd/selftest.c b/drivers/iommu/iommufd/selftest.c
index debf2d588990..d3d3342e95b6 100644
--- a/drivers/iommu/iommufd/selftest.c
+++ b/drivers/iommu/iommufd/selftest.c
@@ -14,6 +14,7 @@
 #include "io_pagetable.h"
 #include "iommufd_private.h"
 #include "iommufd_test.h"
+#include "../io-pgfault.h"
 
 static DECLARE_FAULT_ATTR(fail_iommufd);
 static struct dentry *dbgfs_root;
@@ -96,6 +97,8 @@ enum selftest_obj_type {
 struct mock_dev {
 	struct device dev;
 	u32 dev_data;
+	unsigned char iopfq_name[16];
+	struct iopf_queue *iopf_queue;
 };
 
 struct selftest_obj {
@@ -360,6 +363,64 @@ static int mock_domain_user_data_len(u32 hwpt_type)
 	return sizeof(struct iommu_hwpt_selftest);
 };
 
+static int mock_dev_enable_feat(struct device *dev, enum iommu_dev_features feat)
+{
+	struct mock_dev *mdev = container_of(dev, struct mock_dev, dev);
+	struct iommu_group *group;
+	int rc;
+
+	if (feat != IOMMU_DEV_FEAT_IOPF)
+		return -EOPNOTSUPP;
+
+	group = iommu_group_get(dev);
+	if (!group)
+		return -ENODEV;
+
+	/* Allocate the iopf queue: */
+	snprintf(mdev->iopfq_name, sizeof(mdev->iopfq_name),
+		 "mock%d-iopfq", iommu_group_id(group));
+	mdev->iopf_queue = iopf_queue_alloc(mdev->iopfq_name);
+	if (!mdev->iopf_queue) {
+		rc = -ENOMEM;
+		goto err_put_group;
+	}
+
+	/* Register I/O page fault: */
+	rc = iopf_queue_add_device(mdev->iopf_queue, &mdev->dev);
+	if (rc)
+		goto err_free_queue;
+	rc = iommu_register_device_fault_handler(&mdev->dev, iommu_queue_iopf,
+						 &mdev->dev);
+	if (rc)
+		goto err_remove_device;
+
+	iommu_group_put(group);
+
+	return 0;
+
+err_remove_device:
+	iopf_queue_remove_device(mdev->iopf_queue, &mdev->dev);
+err_free_queue:
+	iopf_queue_free(mdev->iopf_queue);
+err_put_group:
+	iommu_group_put(group);
+	return rc;
+}
+
+static int mock_dev_disable_feat(struct device *dev, enum iommu_dev_features feat)
+{
+	struct mock_dev *mdev = container_of(dev, struct mock_dev, dev);
+
+	if (feat != IOMMU_DEV_FEAT_IOPF)
+		return -EOPNOTSUPP;
+
+	iommu_unregister_device_fault_handler(dev);
+	iopf_queue_remove_device(mdev->iopf_queue, dev);
+	iopf_queue_free(mdev->iopf_queue);
+
+	return 0;
+}
+
 static const struct iommu_ops mock_ops = {
 	.owner = THIS_MODULE,
 	.pgsize_bitmap = MOCK_IO_PAGE_SIZE,
@@ -373,6 +434,8 @@ static const struct iommu_ops mock_ops = {
 	.set_dev_user_data = mock_domain_set_dev_user_data,
 	.unset_dev_user_data = mock_domain_unset_dev_user_data,
 	.dev_user_data_len = sizeof(struct iommu_test_device_data),
+	.dev_enable_feat = mock_dev_enable_feat,
+	.dev_disable_feat = mock_dev_disable_feat,
 	.default_domain_ops =
 		&(struct iommu_domain_ops){
 			.free = mock_domain_free,
@@ -494,9 +557,16 @@ static struct mock_dev *mock_dev_create(void)
 	rc = iommu_group_add_device(iommu_group, &mdev->dev);
 	if (rc)
 		goto err_del;
+
+	rc = iommu_dev_enable_feature(&mdev->dev, IOMMU_DEV_FEAT_IOPF);
+	if (rc)
+		goto err_remove;
+
 	iommu_group_put(iommu_group);
 	return mdev;
 
+err_remove:
+	iommu_group_remove_device(&mdev->dev);
 err_del:
 	device_del(&mdev->dev);
 err_dev_iommu:
@@ -511,6 +581,7 @@ static struct mock_dev *mock_dev_create(void)
 
 static void mock_dev_destroy(struct mock_dev *mdev)
 {
+	iommu_dev_disable_feature(&mdev->dev, IOMMU_DEV_FEAT_IOPF);
 	iommu_group_remove_device(&mdev->dev);
 	device_del(&mdev->dev);
 	kfree(mdev->dev.iommu);
-- 
2.34.1