[PATCH v1 3/4] iommu: Implement iommu_set_rid_fault_notifier()

[Sergey Temerkhanov <sergey.temerkhanov@intel.com>]

Introduce the iommu_set_rid_fault_notifier() function which sets
an IOMMU fault notification callback for a specified endpoint device.
PCI endpoint device support is implemented in this patch, with possibility
of extension.

This change will provide endpoint device drivers with means of retrieving
details of IOMMU page faults occurring during operation and taking
device-specific actions.

Sample code using this facility:

static int sample_iommu_fault_notify(struct device *dev,
				     struct iommu_fault *fault,
                                     enum iommu_page_response_code,
				     void *data)
{
       printk("Fault data: rid: %#x, pasid: %u, ...  addr: %#llx\n",
              fault->prm.rid, fault->prm.pasid, ...
	      fault->prm.addr);
}

...

       ret = iommu_set_rid_fault_notifier(&pdev->dev,
					  sample_iommu_fault_notify,
					  data);
       if (ret)
               ...

Signed-off-by: Sergey Temerkhanov <sergey.temerkhanov@intel.com>
Reviewed-by: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
---
 drivers/iommu/io-pgfault.c | 53 ++++++++++++++++++++++++++++++++++++++
 drivers/iommu/iommu-priv.h | 11 ++++++++
 drivers/iommu/iommu.c      | 12 +++++++++
 include/linux/iommu.h      | 17 ++++++++++--
 4 files changed, 91 insertions(+), 2 deletions(-)

diff --git a/drivers/iommu/io-pgfault.c b/drivers/iommu/io-pgfault.c
index 8b5926c1452e..8e50c5f03da2 100644
--- a/drivers/iommu/io-pgfault.c
+++ b/drivers/iommu/io-pgfault.c
@@ -5,6 +5,7 @@
  * Copyright (C) 2020 ARM Ltd.
  */
 
+#include <linux/cleanup.h>
 #include <linux/iommu.h>
 #include <linux/list.h>
 #include <linux/sched/mm.h>
@@ -547,3 +548,55 @@ void iopf_queue_free(struct iopf_queue *queue)
 	kfree(queue);
 }
 EXPORT_SYMBOL_GPL(iopf_queue_free);
+
+/**
+ * iommu_set_rid_fault_notifier() - Set a Requester ID fault notifier
+ * @dev: the requester device
+ * @notifier: notifier function pointer (NULL unsets the notifier)
+ * @data: data to pass to the notifier function as a parameter
+ *
+ * Set or remove a device Requester ID based IOMMU fault failure
+ * notifier function.
+ *
+ * Return: 0 on success, or an error code.
+ */
+int iommu_set_rid_fault_notifier(struct device *dev,
+				 iommu_fault_rid_notifier_t notifier,
+				 void *data)
+{
+	struct iommu_rid_notifier *entry, *old;
+	struct dev_iommu *param = dev->iommu;
+	u32 rid;
+	int ret;
+
+	if (!param || !param->fault_param)
+		return -EINVAL;
+
+	rid = iommu_get_dev_rid(dev);
+
+	if (rid == IOMMU_INVALID_RID)
+		return -EINVAL;
+
+	guard(mutex)(&param->lock);
+
+	entry = kmalloc(sizeof(struct iommu_rid_notifier), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	entry->notifier = notifier;
+	entry->data = data;
+	entry->dev = dev;
+
+	old = xa_store(&param->rid_notifiers, rid, entry, GFP_KERNEL);
+
+	ret = xa_err(old);
+	if (ret) {
+		kfree(entry);
+		return ret;
+	}
+
+	kfree(old);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iommu_set_rid_fault_notifier);
diff --git a/drivers/iommu/iommu-priv.h b/drivers/iommu/iommu-priv.h
index e236b932e766..71e187bc024b 100644
--- a/drivers/iommu/iommu-priv.h
+++ b/drivers/iommu/iommu-priv.h
@@ -63,4 +63,15 @@ static inline int iommufd_sw_msi(struct iommu_domain *domain,
 int iommu_replace_device_pasid(struct iommu_domain *domain,
 			       struct device *dev, ioasid_t pasid,
 			       struct iommu_attach_handle *handle);
+
+struct iommu_rid_notifier {
+	iommu_fault_rid_notifier_t notifier;
+	struct device *dev;
+	void *data;
+};
+
+#define IOMMU_INVALID_RID	U32_MAX
+
+u32 iommu_get_dev_rid(struct device *dev);
+
 #endif /* __LINUX_IOMMU_PRIV_H */
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 3789c16806a1..c34c602dfb8a 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -3845,3 +3845,15 @@ int iommu_dma_prepare_msi(struct msi_desc *desc, phys_addr_t msi_addr)
 	return ret;
 }
 #endif /* CONFIG_IRQ_MSI_IOMMU */
+
+u32 iommu_get_dev_rid(struct device *dev)
+{
+	struct pci_dev *pdev;
+
+	if (!dev_is_pci(dev))
+		return IOMMU_INVALID_RID;
+
+	pdev = to_pci_dev(dev);
+
+	return PCI_DEVID(pdev->bus->number, pdev->devfn);
+}
diff --git a/include/linux/iommu.h b/include/linux/iommu.h
index 2062623fdf32..3a3f92222e27 100644
--- a/include/linux/iommu.h
+++ b/include/linux/iommu.h
@@ -161,6 +161,8 @@ struct iopf_queue {
 
 typedef int (*iommu_fault_handler_t)(struct iommu_domain *,
 			struct device *, unsigned long, int, void *);
+typedef void (*iommu_fault_rid_notifier_t)(struct device *, struct iommu_fault *,
+					   enum iommu_page_response_code, void *);
 
 struct iommu_domain_geometry {
 	dma_addr_t aperture_start; /* First address that can be mapped    */
@@ -803,6 +805,7 @@ struct iommu_fault_param {
  */
 struct dev_iommu {
 	struct mutex lock;
+	struct xarray			rid_notifiers;
 	struct iommu_fault_param __rcu	*fault_param;
 	struct iommu_fwspec		*fwspec;
 	struct iommu_device		*iommu_dev;
@@ -812,8 +815,6 @@ struct dev_iommu {
 	u32				pci_32bit_workaround:1;
 	u32				require_direct:1;
 	u32				shadow_on_flush:1;
-
-	struct xarray			rid_notifiers;
 };
 
 int iommu_device_register(struct iommu_device *iommu,
@@ -931,6 +932,10 @@ void iommu_set_dma_strict(void);
 extern int report_iommu_fault(struct iommu_domain *domain, struct device *dev,
 			      unsigned long iova, int flags);
 
+extern int iommu_set_rid_fault_notifier(struct device *dev,
+					iommu_fault_rid_notifier_t notifier,
+					void *data);
+
 static inline void iommu_flush_iotlb_all(struct iommu_domain *domain)
 {
 	if (domain->ops->flush_iotlb_all)
@@ -1480,6 +1485,14 @@ static inline int iommu_dma_prepare_msi(struct msi_desc *desc,
 {
 	return 0;
 }
+
+static inline int iommu_set_rid_fault_notifier(struct device *dev,
+					       iommu_fault_rid_notifier_t notifier,
+					       void *data)
+{
+	return 0;
+}
+
 #endif /* CONFIG_IOMMU_API */
 #endif /* CONFIG_IRQ_MSI_IOMMU */
 
-- 
2.43.0