3 files changed, 31 insertions, 14 deletions

diff --git a/arch/arm/mach-omap2/mcbsp.c b/arch/arm/mach-omap2/mcbsp.c
index d6cce00a730b..92bd5e22a24e 100644
--- a/arch/arm/mach-omap2/mcbsp.c
+++ b/arch/arm/mach-omap2/mcbsp.c
@@ -27,6 +27,13 @@
 
 #include "control.h"
 
+/*
+ * FIXME: Find a mechanism to enable/disable runtime the McBSP ICLK autoidle.
+ * Sidetone needs non-gated ICLK and sidetone autoidle is broken.
+ */
+#include "cm2xxx_3xxx.h"
+#include "cm-regbits-34xx.h"
+
 /* McBSP internal signal muxing functions */
 
 void omap2_mcbsp1_mux_clkr_src(u8 mux)
@@ -102,6 +109,24 @@ int omap2_mcbsp_set_clks_src(u8 id, u8 fck_src_id)
 }
 EXPORT_SYMBOL(omap2_mcbsp_set_clks_src);
 
+static int omap3_enable_st_clock(unsigned int id, bool enable)
+{
+        unsigned int w;
+
+        /*
+         * Sidetone uses McBSP ICLK - which must not idle when sidetones
+         * are enabled or sidetones start sounding ugly.
+         */
+        w = omap2_cm_read_mod_reg(OMAP3430_PER_MOD, CM_AUTOIDLE);
+        if (enable)
+                w &= ~(1 << (id - 2));
+        else
+                w |= 1 << (id - 2);
+        omap2_cm_write_mod_reg(w, OMAP3430_PER_MOD, CM_AUTOIDLE);
+
+        return 0;
+}
+
 struct omap_device_pm_latency omap2_mcbsp_latency[] = {
         {
                 .deactivate_func = omap_device_idle_hwmods,
@@ -151,6 +176,7 @@ static int omap_init_mcbsp(struct omap_hwmod *oh, void *unused)
         if (oh->dev_attr) {
                 oh_device[1] = omap_hwmod_lookup((
                 (struct omap_mcbsp_dev_attr *)(oh->dev_attr))->sidetone);
+                pdata->enable_st_clock = omap3_enable_st_clock;
                 count++;
         }
         pdev = omap_device_build_ss(name, id, oh_device, count, pdata,
diff --git a/arch/arm/plat-omap/include/plat/mcbsp.h b/arch/arm/plat-omap/include/plat/mcbsp.h
index 648344a89a94..e451a6e8b065 100644
--- a/arch/arm/plat-omap/include/plat/mcbsp.h
+++ b/arch/arm/plat-omap/include/plat/mcbsp.h
@@ -321,6 +321,7 @@ struct omap_mcbsp_platform_data {
         /* McBSP platform and instance specific features */
         bool has_wakeup; /* Wakeup capability */
         bool has_ccr; /* Transceiver has configuration control registers */
+        int (*enable_st_clock)(unsigned int, bool);
 };
 
 struct omap_mcbsp_st_data {
diff --git a/arch/arm/plat-omap/mcbsp.c b/arch/arm/plat-omap/mcbsp.c
index 3ad536ea6c37..e96d747fbd80 100644
--- a/arch/arm/plat-omap/mcbsp.c
+++ b/arch/arm/plat-omap/mcbsp.c
@@ -26,10 +26,6 @@
 #include <plat/mcbsp.h>
 #include <linux/pm_runtime.h>
 
-/* XXX These "sideways" includes are a sign that something is wrong */
-#include "../mach-omap2/cm2xxx_3xxx.h"
-#include "../mach-omap2/cm-regbits-34xx.h"
-
 struct omap_mcbsp **mcbsp_ptr;
 int omap_mcbsp_count;
 
@@ -257,13 +253,8 @@ static void omap_st_on(struct omap_mcbsp *mcbsp)
 {
         unsigned int w;
 
-        /*
-         * Sidetone uses McBSP ICLK - which must not idle when sidetones
-         * are enabled or sidetones start sounding ugly.
-         */
-        w = omap2_cm_read_mod_reg(OMAP3430_PER_MOD, CM_AUTOIDLE);
-        w &= ~(1 << (mcbsp->id - 2));
-        omap2_cm_write_mod_reg(w, OMAP3430_PER_MOD, CM_AUTOIDLE);
+        if (mcbsp->pdata->enable_st_clock)
+                mcbsp->pdata->enable_st_clock(mcbsp->id, 1);
 
         /* Enable McBSP Sidetone */
         w = MCBSP_READ(mcbsp, SSELCR);
@@ -284,9 +275,8 @@ static void omap_st_off(struct omap_mcbsp *mcbsp)
         w = MCBSP_READ(mcbsp, SSELCR);
         MCBSP_WRITE(mcbsp, SSELCR, w & ~(SIDETONEEN));
 
-        w = omap2_cm_read_mod_reg(OMAP3430_PER_MOD, CM_AUTOIDLE);
-        w |= 1 << (mcbsp->id - 2);
-        omap2_cm_write_mod_reg(w, OMAP3430_PER_MOD, CM_AUTOIDLE);
+        if (mcbsp->pdata->enable_st_clock)
+                mcbsp->pdata->enable_st_clock(mcbsp->id, 0);
 }
 
 static void omap_st_fir_write(struct omap_mcbsp *mcbsp, s16 *fir)