1 files changed, 137 insertions, 93 deletions

diff --git a/arch/arm/mach-msm/iommu_dev.c b/arch/arm/mach-msm/iommu_dev.c
index b83c73b41fd1..8e8fb079852d 100644
--- a/arch/arm/mach-msm/iommu_dev.c
+++ b/arch/arm/mach-msm/iommu_dev.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010-2011, Code Aurora Forum. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -29,6 +29,7 @@
 
 #include <mach/iommu_hw-8xxx.h>
 #include <mach/iommu.h>
+#include <mach/clk.h>
 
 struct iommu_ctx_iter_data {
         /* input */
@@ -84,9 +85,9 @@ fail:
 }
 EXPORT_SYMBOL(msm_iommu_get_ctx);
 
-static void msm_iommu_reset(void __iomem *base)
+static void msm_iommu_reset(void __iomem *base, int ncb)
 {
-        int ctx, ncb;
+        int ctx;
 
         SET_RPUE(base, 0);
         SET_RPUEIE(base, 0);
@@ -99,7 +100,6 @@ static void msm_iommu_reset(void __iomem *base)
         SET_GLOBAL_TLBIALL(base, 0);
         SET_RPU_ACR(base, 0);
         SET_TLBLKCRWE(base, 1);
-        ncb = GET_NCB(base)+1;
 
         for (ctx = 0; ctx < ncb; ctx++) {
                 SET_BPRCOSH(base, ctx, 0);
@@ -130,117 +130,140 @@ static int msm_iommu_probe(struct platform_device *pdev)
 {
         struct resource *r, *r2;
         struct clk *iommu_clk;
+        struct clk *iommu_pclk;
         struct msm_iommu_drvdata *drvdata;
         struct msm_iommu_dev *iommu_dev = pdev->dev.platform_data;
         void __iomem *regs_base;
         resource_size_t len;
-        int ret = 0, ncb, nm2v, irq;
+        int ret, irq, par;
 
-        if (pdev->id != -1) {
-                drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
+        if (pdev->id == -1) {
+                msm_iommu_root_dev = pdev;
+                return 0;
+        }
 
-                if (!drvdata) {
-                        ret = -ENOMEM;
-                        goto fail;
-                }
+        drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
 
-                if (!iommu_dev) {
-                        ret = -ENODEV;
-                        goto fail;
-                }
+        if (!drvdata) {
+                ret = -ENOMEM;
+                goto fail;
+        }
 
-                if (iommu_dev->clk_rate != 0) {
-                        iommu_clk = clk_get(&pdev->dev, "iommu_clk");
-
-                        if (IS_ERR(iommu_clk)) {
-                                ret = -ENODEV;
-                                goto fail;
-                        }
-
-                        if (iommu_dev->clk_rate > 0) {
-                                ret = clk_set_rate(iommu_clk,
-                                                        iommu_dev->clk_rate);
-                                if (ret) {
-                                        clk_put(iommu_clk);
-                                        goto fail;
-                                }
-                        }
-
-                        ret = clk_enable(iommu_clk);
-                        if (ret) {
-                                clk_put(iommu_clk);
-                                goto fail;
-                        }
+        if (!iommu_dev) {
+                ret = -ENODEV;
+                goto fail;
+        }
+
+        iommu_pclk = clk_get(NULL, "smmu_pclk");
+        if (IS_ERR(iommu_pclk)) {
+                ret = -ENODEV;
+                goto fail;
+        }
+
+        ret = clk_enable(iommu_pclk);
+        if (ret)
+                goto fail_enable;
+
+        iommu_clk = clk_get(&pdev->dev, "iommu_clk");
+
+        if (!IS_ERR(iommu_clk)) {
+                if (clk_get_rate(iommu_clk) == 0)
+                        clk_set_min_rate(iommu_clk, 1);
+
+                ret = clk_enable(iommu_clk);
+                if (ret) {
                         clk_put(iommu_clk);
+                        goto fail_pclk;
                 }
+        } else
+                iommu_clk = NULL;
 
-                r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
-                                                 "physbase");
-                if (!r) {
-                        ret = -ENODEV;
-                        goto fail;
-                }
+        r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "physbase");
 
-                len = r->end - r->start + 1;
+        if (!r) {
+                ret = -ENODEV;
+                goto fail_clk;
+        }
 
-                r2 = request_mem_region(r->start, len, r->name);
-                if (!r2) {
-                        pr_err("Could not request memory region: "
-                        "start=%p, len=%d\n", (void *) r->start, len);
-                        ret = -EBUSY;
-                        goto fail;
-                }
+        len = resource_size(r);
 
-                regs_base = ioremap(r2->start, len);
+        r2 = request_mem_region(r->start, len, r->name);
+        if (!r2) {
+                pr_err("Could not request memory region: start=%p, len=%d\n",
+                                                        (void *) r->start, len);
+                ret = -EBUSY;
+                goto fail_clk;
+        }
 
-                if (!regs_base) {
-                        pr_err("Could not ioremap: start=%p, len=%d\n",
-                                 (void *) r2->start, len);
-                        ret = -EBUSY;
-                        goto fail_mem;
-                }
+        regs_base = ioremap(r2->start, len);
 
-                irq = platform_get_irq_byname(pdev, "secure_irq");
-                if (irq < 0) {
-                        ret = -ENODEV;
-                        goto fail_io;
-                }
+        if (!regs_base) {
+                pr_err("Could not ioremap: start=%p, len=%d\n",
+                         (void *) r2->start, len);
+                ret = -EBUSY;
+                goto fail_mem;
+        }
 
-                mb();
+        irq = platform_get_irq_byname(pdev, "secure_irq");
+        if (irq < 0) {
+                ret = -ENODEV;
+                goto fail_io;
+        }
 
-                if (GET_IDR(regs_base) == 0) {
-                        pr_err("Invalid IDR value detected\n");
-                        ret = -ENODEV;
-                        goto fail_io;
-                }
+        msm_iommu_reset(regs_base, iommu_dev->ncb);
 
-                ret = request_irq(irq, msm_iommu_fault_handler, 0,
-                                "msm_iommu_secure_irpt_handler", drvdata);
-                if (ret) {
-                        pr_err("Request IRQ %d failed with ret=%d\n", irq, ret);
-                        goto fail_io;
-                }
+        SET_M(regs_base, 0, 1);
+        SET_PAR(regs_base, 0, 0);
+        SET_V2PCFG(regs_base, 0, 1);
+        SET_V2PPR(regs_base, 0, 0);
+        par = GET_PAR(regs_base, 0);
+        SET_V2PCFG(regs_base, 0, 0);
+        SET_M(regs_base, 0, 0);
 
-                msm_iommu_reset(regs_base);
-                drvdata->base = regs_base;
-                drvdata->irq = irq;
+        if (!par) {
+                pr_err("%s: Invalid PAR value detected\n", iommu_dev->name);
+                ret = -ENODEV;
+                goto fail_io;
+        }
 
-                nm2v = GET_NM2VCBMT((unsigned long) regs_base);
-                ncb = GET_NCB((unsigned long) regs_base);
+        ret = request_irq(irq, msm_iommu_fault_handler, 0,
+                        "msm_iommu_secure_irpt_handler", drvdata);
+        if (ret) {
+                pr_err("Request IRQ %d failed with ret=%d\n", irq, ret);
+                goto fail_io;
+        }
 
-                pr_info("device %s mapped at %p, irq %d with %d ctx banks\n",
-                        iommu_dev->name, regs_base, irq, ncb+1);
 
-                platform_set_drvdata(pdev, drvdata);
-        } else
-                msm_iommu_root_dev = pdev;
+        drvdata->pclk = iommu_pclk;
+        drvdata->clk = iommu_clk;
+        drvdata->base = regs_base;
+        drvdata->irq = irq;
+        drvdata->ncb = iommu_dev->ncb;
 
-        return 0;
+        pr_info("device %s mapped at %p, irq %d with %d ctx banks\n",
+                iommu_dev->name, regs_base, irq, iommu_dev->ncb);
+
+        platform_set_drvdata(pdev, drvdata);
+
+        if (iommu_clk)
+                clk_disable(iommu_clk);
+
+        clk_disable(iommu_pclk);
 
+        return 0;
 fail_io:
         iounmap(regs_base);
 fail_mem:
         release_mem_region(r->start, len);
+fail_clk:
+        if (iommu_clk) {
+                clk_disable(iommu_clk);
+                clk_put(iommu_clk);
+        }
+fail_pclk:
+        clk_disable(iommu_pclk);
+fail_enable:
+        clk_put(iommu_pclk);
 fail:
         kfree(drvdata);
         return ret;
@@ -252,7 +275,10 @@ static int msm_iommu_remove(struct platform_device *pdev)
 
         drv = platform_get_drvdata(pdev);
         if (drv) {
-                memset(drv, 0, sizeof(struct msm_iommu_drvdata));
+                if (drv->clk)
+                        clk_put(drv->clk);
+                clk_put(drv->pclk);
+                memset(drv, 0, sizeof(*drv));
                 kfree(drv);
                 platform_set_drvdata(pdev, NULL);
         }
@@ -264,7 +290,7 @@ static int msm_iommu_ctx_probe(struct platform_device *pdev)
         struct msm_iommu_ctx_dev *c = pdev->dev.platform_data;
         struct msm_iommu_drvdata *drvdata;
         struct msm_iommu_ctx_drvdata *ctx_drvdata = NULL;
-        int i, ret = 0;
+        int i, ret;
         if (!c || !pdev->dev.parent) {
                 ret = -EINVAL;
                 goto fail;
@@ -288,6 +314,18 @@ static int msm_iommu_ctx_probe(struct platform_device *pdev)
         INIT_LIST_HEAD(&ctx_drvdata->attached_elm);
         platform_set_drvdata(pdev, ctx_drvdata);
 
+        ret = clk_enable(drvdata->pclk);
+        if (ret)
+                goto fail;
+
+        if (drvdata->clk) {
+                ret = clk_enable(drvdata->clk);
+                if (ret) {
+                        clk_disable(drvdata->pclk);
+                        goto fail;
+                }
+        }
+
         /* Program the M2V tables for this context */
         for (i = 0; i < MAX_NUM_MIDS; i++) {
                 int mid = c->mids[i];
@@ -297,21 +335,27 @@ static int msm_iommu_ctx_probe(struct platform_device *pdev)
                 SET_M2VCBR_N(drvdata->base, mid, 0);
                 SET_CBACR_N(drvdata->base, c->num, 0);
 
-                /* Set VMID = MID */
-                SET_VMID(drvdata->base, mid, mid);
+                /* Set VMID = 0 */
+                SET_VMID(drvdata->base, mid, 0);
 
                 /* Set the context number for that MID to this context */
                 SET_CBNDX(drvdata->base, mid, c->num);
 
-                /* Set MID associated with this context bank */
-                SET_CBVMID(drvdata->base, c->num, mid);
+                /* Set MID associated with this context bank to 0*/
+                SET_CBVMID(drvdata->base, c->num, 0);
+
+                /* Set the ASID for TLB tagging for this context */
+                SET_CONTEXTIDR_ASID(drvdata->base, c->num, c->num);
 
                 /* Set security bit override to be Non-secure */
                 SET_NSCFG(drvdata->base, mid, 3);
         }
 
-        pr_info("context device %s with bank index %d\n", c->name, c->num);
+        if (drvdata->clk)
+                clk_disable(drvdata->clk);
+        clk_disable(drvdata->pclk);
 
+        dev_info(&pdev->dev, "context %s using bank %d\n", c->name, c->num);
         return 0;
 fail:
         kfree(ctx_drvdata);