1 files changed, 58 insertions, 3 deletions

diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index fbceae3c2ee7..7640f2bd7c81 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -883,15 +883,55 @@ static void omap_iommu_detach(struct omap_iommu *obj)
 
 	dma_unmap_single(obj->dev, obj->pd_dma, IOPGD_TABLE_SIZE,
 			 DMA_TO_DEVICE);
-	iommu_disable(obj);
 	obj->pd_dma = 0;
 	obj->iopgd = NULL;
+	iommu_disable(obj);
 
 	spin_unlock(&obj->iommu_lock);
 
 	dev_dbg(obj->dev, "%s: %s\n", __func__, obj->name);
 }
 
+static void omap_iommu_save_tlb_entries(struct omap_iommu *obj)
+{
+	struct iotlb_lock lock;
+	struct cr_regs cr;
+	struct cr_regs *tmp;
+	int i;
+
+	/* check if there are any locked tlbs to save */
+	iotlb_lock_get(obj, &lock);
+	obj->num_cr_ctx = lock.base;
+	if (!obj->num_cr_ctx)
+		return;
+
+	tmp = obj->cr_ctx;
+	for_each_iotlb_cr(obj, obj->num_cr_ctx, i, cr)
+		* tmp++ = cr;
+}
+
+static void omap_iommu_restore_tlb_entries(struct omap_iommu *obj)
+{
+	struct iotlb_lock l;
+	struct cr_regs *tmp;
+	int i;
+
+	/* no locked tlbs to restore */
+	if (!obj->num_cr_ctx)
+		return;
+
+	l.base = 0;
+	tmp = obj->cr_ctx;
+	for (i = 0; i < obj->num_cr_ctx; i++, tmp++) {
+		l.vict = i;
+		iotlb_lock_set(obj, &l);
+		iotlb_load_cr(obj, tmp);
+	}
+	l.base = obj->num_cr_ctx;
+	l.vict = i;
+	iotlb_lock_set(obj, &l);
+}
+
 /**
  * omap_iommu_runtime_suspend - disable an iommu device
  * @dev:	iommu device
@@ -901,7 +941,8 @@ static void omap_iommu_detach(struct omap_iommu *obj)
  * device, or during system/runtime suspend of the device. This
  * includes programming all the appropriate IOMMU registers, and
  * managing the associated omap_hwmod's state and the device's
- * reset line.
+ * reset line. This function also saves the context of any
+ * locked TLBs if suspending.
  **/
 static int omap_iommu_runtime_suspend(struct device *dev)
 {
@@ -910,6 +951,10 @@ static int omap_iommu_runtime_suspend(struct device *dev)
 	struct omap_iommu *obj = to_iommu(dev);
 	int ret;
 
+	/* save the TLBs only during suspend, and not for power down */
+	if (obj->domain && obj->iopgd)
+		omap_iommu_save_tlb_entries(obj);
+
 	omap2_iommu_disable(obj);
 
 	if (pdata && pdata->device_idle)
@@ -938,7 +983,8 @@ static int omap_iommu_runtime_suspend(struct device *dev)
  * device, or during system/runtime resume of the device. This
  * includes programming all the appropriate IOMMU registers, and
  * managing the associated omap_hwmod's state and the device's
- * reset line.
+ * reset line. The function also restores any locked TLBs if
+ * resuming after a suspend.
  **/
 static int omap_iommu_runtime_resume(struct device *dev)
 {
@@ -966,6 +1012,10 @@ static int omap_iommu_runtime_resume(struct device *dev)
 	if (pdata && pdata->device_enable)
 		pdata->device_enable(pdev);
 
+	/* restore the TLBs only during resume, and not for power up */
+	if (obj->domain)
+		omap_iommu_restore_tlb_entries(obj);
+
 	ret = omap2_iommu_enable(obj);
 
 	return ret;
@@ -1066,6 +1116,11 @@ static int omap_iommu_probe(struct platform_device *pdev)
 
 	obj->dev = &pdev->dev;
 	obj->ctx = (void *)obj + sizeof(*obj);
+	obj->cr_ctx = devm_kzalloc(&pdev->dev,
+				   sizeof(*obj->cr_ctx) * obj->nr_tlb_entries,
+				   GFP_KERNEL);
+	if (!obj->cr_ctx)
+		return -ENOMEM;
 
 	spin_lock_init(&obj->iommu_lock);
 	spin_lock_init(&obj->page_table_lock);