McBSP cleanup and generalization

index 455eadc..2a3587e 100644
@@ -969,6 +969,33 @@ void omap_mcbsp_stop(unsigned int id, int tx, int rx)
 }
 EXPORT_SYMBOL(omap_mcbsp_stop);
 
+/*
+ * The following functions are only required on an OMAP1-only build.
+ * mach-omap2/mcbsp.c contains the real functions
+ */
+#ifndef CONFIG_ARCH_OMAP2PLUS
+int omap2_mcbsp_set_clks_src(u8 id, u8 fck_src_id)
+{
+	WARN(1, "%s: should never be called on an OMAP1-only kernel\n",
+	     __func__);
+	return -EINVAL;
+}
+
+void omap2_mcbsp1_mux_clkr_src(u8 mux)
+{
+	WARN(1, "%s: should never be called on an OMAP1-only kernel\n",
+	     __func__);
+	return;
+}
+
+void omap2_mcbsp1_mux_fsr_src(u8 mux)
+{
+	WARN(1, "%s: should never be called on an OMAP1-only kernel\n",
+	     __func__);
+	return;
+}
+#endif
+
 #ifdef CONFIG_ARCH_OMAP3
 #define max_thres(m)			(mcbsp->pdata->buffer_size)
 #define valid_threshold(m, val)		((val) <= max_thres(m))

