1 files changed, 107 insertions, 28 deletions

diff --git a/drivers/staging/tidspbridge/core/tiomap3430.c b/drivers/staging/tidspbridge/core/tiomap3430.c
index c2f5105..3749cd5 100644
--- a/drivers/staging/tidspbridge/core/tiomap3430.c
+++ b/drivers/staging/tidspbridge/core/tiomap3430.c
@@ -289,8 +289,7 @@ static int bridge_brd_monitor(struct bridge_dev_context *dev_ctxt)
 		(*pdata->dsp_cm_write)(OMAP34XX_CLKSTCTRL_DISABLE_AUTO,
 					OMAP3430_IVA2_MOD, OMAP2_CM_CLKSTCTRL);
 	}
-	(*pdata->dsp_prm_rmw_bits)(OMAP3430_RST2_IVA2_MASK, 0,
-					OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
+
 	dsp_clk_enable(DSP_CLK_IVA2);
 
 	/* set the device state to IDLE */
@@ -361,15 +360,17 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 {
 	int status = 0;
 	struct bridge_dev_context *dev_context = dev_ctxt;
-	struct iommu *mmu;
+	struct iommu *mmu = NULL;
+	struct shm_segs *sm_sg;
+	int l4_i = 0, tlb_i = 0;
+	u32 sg0_da = 0, sg1_da = 0;
+	struct bridge_ioctl_extproc *tlb = dev_context->atlb_entry;
 	u32 dw_sync_addr = 0;
 	u32 ul_shm_base;	/* Gpp Phys SM base addr(byte) */
 	u32 ul_shm_base_virt;	/* Dsp Virt SM base addr */
 	u32 ul_tlb_base_virt;	/* Base of MMU TLB entry */
 	/* Offset of shm_base_virt from tlb_base_virt */
 	u32 ul_shm_offset_virt;
-	s32 entry_ndx;
-	s32 itmp_entry_ndx = 0;	/* DSP-MMU TLB entry base address */
 	struct cfg_hostres *resources = NULL;
 	u32 temp;
 	u32 ul_dsp_clk_rate;
@@ -381,7 +382,6 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 	struct omap_dsp_platform_data *pdata =
 		omap_dspbridge_dev->dev.platform_data;
 
-	mmu = dev_context->dsp_mmu;
 	/* The device context contains all the mmu setup info from when the
 	 * last dsp base image was loaded. The first entry is always
 	 * SHMMEM base. */
@@ -391,12 +391,12 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 	ul_shm_base_virt *= DSPWORDSIZE;
 	DBC_ASSERT(ul_shm_base_virt != 0);
 	/* DSP Virtual address */
-	ul_tlb_base_virt = dev_context->atlb_entry[0].ul_dsp_va;
+	ul_tlb_base_virt = dev_context->sh_s.seg0_da;
 	DBC_ASSERT(ul_tlb_base_virt <= ul_shm_base_virt);
 	ul_shm_offset_virt =
 	    ul_shm_base_virt - (ul_tlb_base_virt * DSPWORDSIZE);
 	/* Kernel logical address */
-	ul_shm_base = dev_context->atlb_entry[0].ul_gpp_va + ul_shm_offset_virt;
+	ul_shm_base = dev_context->sh_s.seg0_va + ul_shm_offset_virt;
 
 	DBC_ASSERT(ul_shm_base != 0);
 	/* 2nd wd is used as sync field */
@@ -431,25 +431,70 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 					OMAP343X_CONTROL_IVA2_BOOTMOD));
 		}
 	}
