#include <plat/dma.h>
 #include <plat/cpu.h>
 #include <plat/mcbsp.h>
+#include <plat/control.h>
 
+/* McBSP internal signal muxing functions */
+
+void omap2_mcbsp1_mux_clkr_src(u8 mux)
+{
+       u32 v;
+
+       v = omap_ctrl_readl(OMAP2_CONTROL_DEVCONF0);
+       if (mux == CLKR_SRC_CLKR)
+               v &= OMAP2_MCBSP1_CLKR_MASK;
+       else if (mux == CLKR_SRC_CLKX)
+               v |= OMAP2_MCBSP1_CLKR_MASK;
+       omap_ctrl_writel(v, OMAP2_CONTROL_DEVCONF0);
+}
+EXPORT_SYMBOL(omap2_mcbsp1_mux_clkr_src);
+
+void omap2_mcbsp1_mux_fsr_src(u8 mux)
+{
+       u32 v;
+
+       v = omap_ctrl_readl(OMAP2_CONTROL_DEVCONF0);
+       if (mux == FSR_SRC_FSR)
+               v &= OMAP2_MCBSP1_FSR_MASK;
+       else if (mux == FSR_SRC_FSX)
+               v |= OMAP2_MCBSP1_FSR_MASK;
+       omap_ctrl_writel(v, OMAP2_CONTROL_DEVCONF0);
+}
+EXPORT_SYMBOL(omap2_mcbsp1_mux_fsr_src);
+
+/* Platform data */
 
 #ifdef CONFIG_ARCH_OMAP2420
 static struct omap_mcbsp_platform_data omap2420_mcbsp_pdata[] = {
 
 #define OMAP2_MMCSDIO1ADPCLKISEL       (1 << 24) /* MMC1 loop back clock */
 #define OMAP24XX_USBSTANDBYCTRL                (1 << 15)
 #define OMAP2_MCBSP2_CLKS_MASK         (1 << 6)
+#define OMAP2_MCBSP1_FSR_MASK          (1 << 4)
+#define OMAP2_MCBSP1_CLKR_MASK         (1 << 3)
 #define OMAP2_MCBSP1_CLKS_MASK         (1 << 2)
 
 /* CONTROL_DEVCONF1 bits */
 
 #define RFSREN                 0x0002
 #define RSYNCERREN             0x0001
 
+/* CLKR signal muxing options */
+#define CLKR_SRC_CLKR          0
+#define CLKR_SRC_CLKX          1
+
+/* FSR signal muxing options */
+#define FSR_SRC_FSR            0
+#define FSR_SRC_FSX            1
+
 /* we don't do multichannel for now */
 struct omap_mcbsp_reg_cfg {
        u16 spcr2;
 int omap_mcbsp_spi_master_xmit_word_poll(unsigned int id, u32 word);
 int omap_mcbsp_spi_master_recv_word_poll(unsigned int id, u32 * word);
 
-
 /* SPI specific API */
 void omap_mcbsp_set_spi_mode(unsigned int id, const struct omap_mcbsp_spi_cfg * spi_cfg);
 
 int omap_mcbsp_pollwrite(unsigned int id, u16 buf);
 int omap_mcbsp_set_io_type(unsigned int id, omap_mcbsp_io_type_t io_type);
 
+/* McBSP signal muxing API */
+void omap2_mcbsp1_mux_clkr_src(u8 mux);
+void omap2_mcbsp1_mux_fsr_src(u8 mux);
+
 #ifdef CONFIG_ARCH_OMAP3
 /* Sidetone specific API */
 int omap_st_set_chgain(unsigned int id, int channel, s16 chgain);
 
 
 #include <plat/dma.h>
 #include <plat/mcbsp.h>
+#include <plat/control.h>
 
 #include "../mach-omap2/cm-regbits-34xx.h"
 
 
        return 0;
 }
 
-static int omap_mcbsp_dai_set_rcvr_src(struct omap_mcbsp_data *mcbsp_data,
-                                      int clk_id)
-{
-       int sel_bit, set = 0;
-       u16 reg = OMAP2_CONTROL_DEVCONF0;
-
-       if (cpu_class_is_omap1())
-               return -EINVAL; /* TODO: Can this be implemented for OMAP1? */
-       if (mcbsp_data->bus_id != 0)
-               return -EINVAL;
-
-       switch (clk_id) {
-       case OMAP_MCBSP_CLKR_SRC_CLKX:
-               set = 1;
-       case OMAP_MCBSP_CLKR_SRC_CLKR:
-               sel_bit = 3;
-               break;
-       case OMAP_MCBSP_FSR_SRC_FSX:
-               set = 1;
-       case OMAP_MCBSP_FSR_SRC_FSR:
-               sel_bit = 4;
-               break;
-       default:
-               return -EINVAL;
-       }
-
-       if (set)
-               omap_ctrl_writel(omap_ctrl_readl(reg) | (1 << sel_bit), reg);
-       else
-               omap_ctrl_writel(omap_ctrl_readl(reg) & ~(1 << sel_bit), reg);
-
-       return 0;
-}
-
 static int omap_mcbsp_dai_set_dai_sysclk(struct snd_soc_dai *cpu_dai,
                                         int clk_id, unsigned int freq,
                                         int dir)
 {
        struct omap_mcbsp_data *mcbsp_data = to_mcbsp(cpu_dai->private_data);
        struct omap_mcbsp_reg_cfg *regs = &mcbsp_data->regs;
+       struct omap_mcbsp_platform_data *pdata = cpu_dai->dev->platform_data;
        int err = 0;
 
+       /* The McBSP signal muxing functions are only available on McBSP1 */
+       if (clk_id == OMAP_MCBSP_CLKR_SRC_CLKR ||
+           clk_id == OMAP_MCBSP_CLKR_SRC_CLKX ||
+           clk_id == OMAP_MCBSP_FSR_SRC_FSR ||
+           clk_id == OMAP_MCBSP_FSR_SRC_FSX)
+               if (cpu_class_is_omap1() || mcbsp_data->bus_id != 0)
+                       return -EINVAL;
+
        mcbsp_data->in_freq = freq;
 
        switch (clk_id) {
                regs->pcr0      |= SCLKME;
                break;
 
+
        case OMAP_MCBSP_CLKR_SRC_CLKR:
+               omap2_mcbsp1_mux_clkr_src(CLKR_SRC_CLKR);
+               break;
        case OMAP_MCBSP_CLKR_SRC_CLKX:
+               omap2_mcbsp1_mux_clkr_src(CLKR_SRC_CLKX);
+               break;
        case OMAP_MCBSP_FSR_SRC_FSR:
+               omap2_mcbsp1_mux_fsr_src(FSR_SRC_FSR);
+               break;
        case OMAP_MCBSP_FSR_SRC_FSX:
-               err = omap_mcbsp_dai_set_rcvr_src(mcbsp_data, clk_id);
+               omap2_mcbsp1_mux_fsr_src(FSR_SRC_FSX);
                break;
        default:
                err = -ENODEV;