Fix I2S emulation on pxa2xx-ssp.

Signed-off-by: Daniel Ribeiro <wyrm@openezx.org>

Index: linux-2.6.25.3/sound/soc/pxa/pxa2xx-ssp.c
===================================================================
--- linux-2.6.25.3.orig/sound/soc/pxa/pxa2xx-ssp.c
+++ linux-2.6.25.3/sound/soc/pxa/pxa2xx-ssp.c
@@ -392,42 +392,13 @@
 		unsigned int fmt)
 {
 	void __iomem *mmio_base = ssp[cpu_dai->id].ssp->mmio_base;
-	int val;
+	int val, tmp;
 
 	/* reset port settings */
 	__raw_writel(0, mmio_base + SSCR0);
 	__raw_writel(0, mmio_base + SSCR1);
 	__raw_writel(0, mmio_base + SSPSP);
 
-	/* NOTE: I2S emulation is still very much work in progress here */
-
-	/* FIXME: this is what wince uses for msb */
-	if ((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_MSB) {
-		__raw_writel(SSCR0_EDSS | SSCR0_TISSP | SSCR0_DataSize(16),
-				mmio_base + SSCR0);
-		goto master;
-	}
-
-	/* check for I2S emulation mode - handle it separately  */
-	if ((fmt & SND_SOC_DAIFMT_FORMAT_MASK) == SND_SOC_DAIFMT_I2S) {
-		/* 8.4.11 */
-
-		/* Only SSCR0[NCS] or SSCR0[ECS] bit fields settings are optional */
-		__raw_writel(SSCR0_EDSS | SSCR0_PSP | SSCR0_DataSize(16),
-				mmio_base + SSCR0);
-
-		/* set FIFO thresholds */
-		__raw_writel(SSCR1_RxTresh(14) | SSCR1_TxTresh(1),
-				mmio_base + SSCR1);
-
-		/* normal: */
-		/* all bit fields must be cleared except: FSRT = 1 and
-		 * SFRMWDTH = 16, DMYSTART=0,1) */
-		__raw_writel(SSPSP_FSRT | SSPSP_SFRMWDTH(16) | SSPSP_DMYSTRT(0),
-				mmio_base + SSPSP);
-		goto master;
-	}
-
 	val = __raw_readl(mmio_base + SSCR0) | SSCR0_PSP;
 	__raw_writel(val, mmio_base + SSCR0);
 	__raw_writel(SSCR1_RxTresh(14) | SSCR1_TxTresh(1) |
@@ -465,15 +436,20 @@
 	}
 
 	val = __raw_readl(mmio_base + SSPSP);
+	val |= SSPSP_SCMODE(2);
 	switch (fmt & SND_SOC_DAIFMT_FORMAT_MASK) {
 	case SND_SOC_DAIFMT_DSP_A:
 		val |= SSPSP_DMYSTRT(1);
 	case SND_SOC_DAIFMT_DSP_B:
-		val |= SSPSP_SCMODE(2);
 		break;
 	case SND_SOC_DAIFMT_I2S:
+		val |= SSPSP_FSRT;
 	case SND_SOC_DAIFMT_MSB:
-		/* handled above */
+		val |= SSPSP_SFRMWDTH(16);
+
+		tmp = __raw_readl(mmio_base + SSCR0) | SSCR0_EDSS |
+			SSCR0_DataSize(16);
+		__raw_writel(tmp, mmio_base + SSCR0);
 		break;
 	default:
 		return -EINVAL;
@@ -505,12 +481,20 @@
 
 	pr_debug("pxa2xx_ssp_hw_params: dma %d", dma);
 
+	sscr0 = __raw_readl(mmio_base + SSCR0);
+
 	/* we can only change the settings if the port is not in use */
-	if (__raw_readl(mmio_base + SSCR0) & SSCR0_SSE)
-		return 0;
+	if (sscr0 & SSCR0_SSE)
+		goto ret;
+
+	/* check if we are running on I2S mode */
+	/* FIXME: Is there a better way to check this? */
+	if ((sscr0 & (SSCR0_EDSS | SSCR0_DataSize(16) | SSCR0_PSP)) ==
+			(SSCR0_EDSS | SSCR0_DataSize(16) | SSCR0_PSP))
+		goto ret;
 
 	/* clear selected SSP bits */
-	sscr0 = __raw_readl(mmio_base + SSCR0) & ~(SSCR0_DSS | SSCR0_EDSS);
+	sscr0 &= ~(SSCR0_DSS | SSCR0_EDSS);
 	__raw_writel(sscr0, mmio_base + SSCR0);
 
 	/* bit size */
@@ -530,6 +514,7 @@
 	}
 	__raw_writel(sscr0, mmio_base + SSCR0);
 
+ret:
 	pr_debug("SSCR0 0x%08x SSCR1 0x%08x SSTO 0x%08x SSPSP 0x%08x SSSR 0x%08x SSACD 0x%08x",
 		__raw_readl(mmio_base + SSCR0), __raw_readl(mmio_base + SSCR1),
 		__raw_readl(mmio_base + SSTO), __raw_readl(mmio_base + SSPSP),