-	if (!status) {
-		/* Only make TLB entry if both addresses are non-zero */
-		for (entry_ndx = 0; entry_ndx < BRDIOCTL_NUMOFMMUTLB;
-		     entry_ndx++) {
-			struct bridge_ioctl_extproc *e = &dev_context->atlb_entry[entry_ndx];
 
-			if (!e->ul_gpp_pa || !e->ul_dsp_va)
+	if (!status) {
+		(*pdata->dsp_prm_rmw_bits)(OMAP3430_RST2_IVA2_MASK, 0,
+					OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
+		mmu = dev_context->dsp_mmu;
+		if (mmu)
+			iommu_put(mmu);
+		mmu = iommu_get("iva2");
+		if (IS_ERR(mmu)) {
+			dev_err(bridge, "iommu_get failed!\n");
+			dev_context->dsp_mmu = NULL;
+			status = (int)mmu;
+		}
+	}
+	if (!status) {
+		dev_context->dsp_mmu = mmu;
+		sm_sg = &dev_context->sh_s;
+		sg0_da = iommu_kmap(mmu, sm_sg->seg0_da, sm_sg->seg0_pa,
+			sm_sg->seg0_size, IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_32);
+		if (IS_ERR_VALUE(sg0_da)) {
+			status = (int)sg0_da;
+			sg0_da = 0;
+		}
+	}
+	if (!status) {
+		sg1_da = iommu_kmap(mmu, sm_sg->seg1_da, sm_sg->seg1_pa,
+			sm_sg->seg1_size, IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_32);
+		if (IS_ERR_VALUE(sg1_da)) {
+			status = (int)sg1_da;
+			sg1_da = 0;
+		}
+	}
+	if (!status) {
+		u32 da;
+		for (tlb_i = 0; tlb_i < BRDIOCTL_NUMOFMMUTLB; tlb_i++) {
+			if (!tlb[tlb_i].ul_gpp_pa)
 				continue;
 
-			dev_dbg(bridge,
-					"MMU %d, pa: 0x%x, va: 0x%x, size: 0x%x",
-					itmp_entry_ndx,
-					e->ul_gpp_pa,
-					e->ul_dsp_va,
-					e->ul_size);
+			dev_dbg(bridge, "IOMMU %d GppPa: 0x%x DspVa 0x%x Size"
+				" 0x%x\n", tlb_i, tlb[tlb_i].ul_gpp_pa,
+				tlb[tlb_i].ul_dsp_va, tlb[tlb_i].ul_size);
 
-			iommu_kmap(mmu, e->ul_dsp_va, e->ul_gpp_pa, e->ul_size,
+			da = iommu_kmap(mmu, tlb[tlb_i].ul_dsp_va,
+				tlb[tlb_i].ul_gpp_pa, PAGE_SIZE,
 				IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_32);
-			itmp_entry_ndx++;
+			if (IS_ERR_VALUE(da)) {
+				status = (int)da;
+				break;
+			}
+		}
+	}
+	if (!status) {
+		u32 da;
+		l4_i = 0;
+		while (l4_peripheral_table[l4_i].phys_addr) {
+			da = iommu_kmap(mmu, l4_peripheral_table[l4_i].
+				dsp_virt_addr, l4_peripheral_table[l4_i].
+				phys_addr, PAGE_SIZE,
+				IOVMF_ENDIAN_LITTLE | IOVMF_ELSZ_32);
+			if (IS_ERR_VALUE(da)) {
+				status = (int)da;
+				break;
+			}
+			l4_i++;
 		}
 	}
 
@@ -562,11 +607,23 @@ static int bridge_brd_start(struct bridge_dev_context *dev_ctxt,
 
 			/* update board state */
 			dev_context->dw_brd_state = BRD_RUNNING;
-			/* (void)chnlsm_enable_interrupt(dev_context); */
+			return 0;
 		} else {
 			dev_context->dw_brd_state = BRD_UNKNOWN;
 		}
 	}
+
+	while (tlb_i--) {
+		if (!tlb[tlb_i].ul_gpp_pa)
+			continue;
+		iommu_kunmap(mmu, tlb[tlb_i].ul_gpp_va);
+	}
+	while (l4_i--)
+		iommu_kunmap(mmu, l4_peripheral_table[l4_i].dsp_virt_addr);
+	if (sg0_da)
+		iommu_kunmap(mmu, sg0_da);
+	if (sg1_da)
+		iommu_kunmap(mmu, sg1_da);
 	return status;
 }
 
@@ -584,6 +641,8 @@ static int bridge_brd_stop(struct bridge_dev_context *dev_ctxt)
 	struct bridge_dev_context *dev_context = dev_ctxt;
 	struct pg_table_attrs *pt_attrs;
 	u32 dsp_pwr_state;
+	int i;
+	struct bridge_ioctl_extproc *tlb = dev_context->atlb_entry;
 	struct omap_dsp_platform_data *pdata =
 		omap_dspbridge_dev->dev.platform_data;
 
@@ -627,17 +686,37 @@ static int bridge_brd_stop(struct bridge_dev_context *dev_ctxt)
 		memset((u8 *) pt_attrs->pg_info, 0x00,
 		       (pt_attrs->l2_num_pages * sizeof(struct page_info)));
 	}
+	/* Reset DSP */
+	(*pdata->dsp_prm_rmw_bits)(OMAP3430_RST1_IVA2_MASK,
+		OMAP3430_RST1_IVA2_MASK, OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
+
 	/* Disable the mailbox interrupts */
 	if (dev_context->mbox) {
 		omap_mbox_disable_irq(dev_context->mbox, IRQ_RX);
 		omap_mbox_put(dev_context->mbox);
 		dev_context->mbox = NULL;
 	}
-	if (dev_context->dsp_mmu)
-		dev_context->dsp_mmu = (iommu_put(dev_context->dsp_mmu), NULL);
-	/* Reset IVA2 clocks*/
-	(*pdata->dsp_prm_write)(OMAP3430_RST1_IVA2_MASK | OMAP3430_RST2_IVA2_MASK |
-			OMAP3430_RST3_IVA2_MASK, OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
+	if (dev_context->dsp_mmu) {
+		pr_err("Proc stop mmu if statement\n");
+		for (i = 0; i < BRDIOCTL_NUMOFMMUTLB; i++) {
+			if (!tlb[i].ul_gpp_pa)
+				continue;
+			iommu_kunmap(dev_context->dsp_mmu, tlb[i].ul_gpp_va);
+		}
+		i = 0;
+		while (l4_peripheral_table[i].phys_addr) {
+			iommu_kunmap(dev_context->dsp_mmu,
+				l4_peripheral_table[i].dsp_virt_addr);
+			i++;
+		}
+		iommu_kunmap(dev_context->dsp_mmu, dev_context->sh_s.seg0_da);
+		iommu_kunmap(dev_context->dsp_mmu, dev_context->sh_s.seg1_da);
+		iommu_put(dev_context->dsp_mmu);
+		dev_context->dsp_mmu = NULL;
+	}
+	/* Reset IVA IOMMU*/
+	(*pdata->dsp_prm_rmw_bits)(OMAP3430_RST2_IVA2_MASK,
+		OMAP3430_RST2_IVA2_MASK, OMAP3430_IVA2_MOD, OMAP2_RM_RSTCTRL);
 
 	dsp_clock_disable_all(dev_context->dsp_per_clks);
 	dsp_clk_disable(DSP_CLK_IVA2);