index 6c53508..63464ad 100644
@@ -385,8 +385,6 @@ struct omap_mcbsp {
 	void __iomem *io_base;
 	u8 id;
 	u8 free;
-	omap_mcbsp_word_length rx_word_length;
-	omap_mcbsp_word_length tx_word_length;
 
 	int rx_irq;
 	int tx_irq;

index 2a3587e..6c62af1 100644
@@ -869,9 +869,6 @@ void omap_mcbsp_start(unsigned int id, int tx, int rx)
 	if (cpu_is_omap34xx())
 		omap_st_start(mcbsp);
 
-	mcbsp->rx_word_length = (MCBSP_READ_CACHE(mcbsp, RCR1) >> 5) & 0x7;
-	mcbsp->tx_word_length = (MCBSP_READ_CACHE(mcbsp, XCR1) >> 5) & 0x7;
-
 	/* Only enable SRG, if McBSP is master */
 	w = MCBSP_READ_CACHE(mcbsp, PCR0);
 	if (w & (FSXM | FSRM | CLKXM | CLKRM))

index d9af981..ab7395d 100644
@@ -38,7 +38,7 @@ static void omap1_mcbsp_request(unsigned int id)
 	 * On 1510, 1610 and 1710, McBSP1 and McBSP3
 	 * are DSP public peripherals.
 	 */
-	if (id == OMAP_MCBSP1 || id == OMAP_MCBSP3) {
+	if (id == 0 || id == 2) {
 		if (dsp_use++ == 0) {
 			api_clk = clk_get(NULL, "api_ck");
 			dsp_clk = clk_get(NULL, "dsp_ck");

@@ -59,7 +59,7 @@ static void omap1_mcbsp_request(unsigned int id)
 
 static void omap1_mcbsp_free(unsigned int id)
 {
-	if (id == OMAP_MCBSP1 || id == OMAP_MCBSP3) {
+	if (id == 0 || id == 2) {
 		if (--dsp_use == 0) {
 			if (!IS_ERR(api_clk)) {
 				clk_disable(api_clk);

index 63464ad..9882c65 100644
@@ -33,7 +33,7 @@
 #define OMAP_MCBSP_PLATFORM_DEVICE(port_nr)		\
 static struct platform_device omap_mcbsp##port_nr = {	\
 	.name	= "omap-mcbsp-dai",			\
-	.id	= OMAP_MCBSP##port_nr,			\
+	.id	= port_nr - 1,			\
 }
 
 #define MCBSP_CONFIG_TYPE2	0x2

@@ -332,14 +332,6 @@ struct omap_mcbsp_reg_cfg {
 };
 
 typedef enum {
-	OMAP_MCBSP1 = 0,
-	OMAP_MCBSP2,
-	OMAP_MCBSP3,
-	OMAP_MCBSP4,
-	OMAP_MCBSP5
-} omap_mcbsp_id;
-
-typedef enum {
 	OMAP_MCBSP_WORD_8 = 0,
 	OMAP_MCBSP_WORD_12,
 	OMAP_MCBSP_WORD_16,

index 9882c65..6db02b3 100644
@@ -51,54 +51,16 @@ static struct platform_device omap_mcbsp##port_nr = {	\
 #define OMAP1610_MCBSP2_BASE	0xfffb1000
 #define OMAP1610_MCBSP3_BASE	0xe1017000
 
-#ifdef CONFIG_ARCH_OMAP1
-
-#define OMAP_MCBSP_REG_DRR2	0x00
-#define OMAP_MCBSP_REG_DRR1	0x02
-#define OMAP_MCBSP_REG_DXR2	0x04
-#define OMAP_MCBSP_REG_DXR1	0x06
-#define OMAP_MCBSP_REG_DRR	0x02
-#define OMAP_MCBSP_REG_DXR	0x06
-#define OMAP_MCBSP_REG_SPCR2	0x08
-#define OMAP_MCBSP_REG_SPCR1	0x0a
-#define OMAP_MCBSP_REG_RCR2	0x0c
-#define OMAP_MCBSP_REG_RCR1	0x0e
-#define OMAP_MCBSP_REG_XCR2	0x10
-#define OMAP_MCBSP_REG_XCR1	0x12
-#define OMAP_MCBSP_REG_SRGR2	0x14
-#define OMAP_MCBSP_REG_SRGR1	0x16
-#define OMAP_MCBSP_REG_MCR2	0x18
-#define OMAP_MCBSP_REG_MCR1	0x1a
-#define OMAP_MCBSP_REG_RCERA	0x1c
-#define OMAP_MCBSP_REG_RCERB	0x1e
-#define OMAP_MCBSP_REG_XCERA	0x20
-#define OMAP_MCBSP_REG_XCERB	0x22
-#define OMAP_MCBSP_REG_PCR0	0x24
-#define OMAP_MCBSP_REG_RCERC	0x26
-#define OMAP_MCBSP_REG_RCERD	0x28
-#define OMAP_MCBSP_REG_XCERC	0x2A
-#define OMAP_MCBSP_REG_XCERD	0x2C
-#define OMAP_MCBSP_REG_RCERE	0x2E
-#define OMAP_MCBSP_REG_RCERF	0x30
-#define OMAP_MCBSP_REG_XCERE	0x32
-#define OMAP_MCBSP_REG_XCERF	0x34
-#define OMAP_MCBSP_REG_RCERG	0x36
-#define OMAP_MCBSP_REG_RCERH	0x38
-#define OMAP_MCBSP_REG_XCERG	0x3A
-#define OMAP_MCBSP_REG_XCERH	0x3C
-
-/* Dummy defines, these are not available on omap1 */
-#define OMAP_MCBSP_REG_XCCR	0x00
-#define OMAP_MCBSP_REG_RCCR	0x00
-
-#else
-
+/* McBSP register addresses. OMAP1 address = address definition / 2 */
+/* OMAP1-OMAP2420 registers */
 #define OMAP_MCBSP_REG_DRR2	0x00
 #define OMAP_MCBSP_REG_DRR1	0x04
 #define OMAP_MCBSP_REG_DXR2	0x08
 #define OMAP_MCBSP_REG_DXR1	0x0C
+/* OMAP2430 and onwards */
 #define OMAP_MCBSP_REG_DRR	0x00
 #define OMAP_MCBSP_REG_DXR	0x08
+/* Common registers */
 #define OMAP_MCBSP_REG_SPCR2	0x10
 #define OMAP_MCBSP_REG_SPCR1	0x14
 #define OMAP_MCBSP_REG_RCR2	0x18

@@ -126,6 +88,7 @@ static struct platform_device omap_mcbsp##port_nr = {	\
 #define OMAP_MCBSP_REG_RCERH	0x70
 #define OMAP_MCBSP_REG_XCERG	0x74
 #define OMAP_MCBSP_REG_XCERH	0x78
+/* OMAP2430 and onwards */
 #define OMAP_MCBSP_REG_SYSCON	0x8C
 #define OMAP_MCBSP_REG_THRSH2	0x90
 #define OMAP_MCBSP_REG_THRSH1	0x94

@@ -138,6 +101,7 @@ static struct platform_device omap_mcbsp##port_nr = {	\
 #define OMAP_MCBSP_REG_RBUFFSTAT	0xB8
 #define OMAP_MCBSP_REG_SSELCR	0xBC
 
+/* OMAP3 sidetone control registers */
 #define OMAP_ST_REG_REV		0x00
 #define OMAP_ST_REG_SYSCONFIG	0x10
 #define OMAP_ST_REG_IRQSTATUS	0x18

@@ -146,8 +110,6 @@ static struct platform_device omap_mcbsp##port_nr = {	\
 #define OMAP_ST_REG_SFIRCR	0x28
 #define OMAP_ST_REG_SSELCR	0x2C
 
-#endif
-
 /************************** McBSP SPCR1 bit definitions ***********************/
 #define RRST			0x0001
 #define RRDY			0x0002

index 6c62af1..a7ced1b 100644
@@ -37,6 +37,7 @@ int omap_mcbsp_count, omap_mcbsp_cache_size;
 static void omap_mcbsp_write(struct omap_mcbsp *mcbsp, u16 reg, u32 val)
 {
 	if (cpu_class_is_omap1()) {
+		reg /= 2;
 		((u16 *)mcbsp->reg_cache)[reg / sizeof(u16)] = (u16)val;
 		__raw_writew((u16)val, mcbsp->io_base + reg);
 	} else if (cpu_is_omap2420()) {

@@ -51,6 +52,7 @@ static void omap_mcbsp_write(struct omap_mcbsp *mcbsp, u16 reg, u32 val)
 static int omap_mcbsp_read(struct omap_mcbsp *mcbsp, u16 reg, bool from_cache)
 {
 	if (cpu_class_is_omap1()) {
+		reg /= 2;
 		return !from_cache ? __raw_readw(mcbsp->io_base + reg) :
 				((u16 *)mcbsp->reg_cache)[reg / sizeof(u16)];
 	} else if (cpu_is_omap2420()) {

index ab7395d..3c985ac 100644
@@ -31,6 +31,7 @@
 static int dsp_use;
 static struct clk *api_clk;
 static struct clk *dsp_clk;
+static struct platform_device **omap_mcbsp_devices;
 
 static void omap1_mcbsp_request(unsigned int id)
 {

@@ -369,6 +370,37 @@ static struct omap_mcbsp_platform_data omap16xx_mcbsp_pdata[] = {
 #define OMAP16XX_MCBSP_COUNT		0
 #endif
 
+static void omap_mcbsp_register_board_cfg(struct resource *res, int res_count,
+			struct omap_mcbsp_platform_data *config, int size)
+{
+	int i;
+
+	omap_mcbsp_devices = kzalloc(size * sizeof(struct platform_device *),
+				     GFP_KERNEL);
+	if (!omap_mcbsp_devices) {
+		printk(KERN_ERR "Could not register McBSP devices\n");
+		return;
+	}
+
+	for (i = 0; i < size; i++) {
+		struct platform_device *new_mcbsp;
+		int ret;
+
+		new_mcbsp = platform_device_alloc("omap-mcbsp", i + 1);
+		if (!new_mcbsp)
+			continue;
+		platform_device_add_resources(new_mcbsp, &res[i * res_count],
+					res_count);
+		new_mcbsp->dev.platform_data = &config[i];
+		ret = platform_device_add(new_mcbsp);
+		if (ret) {
+			platform_device_put(new_mcbsp);
+			continue;
+		}
+		omap_mcbsp_devices[i] = new_mcbsp;
+	}
+}
+
 static int __init omap1_mcbsp_init(void)
 {
 	if (!cpu_class_is_omap1())

54 lines not on this page · raw message

index 6db02b3..2202457 100644
@@ -376,8 +376,6 @@ extern int omap_mcbsp_count, omap_mcbsp_cache_size;
 #define id_to_mcbsp_ptr(id)		mcbsp_ptr[id];
 
 int omap_mcbsp_init(void);
-void omap_mcbsp_register_board_cfg(struct resource *res, int res_count,
-			struct omap_mcbsp_platform_data *config, int size);
 void omap_mcbsp_config(unsigned int id, const struct omap_mcbsp_reg_cfg * config);
 #ifdef CONFIG_ARCH_OMAP3
 void omap_mcbsp_set_tx_threshold(unsigned int id, u16 threshold);

index 3c985ac..36ab5d8 100644
@@ -391,6 +391,8 @@ static void omap_mcbsp_register_board_cfg(struct resource *res, int res_count,
 			continue;
 		platform_device_add_resources(new_mcbsp, &res[i * res_count],
 					res_count);
+		config[i].reg_size = 2;
+		config[i].reg_step = 2;
 		new_mcbsp->dev.platform_data = &config[i];
 		ret = platform_device_add(new_mcbsp);
 		if (ret) {

index 4a6ef6a..1408156 100644
@@ -137,6 +137,13 @@ static int omap_init_mcbsp(struct omap_hwmod *oh, void *unused)
 			pdata->buffer_size = 0x80;
 	}
 
+	pdata->reg_step = 4;
+	if (oh->class->rev < MCBSP_CONFIG_TYPE2)
+		pdata->reg_size = 2;
+	else
+		pdata->reg_size = 4;
+
+
 	oh_device[0] = oh;
 
 	if (oh->dev_attr) {

index 2202457..31b24c9 100644
@@ -320,6 +320,8 @@ struct omap_mcbsp_platform_data {
 #endif
 	u16 buffer_size;
 	unsigned int mcbsp_config_type;
+	u8 reg_size;
+	u8 reg_step;
 };
 
 struct omap_mcbsp_st_data {

index a7ced1b..92d6d4f 100644
@@ -36,31 +36,35 @@ int omap_mcbsp_count, omap_mcbsp_cache_size;
 
 static void omap_mcbsp_write(struct omap_mcbsp *mcbsp, u16 reg, u32 val)
 {
-	if (cpu_class_is_omap1()) {
-		reg /= 2;
-		((u16 *)mcbsp->reg_cache)[reg / sizeof(u16)] = (u16)val;
-		__raw_writew((u16)val, mcbsp->io_base + reg);
-	} else if (cpu_is_omap2420()) {
-		((u16 *)mcbsp->reg_cache)[reg / sizeof(u32)] = (u16)val;
+	int index;
+
+	if (mcbsp->pdata->reg_size == 2 && mcbsp->pdata->reg_step == 2)
+		reg /= 2; /* Calculate OMAP1 register offset */
+
+	index = reg / mcbsp->pdata->reg_step;
+	if (mcbsp->pdata->reg_size == 2) {
+		((u16 *)mcbsp->reg_cache)[index] = (u16)val;
 		__raw_writew((u16)val, mcbsp->io_base + reg);
 	} else {
-		((u32 *)mcbsp->reg_cache)[reg / sizeof(u32)] = val;
+		((u32 *)mcbsp->reg_cache)[index] = val;
 		__raw_writel(val, mcbsp->io_base + reg);
 	}
 }
 
 static int omap_mcbsp_read(struct omap_mcbsp *mcbsp, u16 reg, bool from_cache)
 {
-	if (cpu_class_is_omap1()) {
-		reg /= 2;
-		return !from_cache ? __raw_readw(mcbsp->io_base + reg) :
-				((u16 *)mcbsp->reg_cache)[reg / sizeof(u16)];
-	} else if (cpu_is_omap2420()) {
+	int index;
+
+	if (mcbsp->pdata->reg_size == 2 && mcbsp->pdata->reg_step == 2)
+		reg /= 2; /* Calculate OMAP1 register offset */
+
+	index = reg / mcbsp->pdata->reg_step;
+	if (mcbsp->pdata->reg_size == 2) {
 		return !from_cache ? __raw_readw(mcbsp->io_base + reg) :
-				((u16 *)mcbsp->reg_cache)[reg / sizeof(u32)];
+				     ((u16 *)mcbsp->reg_cache)[index];
 	} else {
 		return !from_cache ? __raw_readl(mcbsp->io_base + reg) :
-				((u32 *)mcbsp->reg_cache)[reg / sizeof(u32)];
+				     ((u32 *)mcbsp->reg_cache)[index];
 	}
 }
 

index 92d6d4f..47720b1 100644
@@ -193,7 +193,7 @@ void omap_mcbsp_config(unsigned int id, const struct omap_mcbsp_reg_cfg *config)
 	MCBSP_WRITE(mcbsp, MCR2, config->mcr2);
 	MCBSP_WRITE(mcbsp, MCR1, config->mcr1);
 	MCBSP_WRITE(mcbsp, PCR0, config->pcr0);
-	if (cpu_is_omap2430() || cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE2) {
 		MCBSP_WRITE(mcbsp, XCCR, config->xccr);
 		MCBSP_WRITE(mcbsp, RCCR, config->rccr);
 	}

@@ -531,14 +531,13 @@ void omap_mcbsp_set_tx_threshold(unsigned int id, u16 threshold)
 {
 	struct omap_mcbsp *mcbsp;
 
-	if (!cpu_is_omap34xx() && !cpu_is_omap44xx())
-		return;
-
 	if (!omap_mcbsp_check_valid_id(id)) {
 		printk(KERN_ERR "%s: Invalid id (%d)\n", __func__, id + 1);
 		return;
 	}
 	mcbsp = id_to_mcbsp_ptr(id);
+	if (mcbsp->mcbsp_config_type < MCBSP_CONFIG_TYPE3)
+		return;
 
 	if (threshold && threshold <= mcbsp->max_tx_thres)
 		MCBSP_WRITE(mcbsp, THRSH2, threshold - 1);

@@ -554,14 +553,13 @@ void omap_mcbsp_set_rx_threshold(unsigned int id, u16 threshold)
 {
 	struct omap_mcbsp *mcbsp;
 
-	if (!cpu_is_omap34xx() && !cpu_is_omap44xx())
-		return;
-
 	if (!omap_mcbsp_check_valid_id(id)) {
 		printk(KERN_ERR "%s: Invalid id (%d)\n", __func__, id + 1);
 		return;
 	}
 	mcbsp = id_to_mcbsp_ptr(id);
+	if (mcbsp->mcbsp_config_type < MCBSP_CONFIG_TYPE3)
+		return;
 
 	if (threshold && threshold <= mcbsp->max_rx_thres)
 		MCBSP_WRITE(mcbsp, THRSH1, threshold - 1);

@@ -698,7 +696,7 @@ static inline void omap34xx_mcbsp_request(struct omap_mcbsp *mcbsp)
 	 * Enable wakup behavior, smart idle and all wakeups
 	 * REVISIT: some wakeups may be unnecessary
 	 */
-	if (cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3) {
 		MCBSP_WRITE(mcbsp, WAKEUPEN, XRDYEN | RRDYEN);
 	}
 }

@@ -712,7 +710,7 @@ static inline void omap34xx_mcbsp_free(struct omap_mcbsp *mcbsp)
 	/*
 	 * Disable wakup behavior, smart idle and all wakeups
 	 */
-	if (cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3) {
 		/*
 		 * HW bug workaround - If no_idle mode is taken, we need to
 		 * go to smart_idle before going to always_idle, or the

@@ -872,7 +870,7 @@ void omap_mcbsp_start(unsigned int id, int tx, int rx)
 	}
 	mcbsp = id_to_mcbsp_ptr(id);
 
-	if (cpu_is_omap34xx())
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3)
 		omap_st_start(mcbsp);
 
 	/* Only enable SRG, if McBSP is master */

@@ -910,7 +908,7 @@ void omap_mcbsp_start(unsigned int id, int tx, int rx)
 		MCBSP_WRITE(mcbsp, SPCR2, w | (1 << 7));
 	}
 
-	if (cpu_is_omap2430() || cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE2) {
 		/* Release the transmitter and receiver */
 		w = MCBSP_READ_CACHE(mcbsp, XCCR);
 		w &= ~(tx ? XDISABLE : 0);

@@ -940,7 +938,7 @@ void omap_mcbsp_stop(unsigned int id, int tx, int rx)
 
 	/* Reset transmitter */
 	tx &= 1;
-	if (cpu_is_omap2430() || cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE2) {
 		w = MCBSP_READ_CACHE(mcbsp, XCCR);
 		w |= (tx ? XDISABLE : 0);
 		MCBSP_WRITE(mcbsp, XCCR, w);

@@ -950,7 +948,7 @@ void omap_mcbsp_stop(unsigned int id, int tx, int rx)
 
 	/* Reset receiver */
 	rx &= 1;
-	if (cpu_is_omap2430() || cpu_is_omap34xx() || cpu_is_omap44xx()) {
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE2) {
 		w = MCBSP_READ_CACHE(mcbsp, RCCR);
 		w |= (rx ? RDISABLE : 0);
 		MCBSP_WRITE(mcbsp, RCCR, w);

@@ -967,7 +965,7 @@ void omap_mcbsp_stop(unsigned int id, int tx, int rx)
 		MCBSP_WRITE(mcbsp, SPCR2, w & ~(1 << 6));
 	}
 
-	if (cpu_is_omap34xx())
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3)
 		omap_st_stop(mcbsp);
 }
 EXPORT_SYMBOL(omap_mcbsp_stop);

@@ -1230,7 +1228,7 @@ static void __devexit omap_st_remove(struct omap_mcbsp *mcbsp)
 static inline void __devinit omap34xx_device_init(struct omap_mcbsp *mcbsp)
 {
 	mcbsp->dma_op_mode = MCBSP_DMA_MODE_ELEMENT;
-	if (cpu_is_omap34xx()) {
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3) {
 		/*
 		 * Initially configure the maximum thresholds to a safe value.
 		 * The McBSP FIFO usage with these values should not go under

@@ -1262,7 +1260,7 @@ static inline void __devinit omap34xx_device_init(struct omap_mcbsp *mcbsp)
 
 static inline void __devexit omap34xx_device_exit(struct omap_mcbsp *mcbsp)
 {
-	if (cpu_is_omap34xx()) {
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3) {
 		omap_additional_remove(mcbsp->dev);
 
 		if (mcbsp->id == 2 || mcbsp->id == 3)

index 47720b1..7511287 100644
@@ -24,7 +24,6 @@
 #include <linux/slab.h>
 
 #include <plat/mcbsp.h>
-#include <plat/omap_device.h>
 #include <linux/pm_runtime.h>
 
 /* XXX These "sideways" includes are a sign that something is wrong */

@@ -264,19 +263,9 @@ int omap_mcbsp_dma_reg_params(unsigned int id, unsigned int stream)
 EXPORT_SYMBOL(omap_mcbsp_dma_reg_params);
 
 #ifdef CONFIG_ARCH_OMAP3
-static struct omap_device *find_omap_device_by_dev(struct device *dev)
-{
-	struct platform_device *pdev = container_of(dev,
-					struct platform_device, dev);
-	return container_of(pdev, struct omap_device, pdev);
-}
-
 static void omap_st_on(struct omap_mcbsp *mcbsp)
 {
 	unsigned int w;
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
 
 	/*
 	 * Sidetone uses McBSP ICLK - which must not idle when sidetones

@@ -298,9 +287,6 @@ static void omap_st_on(struct omap_mcbsp *mcbsp)
 static void omap_st_off(struct omap_mcbsp *mcbsp)
 {
 	unsigned int w;
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
 
 	w = MCBSP_ST_READ(mcbsp, SSELCR);
 	MCBSP_ST_WRITE(mcbsp, SSELCR, w & ~(ST_SIDETONEEN));

@@ -316,9 +302,6 @@ static void omap_st_off(struct omap_mcbsp *mcbsp)
 static void omap_st_fir_write(struct omap_mcbsp *mcbsp, s16 *fir)
 {
 	u16 val, i;
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
 
 	val = MCBSP_ST_READ(mcbsp, SSELCR);
 

@@ -346,9 +329,6 @@ static void omap_st_chgain(struct omap_mcbsp *mcbsp)
 {
 	u16 w;
 	struct omap_mcbsp_st_data *st_data = mcbsp->st_data;
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
 
 	w = MCBSP_ST_READ(mcbsp, SSELCR);
 

@@ -689,9 +669,6 @@ EXPORT_SYMBOL(omap_mcbsp_get_dma_op_mode);
 
 static inline void omap34xx_mcbsp_request(struct omap_mcbsp *mcbsp)
 {
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
 	/*
 	 * Enable wakup behavior, smart idle and all wakeups
 	 * REVISIT: some wakeups may be unnecessary

@@ -703,10 +680,6 @@ static inline void omap34xx_mcbsp_request(struct omap_mcbsp *mcbsp)
 
 static inline void omap34xx_mcbsp_free(struct omap_mcbsp *mcbsp)
 {
-	struct omap_device *od;
-
-	od = find_omap_device_by_dev(mcbsp->dev);
-
 	/*
 	 * Disable wakup behavior, smart idle and all wakeups
 	 */

index 7511287..202292a 100644
@@ -667,35 +667,7 @@ int omap_mcbsp_get_dma_op_mode(unsigned int id)
 }
 EXPORT_SYMBOL(omap_mcbsp_get_dma_op_mode);
 
-static inline void omap34xx_mcbsp_request(struct omap_mcbsp *mcbsp)
-{
-	/*
-	 * Enable wakup behavior, smart idle and all wakeups
-	 * REVISIT: some wakeups may be unnecessary
-	 */
-	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3) {
-		MCBSP_WRITE(mcbsp, WAKEUPEN, XRDYEN | RRDYEN);
-	}
-}
-
-static inline void omap34xx_mcbsp_free(struct omap_mcbsp *mcbsp)
-{
-	/*
-	 * Disable wakup behavior, smart idle and all wakeups
-	 */
-	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3) {
-		/*
-		 * HW bug workaround - If no_idle mode is taken, we need to
-		 * go to smart_idle before going to always_idle, or the
-		 * device will not hit retention anymore.
-		 */
-
-		MCBSP_WRITE(mcbsp, WAKEUPEN, 0);
-	}
-}
 #else
-static inline void omap34xx_mcbsp_request(struct omap_mcbsp *mcbsp) {}
-static inline void omap34xx_mcbsp_free(struct omap_mcbsp *mcbsp) {}
 static inline void omap_st_start(struct omap_mcbsp *mcbsp) {}
 static inline void omap_st_stop(struct omap_mcbsp *mcbsp) {}
 #endif

@@ -734,8 +706,9 @@ int omap_mcbsp_request(unsigned int id)
 
 	pm_runtime_get_sync(mcbsp->dev);
 
-	/* Do procedure specific to omap34xx arch, if applicable */
-	omap34xx_mcbsp_request(mcbsp);
+	/* Enable wakup behavior */
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3)
+		MCBSP_WRITE(mcbsp, WAKEUPEN, XRDYEN | RRDYEN);
 
 	/*
 	 * Make sure that transmitter, receiver and sample-rate generator are

@@ -772,8 +745,9 @@ err_clk_disable:
 	if (mcbsp->pdata && mcbsp->pdata->ops && mcbsp->pdata->ops->free)
 		mcbsp->pdata->ops->free(id);
 
-	/* Do procedure specific to omap34xx arch, if applicable */
-	omap34xx_mcbsp_free(mcbsp);
+	/* Disable wakup behavior */
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3)
+		MCBSP_WRITE(mcbsp, WAKEUPEN, 0);
 
 	pm_runtime_put_sync(mcbsp->dev);
 

@@ -802,8 +776,9 @@ void omap_mcbsp_free(unsigned int id)
 	if (mcbsp->pdata && mcbsp->pdata->ops && mcbsp->pdata->ops->free)
 		mcbsp->pdata->ops->free(id);
 
-	/* Do procedure specific to omap34xx arch, if applicable */
-	omap34xx_mcbsp_free(mcbsp);
+	/* Disable wakup behavior */
+	if (mcbsp->mcbsp_config_type >= MCBSP_CONFIG_TYPE3)
+		MCBSP_WRITE(mcbsp, WAKEUPEN, 0);
 
 	pm_runtime_put_sync(mcbsp->dev);
 

index 1408156..88ee00f 100644
@@ -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,

@@ -149,6 +174,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++;
 	}
 	od = omap_device_build_ss(name, id, oh_device, count, pdata,

index 31b24c9..2a7e8d5 100644
@@ -317,6 +317,7 @@ struct omap_mcbsp_platform_data {
 #ifdef CONFIG_ARCH_OMAP3
 	/* Sidetone block for McBSP 2 and 3 */
 	unsigned long phys_base_st;
+	int (*enable_st_clock)(unsigned int, bool);
 #endif
 	u16 buffer_size;
 	unsigned int mcbsp_config_type;

index 202292a..1a7cfb3 100644
@@ -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, omap_mcbsp_cache_size;
 

@@ -267,13 +263,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);

@@ -294,9 +285,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)

index 2a7e8d5..720461c 100644
@@ -26,9 +26,6 @@
 
 #include <linux/spinlock.h>
 
-#include <mach/hardware.h>
-#include <plat/clock.h>
-
 /* macro for building platform_device for McBSP ports */
 #define OMAP_MCBSP_PLATFORM_DEVICE(port_nr)		\
 static struct platform_device omap_mcbsp##port_nr = {	\

@@ -314,11 +311,11 @@ struct omap_mcbsp_platform_data {
 	u8 dma_rx_sync, dma_tx_sync;
 	u16 rx_irq, tx_irq;
 	struct omap_mcbsp_ops *ops;
-#ifdef CONFIG_ARCH_OMAP3
+
 	/* Sidetone block for McBSP 2 and 3 */
 	unsigned long phys_base_st;
 	int (*enable_st_clock)(unsigned int, bool);
-#endif
+
 	u16 buffer_size;
 	unsigned int mcbsp_config_type;
 	u8 reg_size;

@@ -354,12 +351,12 @@ struct omap_mcbsp {
 	spinlock_t lock;
 	struct omap_mcbsp_platform_data *pdata;
 	struct clk *fclk;
-#ifdef CONFIG_ARCH_OMAP3
+
 	struct omap_mcbsp_st_data *st_data;
 	int dma_op_mode;
 	u16 max_tx_thres;
 	u16 max_rx_thres;
-#endif
+
 	void *reg_cache;
 	unsigned int mcbsp_config_type;
 };

@@ -380,7 +377,6 @@ extern int omap_mcbsp_count, omap_mcbsp_cache_size;
 
 int omap_mcbsp_init(void);
 void omap_mcbsp_config(unsigned int id, const struct omap_mcbsp_reg_cfg * config);
-#ifdef CONFIG_ARCH_OMAP3
 void omap_mcbsp_set_tx_threshold(unsigned int id, u16 threshold);
 void omap_mcbsp_set_rx_threshold(unsigned int id, u16 threshold);
 u16 omap_mcbsp_get_max_tx_threshold(unsigned int id);

@@ -389,18 +385,7 @@ u16 omap_mcbsp_get_fifo_size(unsigned int id);
 u16 omap_mcbsp_get_tx_delay(unsigned int id);
 u16 omap_mcbsp_get_rx_delay(unsigned int id);
 int omap_mcbsp_get_dma_op_mode(unsigned int id);
-#else
-static inline void omap_mcbsp_set_tx_threshold(unsigned int id, u16 threshold)
-{ }
-static inline void omap_mcbsp_set_rx_threshold(unsigned int id, u16 threshold)
-{ }
-static inline u16 omap_mcbsp_get_max_tx_threshold(unsigned int id) { return 0; }
-static inline u16 omap_mcbsp_get_max_rx_threshold(unsigned int id) { return 0; }
-static inline u16 omap_mcbsp_get_fifo_size(unsigned int id) { return 0; }
-static inline u16 omap_mcbsp_get_tx_delay(unsigned int id) { return 0; }
-static inline u16 omap_mcbsp_get_rx_delay(unsigned int id) { return 0; }
-static inline int omap_mcbsp_get_dma_op_mode(unsigned int id) { return 0; }
-#endif
+
 int omap_mcbsp_request(unsigned int id);
 void omap_mcbsp_free(unsigned int id);
 void omap_mcbsp_start(unsigned int id, int tx, int rx);

@@ -416,21 +401,11 @@ void omap2_mcbsp1_mux_fsr_src(u8 mux);
 int omap_mcbsp_dma_ch_params(unsigned int id, unsigned int stream);
 int omap_mcbsp_dma_reg_params(unsigned int id, unsigned int stream);
 
-#ifdef CONFIG_ARCH_OMAP3
 /* Sidetone specific API */
 int omap_st_set_chgain(unsigned int id, int channel, s16 chgain);
 int omap_st_get_chgain(unsigned int id, int channel, s16 *chgain);
 int omap_st_enable(unsigned int id);
 int omap_st_disable(unsigned int id);
 int omap_st_is_enabled(unsigned int id);
-#else
-static inline int omap_st_set_chgain(unsigned int id, int channel,
-				     s16 chgain) { return 0; }
-static inline int omap_st_get_chgain(unsigned int id, int channel,
-				     s16 *chgain) { return 0; }
-static inline int omap_st_enable(unsigned int id) { return 0; }
-static inline int omap_st_disable(unsigned int id) { return 0; }
-static inline int omap_st_is_enabled(unsigned int id) {  return 0; }
-#endif
 
 #endif

index 1a7cfb3..d565b36 100644
@@ -63,7 +63,6 @@ static int omap_mcbsp_read(struct omap_mcbsp *mcbsp, u16 reg, bool from_cache)
 	}
 }
 
-#ifdef CONFIG_ARCH_OMAP3
 static void omap_mcbsp_st_write(struct omap_mcbsp *mcbsp, u16 reg, u32 val)
 {
 	__raw_writel(val, mcbsp->st_data->io_base_st + reg);

@@ -73,7 +72,6 @@ static int omap_mcbsp_st_read(struct omap_mcbsp *mcbsp, u16 reg)
 {
 	return __raw_readl(mcbsp->st_data->io_base_st + reg);
 }
-#endif
 
 #define MCBSP_READ(mcbsp, reg) \
 		omap_mcbsp_read(mcbsp, OMAP_MCBSP_REG_##reg, 0)

@@ -258,7 +256,6 @@ int omap_mcbsp_dma_reg_params(unsigned int id, unsigned int stream)
 }
 EXPORT_SYMBOL(omap_mcbsp_dma_reg_params);
 
-#ifdef CONFIG_ARCH_OMAP3
 static void omap_st_on(struct omap_mcbsp *mcbsp)
 {
 	unsigned int w;

@@ -657,11 +654,6 @@ int omap_mcbsp_get_dma_op_mode(unsigned int id)
 }
 EXPORT_SYMBOL(omap_mcbsp_get_dma_op_mode);
 
-#else
-static inline void omap_st_start(struct omap_mcbsp *mcbsp) {}
-static inline void omap_st_stop(struct omap_mcbsp *mcbsp) {}
-#endif
-
 int omap_mcbsp_request(unsigned int id)
 {
 	struct omap_mcbsp *mcbsp;

@@ -935,7 +927,6 @@ void omap2_mcbsp1_mux_fsr_src(u8 mux)
 }
 #endif
 
-#ifdef CONFIG_ARCH_OMAP3
 #define max_thres(m)			(mcbsp->pdata->buffer_size)
 #define valid_threshold(m, val)		((val) <= max_thres(m))
 #define THRESHOLD_PROP_BUILDER(prop)					\

@@ -1205,10 +1196,6 @@ static inline void __devexit omap34xx_device_exit(struct omap_mcbsp *mcbsp)
 			omap_st_remove(mcbsp);
 	}
 }
-#else
-static inline void __devinit omap34xx_device_init(struct omap_mcbsp *mcbsp) {}
-static inline void __devexit omap34xx_device_exit(struct omap_mcbsp *mcbsp) {}
-#endif /* CONFIG_ARCH_OMAP3 */
 
 /*
  * McBSP1 and McBSP3 are directly mapped on 1610 and 1510.

index d565b36..dd6a19e 100644
@@ -1086,16 +1086,6 @@ static const struct attribute_group additional_attr_group = {
 	.attrs = (struct attribute **)additional_attrs,
 };
 
-static inline int __devinit omap_additional_add(struct device *dev)
-{
-	return sysfs_create_group(&dev->kobj, &additional_attr_group);
-}
-
-static inline void __devexit omap_additional_remove(struct device *dev)
-{
-	sysfs_remove_group(&dev->kobj, &additional_attr_group);
-}
-
 static const struct attribute *sidetone_attrs[] = {
 	&dev_attr_st_taps.attr,
 	NULL,

@@ -1105,10 +1095,9 @@ static const struct attribute_group sidetone_attr_group = {
 	.attrs = (struct attribute **)sidetone_attrs,
 };
 
-static int __devinit omap_st_add(struct omap_mcbsp *mcbsp)
+static int __devinit omap_mcbsp_st_add(struct omap_mcbsp *mcbsp,
+				       struct resource *res)
 {
-	struct platform_device *pdev;
-	struct resource *res;
 	struct omap_mcbsp_st_data *st_data;
 	int err;
 

@@ -1118,9 +1107,6 @@ static int __devinit omap_st_add(struct omap_mcbsp *mcbsp)
 		goto err1;
 	}
 
-	pdev = container_of(mcbsp->dev, struct platform_device, dev);
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sidetone");
 	st_data->io_base_st = ioremap(res->start, resource_size(res));
 	if (!st_data->io_base_st) {
 		err = -ENOMEM;

@@ -1140,61 +1126,42 @@ err2:
 	kfree(st_data);
 err1:
 	return err;
-
 }
 
-static void __devexit omap_st_remove(struct omap_mcbsp *mcbsp)
+static void __devexit omap_mcbsp_st_free(struct omap_mcbsp *mcbsp)
 {
 	struct omap_mcbsp_st_data *st_data = mcbsp->st_data;
 
-	if (st_data) {
-		sysfs_remove_group(&mcbsp->dev->kobj, &sidetone_attr_group);
-		iounmap(st_data->io_base_st);
-		kfree(st_data);
-	}
+	sysfs_remove_group(&mcbsp->dev->kobj, &sidetone_attr_group);
+	iounmap(st_data->io_base_st);
+	kfree(st_data);
 }
 
-static inline void __devinit omap34xx_device_init(struct omap_mcbsp *mcbsp)
+static inline int __devinit omap_mcbsp_dmactrl_add(struct omap_mcbsp *mcbsp)
 {
-	mcbsp->dma_op_mode = MCBSP_DMA_MODE_ELEMENT;
-	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3) {
-		/*
-		 * Initially configure the maximum thresholds to a safe value.
-		 * The McBSP FIFO usage with these values should not go under
-		 * 16 locations.
-		 * If the whole FIFO without safety buffer is used, than there
-		 * is a possibility that the DMA will be not able to push the
-		 * new data on time, causing channel shifts in runtime.
-		 */
-		mcbsp->max_tx_thres = max_thres(mcbsp) - 0x10;
-		mcbsp->max_rx_thres = max_thres(mcbsp) - 0x10;
-		/*
-		 * REVISIT: Set dmap_op_mode to THRESHOLD as default
-		 * for mcbsp2 instances.
-		 */
-		if (omap_additional_add(mcbsp->dev))
-			dev_warn(mcbsp->dev,
-				"Unable to create additional controls\n");
-
-		if (mcbsp->id == 2 || mcbsp->id == 3)
-			if (omap_st_add(mcbsp))
-				dev_warn(mcbsp->dev,
-				 "Unable to create sidetone controls\n");
+	int err;
 
-	} else {
-		mcbsp->max_tx_thres = -EINVAL;
-		mcbsp->max_rx_thres = -EINVAL;
-	}
+	/*
+	 * Initially configure the maximum thresholds to a safe value.
+	 * The McBSP FIFO usage with these values should not go under
+	 * 16 locations.
+	 * If the whole FIFO without safety buffer is used, than there
+	 * is a possibility that the DMA will be not able to push the
+	 * new data on time, causing channel shifts in runtime.
+	 */
+	mcbsp->max_tx_thres = max_thres(mcbsp) - 0x10;
+	mcbsp->max_rx_thres = max_thres(mcbsp) - 0x10;
+
+	err = sysfs_create_group(&mcbsp->dev->kobj, &additional_attr_group);
+	if (err)
+		dev_warn(mcbsp->dev, "Unable to create additional controls\n");
+
+	return err;
 }
 
-static inline void __devexit omap34xx_device_exit(struct omap_mcbsp *mcbsp)
+static inline void __devexit omap_mcbsp_dmactrl_free(struct omap_mcbsp *mcbsp)
 {
-	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3) {
-		omap_additional_remove(mcbsp->dev);
-
-		if (mcbsp->id == 2 || mcbsp->id == 3)
-			omap_st_remove(mcbsp);
-	}
+	sysfs_remove_group(&mcbsp->dev->kobj, &additional_attr_group);
 }
 
 /*

@@ -1295,13 +1262,32 @@ static int __devinit omap_mcbsp_probe(struct platform_device *pdev)
 	mcbsp_ptr[id] = mcbsp;
 	mcbsp->mcbsp_config_type = pdata->mcbsp_config_type;
 	platform_set_drvdata(pdev, mcbsp);
-	pm_runtime_enable(mcbsp->dev);
 
-	/* Initialize mcbsp properties for OMAP34XX if needed / applicable */
-	omap34xx_device_init(mcbsp);
+	mcbsp->dma_op_mode = MCBSP_DMA_MODE_ELEMENT;
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3) {
+		ret = omap_mcbsp_dmactrl_add(mcbsp);
+		if (ret)
+			goto err_thres;
+	} else {
+		mcbsp->max_tx_thres = -EINVAL;
+		mcbsp->max_rx_thres = -EINVAL;
+	}
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sidetone");
+	if (res) {
+		ret = omap_mcbsp_st_add(mcbsp, res);
+		if (ret)
+			goto err_st;
+	}
+
+	pm_runtime_enable(mcbsp->dev);
 
 	return 0;
 
+err_st:
+	if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3)
+		omap_mcbsp_dmactrl_free(mcbsp);
+err_thres:
+	clk_put(mcbsp->fclk);
 err_res:
 	iounmap(mcbsp->io_base);
 err_ioremap:

@@ -1316,12 +1302,14 @@ static int __devexit omap_mcbsp_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 	if (mcbsp) {
-
 		if (mcbsp->pdata && mcbsp->pdata->ops &&
 				mcbsp->pdata->ops->free)
 			mcbsp->pdata->ops->free(mcbsp->id);
 
-		omap34xx_device_exit(mcbsp);
+		if (mcbsp->mcbsp_config_type == MCBSP_CONFIG_TYPE3)
+			omap_mcbsp_dmactrl_free(mcbsp);
+		if (mcbsp->st_data)
+			omap_mcbsp_st_free(mcbsp);
 
 		clk_put(mcbsp->fclk);