Merge remote-tracking branch 'fsl-linux-sdk/imx_3.0.35_4.0.0' into imx_3.0.35_androidjb4.2.2_1.0.0-ga

20 files changed, 1972 insertions, 1164 deletions

diff --git a/arch/arm/configs/imx6_defconfig b/arch/arm/configs/imx6_defconfig
index fc897eddcfa3..5ad4a002676d 100644
--- a/arch/arm/configs/imx6_defconfig
+++ b/arch/arm/configs/imx6_defconfig
@@ -331,6 +331,7 @@ CONFIG_ARCH_MXC_AUDMUX_V2=y
 CONFIG_IRAM_ALLOC=y
 CONFIG_CLK_DEBUG=y
 CONFIG_DMA_ZONE_SIZE=184
+#CONFIG_MX6_ENET_IRQ_TO_GPIO is not set
 
 #
 # System MMU
diff --git a/arch/arm/mach-mx6/Kconfig b/arch/arm/mach-mx6/Kconfig
index 59d8ab313f73..df954b40d8e8 100644
--- a/arch/arm/mach-mx6/Kconfig
+++ b/arch/arm/mach-mx6/Kconfig
@@ -319,4 +319,11 @@ config MACH_IMX_BLUETOOTH_RFKILL
        ---help---
 	Say Y to get the standard rfkill interface of Bluetooth
 
+config MX6_ENET_IRQ_TO_GPIO
+	bool "Route ENET interrupts to GPIO"
+	default n
+	help
+	   Enabling this will direct all the ENET interrupts to a board specific GPIO.
+	   This will allow the system to enter WAIT mode when ENET is active.
+
 endif
diff --git a/arch/arm/mach-mx6/board-mx6q_arm2.c b/arch/arm/mach-mx6/board-mx6q_arm2.c
index d627324d77ab..8a2687d04f36 100644
--- a/arch/arm/mach-mx6/board-mx6q_arm2.c
+++ b/arch/arm/mach-mx6/board-mx6q_arm2.c
@@ -156,6 +156,12 @@
 
 #define MX6_ARM2_CAN2_STBY		MX6_ARM2_IO_EXP_GPIO2(1)
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define MX6_ENET_IRQ		IMX_GPIO_NR(1, 6)
+#define IOMUX_OBSRV_MUX1_OFFSET	0x3c
+#define OBSRV_MUX1_MASK			0x3f
+#define OBSRV_MUX1_ENET_IRQ		0x9
+#endif
 
 #define BMCR_PDOWN			0x0800 /* PHY Powerdown */
 
@@ -388,6 +394,9 @@ static struct fec_platform_data fec_data __initdata = {
 	.init			= mx6_arm2_fec_phy_init,
 	.power_hibernate	= mx6_arm2_fec_power_hibernate,
 	.phy			= PHY_INTERFACE_MODE_RGMII,
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	.gpio_irq = MX6_ENET_IRQ,
+#endif
 };
 
 static int mx6_arm2_spi_cs[] = {
@@ -2200,8 +2209,14 @@ static void __init mx6_arm2_init(void)
 
 	imx6q_add_anatop_thermal_imx(1, &mx6_arm2_anatop_thermal_data);
 
-	if (!esai_record)
+	if (!esai_record) {
 		imx6_init_fec(fec_data);
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	/* Make sure the IOMUX_OBSRV_MUX1 is set to ENET_IRQ. */
+	mxc_iomux_set_specialbits_register(IOMUX_OBSRV_MUX1_OFFSET,
+		OBSRV_MUX1_ENET_IRQ, OBSRV_MUX1_MASK);
+#endif
+	}
 
 	imx6q_add_pm_imx(0, &mx6_arm2_pm_data);
 	imx6q_add_sdhci_usdhc_imx(3, &mx6_arm2_sd4_data);
diff --git a/arch/arm/mach-mx6/board-mx6q_arm2.h b/arch/arm/mach-mx6/board-mx6q_arm2.h
index 8295c6e36905..2a6a2052b3f9 100644
--- a/arch/arm/mach-mx6/board-mx6q_arm2.h
+++ b/arch/arm/mach-mx6/board-mx6q_arm2.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2012, 2013 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -183,10 +183,14 @@ static iomux_v3_cfg_t mx6q_arm2_pads[] = {
 	/* USBOTG ID pin */
 	MX6Q_PAD_GPIO_1__USBOTG_ID,
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1,
+#else
 	/* MLB150 */
 	MX6Q_PAD_GPIO_3__MLB_MLBCLK,
 	MX6Q_PAD_GPIO_6__MLB_MLBSIG,
 	MX6Q_PAD_GPIO_2__MLB_MLBDAT,
+#endif
 };
 
 static iomux_v3_cfg_t mx6q_arm2_i2c3_pads[] = {
diff --git a/arch/arm/mach-mx6/board-mx6q_sabreauto.c b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
index 853ecf563222..45f4af592ab9 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabreauto.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabreauto.c
@@ -115,6 +115,13 @@
 #define SABREAUTO_MAX7310_2_BASE_ADDR	IMX_GPIO_NR(8, 8)
 #define SABREAUTO_MAX7310_3_BASE_ADDR	IMX_GPIO_NR(8, 16)
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define MX6_ENET_IRQ		IMX_GPIO_NR(1, 6)
+#define IOMUX_OBSRV_MUX1_OFFSET	0x3c
+#define OBSRV_MUX1_MASK			0x3f
+#define OBSRV_MUX1_ENET_IRQ		0x9
+#endif
+
 #define SABREAUTO_IO_EXP_GPIO1(x)	(SABREAUTO_MAX7310_1_BASE_ADDR + (x))
 #define SABREAUTO_IO_EXP_GPIO2(x)	(SABREAUTO_MAX7310_2_BASE_ADDR + (x))
 #define SABREAUTO_IO_EXP_GPIO3(x)	(SABREAUTO_MAX7310_3_BASE_ADDR + (x))
@@ -409,6 +416,9 @@ static struct fec_platform_data fec_data __initdata = {
 	.init			= mx6q_sabreauto_fec_phy_init,
 	.power_hibernate	= mx6q_sabreauto_fec_power_hibernate,
 	.phy			= PHY_INTERFACE_MODE_RGMII,
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	.gpio_irq = MX6_ENET_IRQ,
+#endif
 };
 
 static int mx6q_sabreauto_spi_cs[] = {
@@ -1723,9 +1733,15 @@ static void __init mx6_board_init(void)
 
 	imx6q_add_anatop_thermal_imx(1, &mx6q_sabreauto_anatop_thermal_data);
 
-	if (!can0_enable)
+	if (!can0_enable) {
 		imx6_init_fec(fec_data);
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+		/* Make sure the IOMUX_OBSRV_MUX1 is set to ENET_IRQ. */
+		mxc_iomux_set_specialbits_register(IOMUX_OBSRV_MUX1_OFFSET,
+			OBSRV_MUX1_ENET_IRQ, OBSRV_MUX1_MASK);
+#endif
 
+	}
 	imx6q_add_pm_imx(0, &mx6q_sabreauto_pm_data);
 
 	imx6q_add_sdhci_usdhc_imx(2, &mx6q_sabreauto_sd3_data);
diff --git a/arch/arm/mach-mx6/board-mx6q_sabreauto.h b/arch/arm/mach-mx6/board-mx6q_sabreauto.h
index 41d0c560f416..e4d62f1baaa5 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabreauto.h
+++ b/arch/arm/mach-mx6/board-mx6q_sabreauto.h
@@ -208,10 +208,14 @@ static iomux_v3_cfg_t mx6q_sabreauto_pads[] = {
 	/* HDMI */
 	MX6Q_PAD_EIM_A25__HDMI_TX_CEC_LINE,
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1,
+#else
 	/* MLB150 */
 	MX6Q_PAD_ENET_TXD1__MLB_MLBCLK,
 	MX6Q_PAD_GPIO_6__MLB_MLBSIG,
 	MX6Q_PAD_GPIO_2__MLB_MLBDAT,
+#endif
 };
 
 static iomux_v3_cfg_t mx6q_sabreauto_can0_pads[] = {
diff --git a/arch/arm/mach-mx6/board-mx6q_sabrelite.c b/arch/arm/mach-mx6/board-mx6q_sabrelite.c
index 24914fe6904b..338c16ab6a52 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabrelite.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabrelite.c
@@ -94,6 +94,13 @@
 #define MX6Q_SABRELITE_CSI0_RST		IMX_GPIO_NR(1, 8)
 #define MX6Q_SABRELITE_CSI0_PWN		IMX_GPIO_NR(1, 6)
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define MX6_ENET_IRQ		IMX_GPIO_NR(1, 6)
+#define IOMUX_OBSRV_MUX1_OFFSET	0x3c
+#define OBSRV_MUX1_MASK			0x3f
+#define OBSRV_MUX1_ENET_IRQ		0x9
+#endif
+
 #define MX6Q_SABRELITE_SD3_WP_PADCFG	(PAD_CTL_PKE | PAD_CTL_PUE |	\
 		PAD_CTL_PUS_22K_UP | PAD_CTL_SPEED_MED |	\
 		PAD_CTL_DSE_40ohm | PAD_CTL_HYS)
@@ -322,7 +329,9 @@ static iomux_v3_cfg_t mx6q_sabrelite_csi0_sensor_pads[] = {
 	MX6Q_PAD_CSI0_MCLK__IPU1_CSI0_HSYNC,
 	MX6Q_PAD_CSI0_PIXCLK__IPU1_CSI0_PIXCLK,
 	MX6Q_PAD_CSI0_VSYNC__IPU1_CSI0_VSYNC,
+#ifndef CONFIG_MX6_ENET_IRQ_TO_GPIO
 	MX6Q_PAD_GPIO_6__GPIO_1_6,		/* J5 - Camera GP */
+#endif
 	MX6Q_PAD_GPIO_8__GPIO_1_8,		/* J5 - Camera Reset */
 	MX6Q_PAD_SD1_DAT0__GPIO_1_16,		/* J5 - Camera GP */
 	MX6Q_PAD_NANDF_D5__GPIO_2_5,		/* J16 - MIPI GP */
@@ -468,6 +477,9 @@ static int mx6q_sabrelite_fec_phy_init(struct phy_device *phydev)
 static struct fec_platform_data fec_data __initdata = {
 	.init = mx6q_sabrelite_fec_phy_init,
 	.phy = PHY_INTERFACE_MODE_RGMII,
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	.gpio_irq = MX6_ENET_IRQ,
+#endif
 };
 
 static int mx6q_sabrelite_spi_cs[] = {
@@ -1259,6 +1271,11 @@ static void __init mx6_sabrelite_board_init(void)
 
 	imx6q_add_anatop_thermal_imx(1, &mx6q_sabrelite_anatop_thermal_data);
 	imx6_init_fec(fec_data);
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	/* Make sure the IOMUX_OBSRV_MUX1 is set to ENET_IRQ. */
+	mxc_iomux_set_specialbits_register(IOMUX_OBSRV_MUX1_OFFSET,
+		OBSRV_MUX1_ENET_IRQ, OBSRV_MUX1_MASK);
+#endif
 	imx6q_add_pm_imx(0, &mx6q_sabrelite_pm_data);
 	imx6q_add_sdhci_usdhc_imx(3, &mx6q_sabrelite_sd4_data);
 	imx6q_add_sdhci_usdhc_imx(2, &mx6q_sabrelite_sd3_data);
diff --git a/arch/arm/mach-mx6/board-mx6q_sabresd.c b/arch/arm/mach-mx6/board-mx6q_sabresd.c
index 2b8a1bedc17b..9971ff55a765 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabresd.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabresd.c
@@ -201,6 +201,13 @@
 #define SABRESD_ELAN_RST	IMX_GPIO_NR(3, 8)
 #define SABRESD_ELAN_INT	IMX_GPIO_NR(3, 28)
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define MX6_ENET_IRQ		IMX_GPIO_NR(1, 6)
+#define IOMUX_OBSRV_MUX1_OFFSET	0x3c
+#define OBSRV_MUX1_MASK			0x3f
+#define OBSRV_MUX1_ENET_IRQ		0x9
+#endif
+
 static struct clk *sata_clk;
 static struct clk *clko;
 static int mma8451_position;
@@ -303,6 +310,9 @@ static int mx6q_sabresd_fec_phy_init(struct phy_device *phydev)
 static struct fec_platform_data fec_data __initdata = {
 	.init = mx6q_sabresd_fec_phy_init,
 	.phy = PHY_INTERFACE_MODE_RGMII,
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	.gpio_irq = MX6_ENET_IRQ,
+#endif
 };
 
 static int mx6q_sabresd_spi_cs[] = {
@@ -1955,6 +1965,12 @@ static void __init mx6_sabresd_board_init(void)
 
 	imx6q_add_anatop_thermal_imx(1, &mx6q_sabresd_anatop_thermal_data);
 	imx6_init_fec(fec_data);
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	/* Make sure the IOMUX_OBSRV_MUX1 is set to ENET_IRQ. */
+	mxc_iomux_set_specialbits_register(IOMUX_OBSRV_MUX1_OFFSET,
+		OBSRV_MUX1_ENET_IRQ, OBSRV_MUX1_MASK);
+#endif
+
 	imx6q_add_pm_imx(0, &mx6q_sabresd_pm_data);
 
 	/* Move sd4 to first because sd4 connect to emmc.
diff --git a/arch/arm/mach-mx6/board-mx6q_sabresd.h b/arch/arm/mach-mx6/board-mx6q_sabresd.h
index 73ee909e50ef..b2bb8c923f0f 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabresd.h
+++ b/arch/arm/mach-mx6/board-mx6q_sabresd.h
@@ -132,9 +132,13 @@ static iomux_v3_cfg_t mx6q_sabresd_pads[] = {
 	MX6Q_PAD_KEY_COL3__I2C2_SCL,
 	MX6Q_PAD_KEY_ROW3__I2C2_SDA,
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1,
+#else
 	/* I2C3 */
 	MX6Q_PAD_GPIO_3__I2C3_SCL,	/* GPIO1[3] */
 	MX6Q_PAD_GPIO_6__I2C3_SDA,
+#endif
 
 	/* DISPLAY */
 	MX6Q_PAD_DI0_DISP_CLK__IPU1_DI0_DISP_CLK,
diff --git a/arch/arm/mach-mx6/clock.c b/arch/arm/mach-mx6/clock.c
index c45fa2cafe71..574b142eb758 100644
--- a/arch/arm/mach-mx6/clock.c
+++ b/arch/arm/mach-mx6/clock.c
@@ -51,6 +51,7 @@ extern int lp_med_freq;
 extern int wait_mode_arm_podf;
 extern int lp_audio_freq;
 extern int cur_arm_podf;
+extern bool enet_is_active;
 
 void __iomem *apll_base;
 
@@ -1153,7 +1154,11 @@ static int _clk_pll_mlb_main_enable(struct clk *clk)
 	reg = __raw_readl(pllbase);
 	reg &= ~ANADIG_PLL_BYPASS;
 
-	reg = 0x0da20800;
+	reg = (0x3 << ANADIG_PLL_MLB_FLT_RES_CFG_OFFSET) |
+		(0x3 << ANADIG_PLL_MLB_RX_CLK_DELAY_CFG_OFFSET) |
+		(0x2 << ANADIG_PLL_MLB_VDDD_DELAY_CFG_OFFSET) |
+		(0x1 << ANADIG_PLL_MLB_VDDA_DELAY_CFG_OFFSET) |
+		(ANADIG_PLL_HOLD_RING_OFF);
 	__raw_writel(reg, pllbase);
 
 	return 0;
@@ -3724,6 +3729,23 @@ static unsigned long _clk_enet_get_rate(struct clk *clk)
 	return 500000000 / div;
 }
 
+static int _clk_enet_enable(struct clk *clk)
+{
+#ifndef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	enet_is_active = true;
+#endif
+	_clk_enable(clk);
+	return 0;
+}
+
+static void _clk_enet_disable(struct clk *clk)
+{
+	_clk_disable(clk);
+#ifndef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	enet_is_active = false;
+#endif
+}
+
 static struct clk enet_clk[] = {
 	{
 	__INIT_CLK_DEBUG(enet_clk)
@@ -3731,8 +3753,8 @@ static struct clk enet_clk[] = {
 	 .parent = &pll8_enet_main_clk,
 	 .enable_reg = MXC_CCM_CCGR1,
 	 .enable_shift = MXC_CCM_CCGRx_CG5_OFFSET,
-	 .enable = _clk_enable,
-	 .disable = _clk_disable,
+	 .enable = _clk_enet_enable,
+	 .disable = _clk_enet_disable,
 	 .set_rate = _clk_enet_set_rate,
 	 .get_rate = _clk_enet_get_rate,
 	.secondary = &enet_clk[1],
@@ -4889,6 +4911,7 @@ static int _clk_mlb_set_parent(struct clk *clk, struct clk *parent)
 static struct clk mlb150_clk = {
 	__INIT_CLK_DEBUG(mlb150_clk)
 	.id = 0,
+	.secondary = &ocram_clk,
 	.set_parent = _clk_mlb_set_parent,
 	.enable_reg = MXC_CCM_CCGR3,
 	.enable_shift = MXC_CCM_CCGRx_CG9_OFFSET,
diff --git a/arch/arm/mach-mx6/system.c b/arch/arm/mach-mx6/system.c
index f6ffa2999ed2..4990a4a0abbb 100644
--- a/arch/arm/mach-mx6/system.c
+++ b/arch/arm/mach-mx6/system.c
@@ -57,6 +57,7 @@ volatile unsigned int num_cpu_idle;
 volatile unsigned int num_cpu_idle_lock = 0x0;
 int wait_mode_arm_podf;
 int cur_arm_podf;
+bool enet_is_active;
 void arch_idle_with_workaround(int cpu);
 
 extern void *mx6sl_wfi_iram_base;
@@ -350,7 +351,7 @@ void arch_idle_single_core(void)
 	}
 }
 
-void arch_idle_with_workaround(cpu)
+void arch_idle_with_workaround(int cpu)
 {
 	u32 podf = wait_mode_arm_podf;
 
@@ -369,18 +370,10 @@ void arch_idle_with_workaround(cpu)
 
 }
 
-void arch_idle_multi_core(void)
+void arch_idle_multi_core(int cpu)
 {
 	u32 reg;
-	int cpu = smp_processor_id();
 
-#ifdef CONFIG_LOCAL_TIMERS
-	if (!tick_broadcast_oneshot_active()
-		|| !tick_oneshot_mode_active())
-		return;
-
-	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
-#endif
 	/* iMX6Q and iMX6DL */
 	if ((cpu_is_mx6q() && chip_rev >= IMX_CHIP_REVISION_1_2) ||
 		(cpu_is_mx6dl() && chip_rev >= IMX_CHIP_REVISION_1_1)) {
@@ -398,24 +391,37 @@ void arch_idle_multi_core(void)
 		ca9_do_idle();
 	} else
 		arch_idle_with_workaround(cpu);
-#ifdef CONFIG_LOCAL_TIMERS
-	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
-#endif
-
 }
 
 void arch_idle(void)
 {
+	int cpu = smp_processor_id();
+
 	if (enable_wait_mode) {
-		mxc_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
+#ifdef CONFIG_LOCAL_TIMERS
+		if (!tick_broadcast_oneshot_active()
+			|| !tick_oneshot_mode_active())
+			return;
+
+		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
+#endif
+		if (enet_is_active)
+			/* Don't allow the chip to enter WAIT mode if enet is active
+			  * and the GPIO workaround for ENET interrupts is not used,
+			  * since all ENET interrupts donot wake up the SOC.
+			  */
+			mxc_cpu_lp_set(WAIT_CLOCKED);
+		else
+			mxc_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
 		if (mem_clk_on_in_wait) {
 			u32 reg;
 			/*
 			  * MX6SL, MX6Q (TO1.2 or later) and
-			  * MX6DL (TO1.1 or later) have a bit in CCM_CGPR that
-			  * when cleared keeps the clocks to memories ON
-			  * when ARM is in WFI. This mode can be used when
-			  * IPG clock is very low (12MHz) and the ARM:IPG ratio
+			  * MX6DL (TO1.1 or later) have a bit in
+			  * CCM_CGPR that when cleared keeps the
+			  * clocks to memories ON when ARM is in WFI.
+			  * This mode can be used when IPG clock is
+			  * very low (12MHz) and the ARM:IPG ratio
 			  * perhaps cannot be maintained.
 			  */
 			reg = __raw_readl(MXC_CCM_CGPR);
@@ -427,8 +433,11 @@ void arch_idle(void)
 			/* iMX6SL or iMX6DLS */
 			arch_idle_single_core();
 		else
-			arch_idle_multi_core();
-	} else {
+			arch_idle_multi_core(cpu);
+#ifdef CONFIG_LOCAL_TIMERS
+		clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
+#endif
+	}  else {
 		mxc_cpu_lp_set(WAIT_CLOCKED);
 		ca9_do_idle();
 	}
diff --git a/arch/arm/plat-mxc/include/mach/iomux-mx6q.h b/arch/arm/plat-mxc/include/mach/iomux-mx6q.h
index 4cc8ad9eb512..557fc4398cd1 100644
--- a/arch/arm/plat-mxc/include/mach/iomux-mx6q.h
+++ b/arch/arm/plat-mxc/include/mach/iomux-mx6q.h
@@ -97,6 +97,9 @@
 		PAD_CTL_SPEED_MED | PAD_CTL_PKE	| PAD_CTL_PUE | 	\
 		PAD_CTL_PUS_100K_UP)
 
+#define ENET_IRQ_PAD_CTRL (PAD_CTL_SRE_FAST | PAD_CTL_DSE_40ohm | \
+		PAD_CTL_SPEED_MED)
+
 #define _MX6Q_PAD_SD2_DAT1__USDHC2_DAT1				\
 		IOMUX_PAD(0x0360, 0x004C, 0, 0x0000, 0, 0)
 #define _MX6Q_PAD_SD2_DAT1__ECSPI5_SS0				\
@@ -2329,8 +2332,13 @@
 
 #define _MX6Q_PAD_GPIO_6__ESAI1_SCKT			\
 		IOMUX_PAD(0x0600, 0x0230, 0, 0x0870, 1, 0)
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define _MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1		\
+		IOMUX_PAD(0x0600, 0x0230, 1 | IOMUX_CONFIG_SION, 0x0000, 0, 0)
+#else
 #define _MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1		\
 		IOMUX_PAD(0x0600, 0x0230, 1, 0x0000, 0, 0)
+#endif
 #define _MX6Q_PAD_GPIO_6__I2C3_SDA			\
 		IOMUX_PAD(0x0600, 0x0230, 2 | IOMUX_CONFIG_SION, 0x08AC, 1, 0)
 #define _MX6Q_PAD_GPIO_6__CCM_CCM_OUT_0			\
@@ -5932,22 +5940,27 @@
 #define  MX6Q_PAD_GPIO_3__MLB_MLBCLK		\
 		(_MX6Q_PAD_GPIO_3__MLB_MLBCLK | MUX_PAD_CTRL(MX6Q_MLB150_PAD_CTRL))
 
-#define  MX6Q_PAD_GPIO_6__ESAI1_SCKT		\
-		(_MX6Q_PAD_GPIO_6__ESAI1_SCKT | MUX_PAD_CTRL(NO_PAD_CTRL))
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+#define  MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1		\
+		(_MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1 | MUX_PAD_CTRL(ENET_IRQ_PAD_CTRL))
+#else
 #define  MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1		\
 		(_MX6Q_PAD_GPIO_6__OBSERVE_MUX_OBSRV_INT_OUT1 | MUX_PAD_CTRL(NO_PAD_CTRL))
+#define  MX6Q_PAD_GPIO_6__ESAI1_SCKT		\
+		(_MX6Q_PAD_GPIO_6__ESAI1_SCKT | MUX_PAD_CTRL(NO_PAD_CTRL))
+#define  MX6Q_PAD_GPIO_6__GPIO_1_6		\
+		(_MX6Q_PAD_GPIO_6__GPIO_1_6 | MUX_PAD_CTRL(NO_PAD_CTRL))
 #define  MX6Q_PAD_GPIO_6__I2C3_SDA		\
 		(_MX6Q_PAD_GPIO_6__I2C3_SDA | MUX_PAD_CTRL(NO_PAD_CTRL))
 #define  MX6Q_PAD_GPIO_6__CCM_CCM_OUT_0		\
 		(_MX6Q_PAD_GPIO_6__CCM_CCM_OUT_0 | MUX_PAD_CTRL(NO_PAD_CTRL))
 #define  MX6Q_PAD_GPIO_6__CSU_CSU_INT_DEB		\
 		(_MX6Q_PAD_GPIO_6__CSU_CSU_INT_DEB | MUX_PAD_CTRL(NO_PAD_CTRL))
-#define  MX6Q_PAD_GPIO_6__GPIO_1_6		\
-		(_MX6Q_PAD_GPIO_6__GPIO_1_6 | MUX_PAD_CTRL(NO_PAD_CTRL))
 #define  MX6Q_PAD_GPIO_6__USDHC2_LCTL		\
 		(_MX6Q_PAD_GPIO_6__USDHC2_LCTL | MUX_PAD_CTRL(MX6Q_USDHC_PAD_CTRL))
 #define  MX6Q_PAD_GPIO_6__MLB_MLBSIG		\
 		(_MX6Q_PAD_GPIO_6__MLB_MLBSIG | MUX_PAD_CTRL(MX6Q_MLB150_PAD_CTRL))
+#endif
 
 #define  MX6Q_PAD_GPIO_2__ESAI1_FST		\
 		(_MX6Q_PAD_GPIO_2__ESAI1_FST | MUX_PAD_CTRL(NO_PAD_CTRL))
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index 727fd9c53199..f3626b0d7b11 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -59,6 +59,9 @@ static void cfi_amdstd_resume (struct mtd_info *);
 static int cfi_amdstd_reboot(struct notifier_block *, unsigned long, void *);
 static int cfi_amdstd_secsi_read (struct mtd_info *, loff_t, size_t, size_t *, u_char *);
 
+static int cfi_amdstd_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
+				  size_t *retlen, const u_char *buf);
+
 static void cfi_amdstd_destroy(struct mtd_info *);
 
 struct mtd_info *cfi_cmdset_0002(struct map_info *, int);
@@ -145,8 +148,7 @@ static void fixup_amd_bootblock(struct mtd_info *mtd)
 	if (((major << 8) | minor) < 0x3131) {
 		/* CFI version 1.0 => don't trust bootloc */
 
-		DEBUG(MTD_DEBUG_LEVEL1,
-			"%s: JEDEC Vendor ID is 0x%02X Device ID is 0x%02X\n",
+		pr_debug("%s: JEDEC Vendor ID is 0x%02X Device ID is 0x%02X\n",
 			map->name, cfi->mfr, cfi->id);
 
 		/* AFAICS all 29LV400 with a bottom boot block have a device ID
@@ -166,8 +168,7 @@ static void fixup_amd_bootblock(struct mtd_info *mtd)
 			 * the 8-bit device ID.
 			 */
 			(cfi->mfr == CFI_MFR_MACRONIX)) {
-			DEBUG(MTD_DEBUG_LEVEL1,
-				"%s: Macronix MX29LV400C with bottom boot block"
+			pr_debug("%s: Macronix MX29LV400C with bottom boot block"
 				" detected\n", map->name);
 			extp->TopBottom = 2;	/* bottom boot */
 		} else
@@ -178,8 +179,7 @@ static void fixup_amd_bootblock(struct mtd_info *mtd)
 			extp->TopBottom = 2;	/* bottom boot */
 		}
 
-		DEBUG(MTD_DEBUG_LEVEL1,
-			"%s: AMD CFI PRI V%c.%c has no boot block field;"
+		pr_debug("%s: AMD CFI PRI V%c.%c has no boot block field;"
 			" deduced %s from Device ID\n", map->name, major, minor,
 			extp->TopBottom == 2 ? "bottom" : "top");
 	}
@@ -191,7 +191,7 @@ static void fixup_use_write_buffers(struct mtd_info *mtd)
 	struct map_info *map = mtd->priv;
 	struct cfi_private *cfi = map->fldrv_priv;
 	if (cfi->cfiq->BufWriteTimeoutTyp) {
-		DEBUG(MTD_DEBUG_LEVEL1, "Using buffer write method\n" );
+		pr_debug("Using buffer write method\n" );
 		mtd->write = cfi_amdstd_write_buffers;
 	}
 }
@@ -317,7 +317,7 @@ static void fixup_s29gl064n_sectors(struct mtd_info *mtd)
 
 	if ((cfi->cfiq->EraseRegionInfo[0] & 0xffff) == 0x003f) {
 		cfi->cfiq->EraseRegionInfo[0] |= 0x0040;
-		pr_warning("%s: Bad S29GL064N CFI data, adjust from 64 to 128 sectors\n", mtd->name);
+		pr_warning("%s: Bad S29GL064N CFI data; adjust from 64 to 128 sectors\n", mtd->name);
 	}
 }
 
@@ -328,10 +328,23 @@ static void fixup_s29gl032n_sectors(struct mtd_info *mtd)
 
 	if ((cfi->cfiq->EraseRegionInfo[1] & 0xffff) == 0x007e) {
 		cfi->cfiq->EraseRegionInfo[1] &= ~0x0040;
-		pr_warning("%s: Bad S29GL032N CFI data, adjust from 127 to 63 sectors\n", mtd->name);
+		pr_warning("%s: Bad S29GL032N CFI data; adjust from 127 to 63 sectors\n", mtd->name);
 	}
 }
 
+static void fixup_s29ns512p_sectors(struct mtd_info *mtd)
+{
+	struct map_info *map = mtd->priv;
+	struct cfi_private *cfi = map->fldrv_priv;
+
+	/*
+	 *  S29NS512P flash uses more than 8bits to report number of sectors,
+	 * which is not permitted by CFI.
+	 */
+	cfi->cfiq->EraseRegionInfo[0] = 0x020001ff;
+	pr_warning("%s: Bad S29NS512P CFI data; adjust to 512 sectors\n", mtd->name);
+}
+
 /* Used to fix CFI-Tables of chips without Extended Query Tables */
 static struct cfi_fixup cfi_nopri_fixup_table[] = {
 	{ CFI_MFR_SST, 0x234a, fixup_sst39vf }, /* SST39VF1602 */
@@ -362,6 +375,7 @@ static struct cfi_fixup cfi_fixup_table[] = {
 	{ CFI_MFR_AMD, 0x1301, fixup_s29gl064n_sectors },
 	{ CFI_MFR_AMD, 0x1a00, fixup_s29gl032n_sectors },
 	{ CFI_MFR_AMD, 0x1a01, fixup_s29gl032n_sectors },
+	{ CFI_MFR_AMD, 0x3f00, fixup_s29ns512p_sectors },
 	{ CFI_MFR_SST, 0x536a, fixup_sst38vf640x_sectorsize }, /* SST38VF6402 */
 	{ CFI_MFR_SST, 0x536b, fixup_sst38vf640x_sectorsize }, /* SST38VF6401 */
 	{ CFI_MFR_SST, 0x536c, fixup_sst38vf640x_sectorsize }, /* SST38VF6404 */
@@ -417,6 +431,68 @@ static void cfi_fixup_major_minor(struct cfi_private *cfi,
 	}
 }
 
+static int is_m29ew(struct cfi_private *cfi)
+{
+	if (cfi->mfr == CFI_MFR_INTEL &&
+	    ((cfi->device_type == CFI_DEVICETYPE_X8 && (cfi->id & 0xff) == 0x7e) ||
+	     (cfi->device_type == CFI_DEVICETYPE_X16 && cfi->id == 0x227e)))
+		return 1;
+	return 0;
+}
+
+/*
+ * From TN-13-07: Patching the Linux Kernel and U-Boot for M29 Flash, page 20:
+ * Some revisions of the M29EW suffer from erase suspend hang ups. In
+ * particular, it can occur when the sequence
+ * Erase Confirm -> Suspend -> Program -> Resume
+ * causes a lockup due to internal timing issues. The consequence is that the
+ * erase cannot be resumed without inserting a dummy command after programming
+ * and prior to resuming. [...] The work-around is to issue a dummy write cycle
+ * that writes an F0 command code before the RESUME command.
+ */
+static void cfi_fixup_m29ew_erase_suspend(struct map_info *map,
+					  unsigned long adr)
+{
+	struct cfi_private *cfi = map->fldrv_priv;
+	/* before resume, insert a dummy 0xF0 cycle for Micron M29EW devices */
+	if (is_m29ew(cfi))
+		map_write(map, CMD(0xF0), adr);
+}
+
+/*
+ * From TN-13-07: Patching the Linux Kernel and U-Boot for M29 Flash, page 22:
+ *
+ * Some revisions of the M29EW (for example, A1 and A2 step revisions)
+ * are affected by a problem that could cause a hang up when an ERASE SUSPEND
+ * command is issued after an ERASE RESUME operation without waiting for a
+ * minimum delay.  The result is that once the ERASE seems to be completed
+ * (no bits are toggling), the contents of the Flash memory block on which
+ * the erase was ongoing could be inconsistent with the expected values
+ * (typically, the array value is stuck to the 0xC0, 0xC4, 0x80, or 0x84
+ * values), causing a consequent failure of the ERASE operation.
+ * The occurrence of this issue could be high, especially when file system
+ * operations on the Flash are intensive.  As a result, it is recommended
+ * that a patch be applied.  Intensive file system operations can cause many
+ * calls to the garbage routine to free Flash space (also by erasing physical
+ * Flash blocks) and as a result, many consecutive SUSPEND and RESUME
+ * commands can occur.  The problem disappears when a delay is inserted after
+ * the RESUME command by using the udelay() function available in Linux.
+ * The DELAY value must be tuned based on the customer's platform.
+ * The maximum value that fixes the problem in all cases is 500us.
+ * But, in our experience, a delay of 30 µs to 50 µs is sufficient
+ * in most cases.
+ * We have chosen 500µs because this latency is acceptable.
+ */
+static void cfi_fixup_m29ew_delay_after_resume(struct cfi_private *cfi)
+{
+	/*
+	 * Resolving the Delay After Resume Issue see Micron TN-13-07
+	 * Worst case delay must be 500µs but 30-50µs should be ok as well
+	 */
+	if (is_m29ew(cfi))
+		cfi_udelay(500);
+}
+
 struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
 {
 	struct cfi_private *cfi = map->fldrv_priv;
@@ -443,9 +519,10 @@ struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
 	mtd->writesize = 1;
 	mtd->writebufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 
-	DEBUG(MTD_DEBUG_LEVEL3, "MTD %s(): write buffer size %d\n",
-		__func__, mtd->writebufsize);
+	pr_debug("MTD %s(): write buffer size %d\n", __func__,
+			mtd->writebufsize);
 
+	mtd->panic_write = cfi_amdstd_panic_write;
 	mtd->reboot_notifier.notifier_call = cfi_amdstd_reboot;
 
 	if (cfi->cfi_mode==CFI_MODE_CFI){
@@ -761,7 +838,10 @@ static void put_chip(struct map_info *map, struct flchip *chip, unsigned long ad
 
 	switch(chip->oldstate) {
 	case FL_ERASING:
+		cfi_fixup_m29ew_erase_suspend(map,
+			chip->in_progress_block_addr);
 		map_write(map, cfi->sector_erase_cmd, chip->in_progress_block_addr);
+		cfi_fixup_m29ew_delay_after_resume(cfi);
 		chip->oldstate = FL_READY;
 		chip->state = FL_ERASING;
 		break;
@@ -773,8 +853,6 @@ static void put_chip(struct map_info *map, struct flchip *chip, unsigned long ad
 
 	case FL_READY:
 	case FL_STATUS:
-		/* We should really make set_vpp() count, rather than doing this */
-		DISABLE_VPP(map);
 		break;
 	default:
 		printk(KERN_ERR "MTD: put_chip() called with oldstate %d!!\n", chip->oldstate);
@@ -903,6 +981,8 @@ static void __xipram xip_udelay(struct map_info *map, struct flchip *chip,
 			/* Disallow XIP again */
 			local_irq_disable();
 
+			/* Correct Erase Suspend Hangups for M29EW */
+			cfi_fixup_m29ew_erase_suspend(map, adr);
 			/* Resume the write or erase operation */
 			map_write(map, cfi->sector_erase_cmd, adr);
 			chip->state = oldstate;
@@ -960,17 +1040,13 @@ static void __xipram xip_udelay(struct map_info *map, struct flchip *chip,
 
 #define UDELAY(map, chip, adr, usec)  \
 do {  \
-	mutex_unlock(&chip->mutex);  \
 	cfi_udelay(usec);  \
-	mutex_lock(&chip->mutex);  \
 } while (0)
 
 #define INVALIDATE_CACHE_UDELAY(map, chip, adr, len, usec)  \
 do {  \
-	mutex_unlock(&chip->mutex);  \
 	INVALIDATE_CACHED_RANGE(map, adr, len);  \
 	cfi_udelay(usec);  \
-	mutex_lock(&chip->mutex);  \
 } while (0)
 
 #endif
@@ -1163,7 +1239,7 @@ static int __xipram do_write_oneword(struct map_info *map, struct flchip *chip,
 		return ret;
 	}
 
-	DEBUG( MTD_DEBUG_LEVEL3, "MTD %s(): WRITE 0x%.8lx(0x%.8lx)\n",
+	pr_debug("MTD %s(): WRITE 0x%.8lx(0x%.8lx)\n",
 	       __func__, adr, datum.x[0] );
 
 	/*
@@ -1174,7 +1250,7 @@ static int __xipram do_write_oneword(struct map_info *map, struct flchip *chip,
 	 */
 	oldd = map_read(map, adr);
 	if (map_word_equal(map, oldd, datum)) {
-		DEBUG( MTD_DEBUG_LEVEL3, "MTD %s(): NOP\n",
+		pr_debug("MTD %s(): NOP\n",
 		       __func__);
 		goto op_done;
 	}
@@ -1237,6 +1313,7 @@ static int __xipram do_write_oneword(struct map_info *map, struct flchip *chip,
 	xip_enable(map, chip, adr);
  op_done:
 	chip->state = FL_READY;
+	DISABLE_VPP(map);
 	put_chip(map, chip, adr);
 	mutex_unlock(&chip->mutex);
 
@@ -1400,7 +1477,7 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 
 	datum = map_word_load(map, buf);
 
-	DEBUG( MTD_DEBUG_LEVEL3, "MTD %s(): WRITE 0x%.8lx(0x%.8lx)\n",
+	pr_debug("MTD %s(): WRITE 0x%.8lx(0x%.8lx)\n",
 	       __func__, adr, datum.x[0] );
 
 	XIP_INVAL_CACHED_RANGE(map, adr, len);
@@ -1468,8 +1545,20 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 		UDELAY(map, chip, adr, 1);
 	}
 
-	/* reset on all failures. */
-	map_write( map, CMD(0xF0), chip->start );
+	/*
+	 * Recovery from write-buffer programming failures requires
+	 * the write-to-buffer-reset sequence.  Since the last part
+	 * of the sequence also works as a normal reset, we can run
+	 * the same commands regardless of why we are here.
+	 * See e.g.
+	 * http://www.spansion.com/Support/Application%20Notes/MirrorBit_Write_Buffer_Prog_Page_Buffer_Read_AN.pdf
+	 */
+	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi,
+			 cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi,
+			 cfi->device_type, NULL);
+	cfi_send_gen_cmd(0xF0, cfi->addr_unlock1, chip->start, map, cfi,
+			 cfi->device_type, NULL);
 	xip_enable(map, chip, adr);
 	/* FIXME - should have reset delay before continuing */
 
@@ -1479,6 +1568,7 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 	ret = -EIO;
  op_done:
 	chip->state = FL_READY;
+	DISABLE_VPP(map);
 	put_chip(map, chip, adr);
 	mutex_unlock(&chip->mutex);
 
@@ -1565,6 +1655,242 @@ static int cfi_amdstd_write_buffers(struct mtd_info *mtd, loff_t to, size_t len,
 	return 0;
 }
 
+/*
+ * Wait for the flash chip to become ready to write data
+ *
+ * This is only called during the panic_write() path. When panic_write()
+ * is called, the kernel is in the process of a panic, and will soon be
+ * dead. Therefore we don't take any locks, and attempt to get access
+ * to the chip as soon as possible.
+ */
+static int cfi_amdstd_panic_wait(struct map_info *map, struct flchip *chip,
+				 unsigned long adr)
+{
+	struct cfi_private *cfi = map->fldrv_priv;
+	int retries = 10;
+	int i;
+
+	/*
+	 * If the driver thinks the chip is idle, and no toggle bits
+	 * are changing, then the chip is actually idle for sure.
+	 */
+	if (chip->state == FL_READY && chip_ready(map, adr))
+		return 0;
+
+	/*
+	 * Try several times to reset the chip and then wait for it
+	 * to become idle. The upper limit of a few milliseconds of
+	 * delay isn't a big problem: the kernel is dying anyway. It
+	 * is more important to save the messages.
+	 */
+	while (retries > 0) {
+		const unsigned long timeo = (HZ / 1000) + 1;
+
+		/* send the reset command */
+		map_write(map, CMD(0xF0), chip->start);
+
+		/* wait for the chip to become ready */
+		for (i = 0; i < jiffies_to_usecs(timeo); i++) {
+			if (chip_ready(map, adr))
+				return 0;
+
+			udelay(1);
+		}
+	}
+
+	/* the chip never became ready */
+	return -EBUSY;
+}
+
+/*
+ * Write out one word of data to a single flash chip during a kernel panic
+ *
+ * This is only called during the panic_write() path. When panic_write()
+ * is called, the kernel is in the process of a panic, and will soon be
+ * dead. Therefore we don't take any locks, and attempt to get access
+ * to the chip as soon as possible.
+ *
+ * The implementation of this routine is intentionally similar to
+ * do_write_oneword(), in order to ease code maintenance.
+ */
+static int do_panic_write_oneword(struct map_info *map, struct flchip *chip,
+				  unsigned long adr, map_word datum)
+{
+	const unsigned long uWriteTimeout = (HZ / 1000) + 1;
+	struct cfi_private *cfi = map->fldrv_priv;
+	int retry_cnt = 0;
+	map_word oldd;
+	int ret = 0;
+	int i;
+
+	adr += chip->start;
+
+	ret = cfi_amdstd_panic_wait(map, chip, adr);
+	if (ret)
+		return ret;
+
+	pr_debug("MTD %s(): PANIC WRITE 0x%.8lx(0x%.8lx)\n",
+			__func__, adr, datum.x[0]);
+
+	/*
+	 * Check for a NOP for the case when the datum to write is already
+	 * present - it saves time and works around buggy chips that corrupt
+	 * data at other locations when 0xff is written to a location that
+	 * already contains 0xff.
+	 */
+	oldd = map_read(map, adr);
+	if (map_word_equal(map, oldd, datum)) {
+		pr_debug("MTD %s(): NOP\n", __func__);
+		goto op_done;
+	}
+
+	ENABLE_VPP(map);
+
+retry:
+	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0xA0, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+	map_write(map, datum, adr);
+
+	for (i = 0; i < jiffies_to_usecs(uWriteTimeout); i++) {
+		if (chip_ready(map, adr))
+			break;
+
+		udelay(1);
+	}
+
+	if (!chip_good(map, adr, datum)) {
+		/* reset on all failures. */
+		map_write(map, CMD(0xF0), chip->start);
+		/* FIXME - should have reset delay before continuing */
+
+		if (++retry_cnt <= MAX_WORD_RETRIES)
+			goto retry;
+
+		ret = -EIO;
+	}
+
+op_done:
+	DISABLE_VPP(map);
+	return ret;
+}
+
+/*
+ * Write out some data during a kernel panic
+ *
+ * This is used by the mtdoops driver to save the dying messages from a
+ * kernel which has panic'd.
+ *
+ * This routine ignores all of the locking used throughout the rest of the
+ * driver, in order to ensure that the data gets written out no matter what
+ * state this driver (and the flash chip itself) was in when the kernel crashed.
+ *
+ * The implementation of this routine is intentionally similar to
+ * cfi_amdstd_write_words(), in order to ease code maintenance.
+ */
+static int cfi_amdstd_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
+				  size_t *retlen, const u_char *buf)
+{
+	struct map_info *map = mtd->priv;
+	struct cfi_private *cfi = map->fldrv_priv;
+	unsigned long ofs, chipstart;
+	int ret = 0;
+	int chipnum;
+
+	*retlen = 0;
+	if (!len)
+		return 0;
+
+	chipnum = to >> cfi->chipshift;
+	ofs = to - (chipnum << cfi->chipshift);
+	chipstart = cfi->chips[chipnum].start;
+
+	/* If it's not bus aligned, do the first byte write */
+	if (ofs & (map_bankwidth(map) - 1)) {
+		unsigned long bus_ofs = ofs & ~(map_bankwidth(map) - 1);
+		int i = ofs - bus_ofs;
+		int n = 0;
+		map_word tmp_buf;
+
+		ret = cfi_amdstd_panic_wait(map, &cfi->chips[chipnum], bus_ofs);
+		if (ret)
+			return ret;
+
+		/* Load 'tmp_buf' with old contents of flash */
+		tmp_buf = map_read(map, bus_ofs + chipstart);
+
+		/* Number of bytes to copy from buffer */
+		n = min_t(int, len, map_bankwidth(map) - i);
+
+		tmp_buf = map_word_load_partial(map, tmp_buf, buf, i, n);
+
+		ret = do_panic_write_oneword(map, &cfi->chips[chipnum],
+					     bus_ofs, tmp_buf);
+		if (ret)
+			return ret;
+
+		ofs += n;
+		buf += n;
+		(*retlen) += n;
+		len -= n;
+
+		if (ofs >> cfi->chipshift) {
+			chipnum++;
+			ofs = 0;
+			if (chipnum == cfi->numchips)
+				return 0;
+		}
+	}
+
+	/* We are now aligned, write as much as possible */
+	while (len >= map_bankwidth(map)) {
+		map_word datum;
+
+		datum = map_word_load(map, buf);
+
+		ret = do_panic_write_oneword(map, &cfi->chips[chipnum],
+					     ofs, datum);
+		if (ret)
+			return ret;
+
+		ofs += map_bankwidth(map);
+		buf += map_bankwidth(map);
+		(*retlen) += map_bankwidth(map);
+		len -= map_bankwidth(map);
+
+		if (ofs >> cfi->chipshift) {
+			chipnum++;
+			ofs = 0;
+			if (chipnum == cfi->numchips)
+				return 0;
+
+			chipstart = cfi->chips[chipnum].start;
+		}
+	}
+
+	/* Write the trailing bytes if any */
+	if (len & (map_bankwidth(map) - 1)) {
+		map_word tmp_buf;
+
+		ret = cfi_amdstd_panic_wait(map, &cfi->chips[chipnum], ofs);
+		if (ret)
+			return ret;
+
+		tmp_buf = map_read(map, ofs + chipstart);
+
+		tmp_buf = map_word_load_partial(map, tmp_buf, buf, 0, len);
+
+		ret = do_panic_write_oneword(map, &cfi->chips[chipnum],
+					     ofs, tmp_buf);
+		if (ret)
+			return ret;
+
+		(*retlen) += len;
+	}
+
+	return 0;
+}
+
 
 /*
  * Handle devices with one erase region, that only implement
@@ -1587,7 +1913,7 @@ static int __xipram do_erase_chip(struct map_info *map, struct flchip *chip)
 		return ret;
 	}
 
-	DEBUG( MTD_DEBUG_LEVEL3, "MTD %s(): ERASE 0x%.8lx\n",
+	pr_debug("MTD %s(): ERASE 0x%.8lx\n",
 	       __func__, chip->start );
 
 	XIP_INVAL_CACHED_RANGE(map, adr, map->size);
@@ -1652,6 +1978,7 @@ static int __xipram do_erase_chip(struct map_info *map, struct flchip *chip)
 
 	chip->state = FL_READY;
 	xip_enable(map, chip, adr);
+	DISABLE_VPP(map);
 	put_chip(map, chip, adr);
 	mutex_unlock(&chip->mutex);
 
@@ -1675,7 +2002,7 @@ static int __xipram do_erase_oneblock(struct map_info *map, struct flchip *chip,
 		return ret;
 	}
 
-	DEBUG( MTD_DEBUG_LEVEL3, "MTD %s(): ERASE 0x%.8lx\n",
+	pr_debug("MTD %s(): ERASE 0x%.8lx\n",
 	       __func__, adr );
 
 	XIP_INVAL_CACHED_RANGE(map, adr, len);
@@ -1742,6 +2069,7 @@ static int __xipram do_erase_oneblock(struct map_info *map, struct flchip *chip,
 	}
 
 	chip->state = FL_READY;
+	DISABLE_VPP(map);
 	put_chip(map, chip, adr);
 	mutex_unlock(&chip->mutex);
 	return ret;
@@ -1801,8 +2129,7 @@ static int do_atmel_lock(struct map_info *map, struct flchip *chip,
 		goto out_unlock;
 	chip->state = FL_LOCKING;
 
-	DEBUG(MTD_DEBUG_LEVEL3, "MTD %s(): LOCK 0x%08lx len %d\n",
-	      __func__, adr, len);
+	pr_debug("MTD %s(): LOCK 0x%08lx len %d\n", __func__, adr, len);
 
 	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi,
 			 cfi->device_type, NULL);
@@ -1837,8 +2164,7 @@ static int do_atmel_unlock(struct map_info *map, struct flchip *chip,
 		goto out_unlock;
 	chip->state = FL_UNLOCKING;
 
-	DEBUG(MTD_DEBUG_LEVEL3, "MTD %s(): LOCK 0x%08lx len %d\n",
-	      __func__, adr, len);
+	pr_debug("MTD %s(): LOCK 0x%08lx len %d\n", __func__, adr, len);
 
 	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi,
 			 cfi->device_type, NULL);
diff --git a/drivers/mxc/mlb/mxc_mlb150.c b/drivers/mxc/mlb/mxc_mlb150.c
index e3ecbe20446f..6b07d0f0a428 100755
--- a/drivers/mxc/mlb/mxc_mlb150.c
+++ b/drivers/mxc/mlb/mxc_mlb150.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -37,161 +37,173 @@
 #include <linux/delay.h>
 #include <linux/spinlock.h>
 #include <linux/sched.h>
+#include <linux/circ_buf.h>
 
 #define DRIVER_NAME "mxc_mlb150"
 
 /*!
  * MLB module memory map registers define
  */
-#define MLB150_REG_MLBC0		0x0
-#define MLB150_MLBC0_MLBEN		(0x1)
-#define MLB150_MLBC0_MLBCLK_MASK	(0x7 << 2)
-#define MLB150_MLBC0_MLBCLK_SHIFT	(2)
-#define MLB150_MLBC0_MLBPEN		(0x1 << 5)
-#define MLB150_MLBC0_MLBLK		(0x1 << 7)
-#define MLB150_MLBC0_ASYRETRY		(0x1 << 12)
-#define MLB150_MLBC0_CTLRETRY		(0x1 << 12)
-#define MLB150_MLBC0_FCNT_MASK		(0x7 << 15)
-#define MLB150_MLBC0_FCNT_SHIFT		(15)
-
-#define MLB150_REG_MLBPC0		0x8
-#define MLB150_MLBPC0_MCLKHYS		(0x1 << 11)
-
-#define MLB150_REG_MS0			0xC
-#define MLB150_REG_MS1			0x14
-
-#define MLB150_REG_MSS			0x20
-#define MLB150_MSS_RSTSYSCMD		(0x1)
-#define MLB150_MSS_LKSYSCMD		(0x1 << 1)
-#define MLB150_MSS_ULKSYSCMD		(0x1 << 2)
-#define MLB150_MSS_CSSYSCMD		(0x1 << 3)
-#define MLB150_MSS_SWSYSCMD		(0x1 << 4)
-#define MLB150_MSS_SERVREQ		(0x1 << 5)
-
-#define MLB150_REG_MSD			0x24
-
-#define MLB150_REG_MIEN			0x2C
-#define MLB150_MIEN_ISOC_PE		(0x1)
-#define MLB150_MIEN_ISOC_BUFO		(0x1 << 1)
-#define MLB150_MIEN_SYNC_PE		(0x1 << 16)
-#define MLB150_MIEN_ARX_DONE		(0x1 << 17)
-#define MLB150_MIEN_ARX_PE		(0x1 << 18)
-#define MLB150_MIEN_ARX_BREAK		(0x1 << 19)
-#define MLB150_MIEN_ATX_DONE		(0x1 << 20)
-#define MLB150_MIEN_ATX_PE		(0x1 << 21)
-#define MLB150_MIEN_ATX_BREAK		(0x1 << 22)
-#define MLB150_MIEN_CRX_DONE		(0x1 << 24)
-#define MLB150_MIEN_CRX_PE		(0x1 << 25)
-#define MLB150_MIEN_CRX_BREAK		(0x1 << 26)
-#define MLB150_MIEN_CTX_DONE		(0x1 << 27)
-#define MLB150_MIEN_CTX_PE		(0x1 << 28)
-#define MLB150_MIEN_CTX_BREAK		(0x1 << 29)
-
-#define MLB150_REG_MLBPC2		0x34
-#define MLB150_REG_MLBPC1		0x38
-#define MLB150_MLBPC1_VAL		(0x00000888)
-
-#define MLB150_REG_MLBC1		0x3C
-#define MLB150_MLBC1_LOCK		(0x1 << 6)
-#define MLB150_MLBC1_CLKM		(0x1 << 7)
-#define MLB150_MLBC1_NDA_MASK		(0xFF << 8)
-#define MLB150_MLBC1_NDA_SHIFT		(8)
-
-#define MLB150_REG_HCTL			0x80
-#define MLB150_HCTL_RST0		(0x1)
-#define MLB150_HCTL_RST1		(0x1 << 1)
-#define MLB150_HCTL_EN			(0x1 << 15)
-
-#define MLB150_REG_HCMR0		0x88
-#define MLB150_REG_HCMR1		0x8C
-#define MLB150_REG_HCER0		0x90
-#define MLB150_REG_HCER1		0x94
-#define MLB150_REG_HCBR0		0x98
-#define MLB150_REG_HCBR1		0x9C
-
-#define MLB150_REG_MDAT0		0xC0
-#define MLB150_REG_MDAT1		0xC4
-#define MLB150_REG_MDAT2		0xC8
-#define MLB150_REG_MDAT3		0xCC
-
-#define MLB150_REG_MDWE0		0xD0
-#define MLB150_REG_MDWE1		0xD4
-#define MLB150_REG_MDWE2		0xD8
-#define MLB150_REG_MDWE3		0xDC
-
-#define MLB150_REG_MCTL			0xE0
-#define MLB150_MCTL_XCMP		(0x1)
-
-#define MLB150_REG_MADR			0xE4
-#define MLB150_MADR_WNR			(0x1 << 31)
-#define MLB150_MADR_TB			(0x1 << 30)
-#define MLB150_MADR_ADDR_MASK		(0x7f << 8)
-#define MLB150_MADR_ADDR_SHIFT		(0)
-
-#define MLB150_REG_ACTL			0x3C0
-#define MLB150_ACTL_MPB			(0x1 << 4)
-#define MLB150_ACTL_DMAMODE		(0x1 << 2)
-#define MLB150_ACTL_SMX			(0x1 << 1)
-#define MLB150_ACTL_SCE			(0x1)
-
-#define MLB150_REG_ACSR0		0x3D0
-#define MLB150_REG_ACSR1		0x3D4
-#define MLB150_REG_ACMR0		0x3D8
-#define MLB150_REG_ACMR1		0x3DC
-
-#define MLB150_REG_CAT_MDATn(ch) (MLB150_REG_MDAT0 + ((ch % 8) >> 1) * 4)
-#define MLB150_REG_CAT_MDWEn(ch) (MLB150_REG_MDWE0 + ((ch % 8) >> 1) * 4)
-
-#define MLB150_LOGIC_CH_NUM		(64)
-#define MLB150_BUF_CDT_OFFSET		(0x0)
-#define MLB150_BUF_ADT_OFFSET		(0x40)
-#define MLB150_BUF_CAT_MLB_OFFSET	(0x80)
-#define MLB150_BUF_CAT_HBI_OFFSET	(0x88)
-#define MLB150_BUF_CTR_END_OFFSET	(0x8F)
-
-#define MLB150_CAT_MODE_RX		(0x1 << 0)
-#define MLB150_CAT_MODE_TX		(0x1 << 1)
-#define MLB150_CAT_MODE_INBOUND_DMA	(0x1 << 8)
-#define MLB150_CAT_MODE_OUTBOUND_DMA	(0x1 << 9)
-
-#define MLB150_CH_SYNC_BUF_DEP		(128 * 4 * 4)
-#define MLB150_CH_CTRL_BUF_DEP		(64)
-#define MLB150_CH_ASYNC_BUF_DEP		(2048)
-#define MLB150_CH_ISOC_BLK_SIZE		(196)
-#define MLB150_CH_ISOC_BLK_NUM		(3)
-#define MLB150_CH_ISOC_BUF_DEP		(MLB150_CH_ISOC_BLK_SIZE * MLB150_CH_ISOC_BLK_NUM)
-
-#define MLB150_CH_SYNC_DBR_BUF_OFFSET	(0x0)
-#define MLB150_CH_CTRL_DBR_BUF_OFFSET	(MLB150_CH_SYNC_DBR_BUF_OFFSET + 2 * MLB150_CH_SYNC_BUF_DEP)
-#define MLB150_CH_ASYNC_DBR_BUF_OFFSET	(MLB150_CH_CTRL_DBR_BUF_OFFSET + 2 * MLB150_CH_CTRL_BUF_DEP)
-#define MLB150_CH_ISOC_DBR_BUF_OFFSET	(MLB150_CH_ASYNC_DBR_BUF_OFFSET + 2 * MLB150_CH_ASYNC_BUF_DEP)
-
-static u32 mlb150_ch_packet_buf_size[4] = {
-	MLB150_CH_SYNC_BUF_DEP,
-	MLB150_CH_CTRL_BUF_DEP,
-	MLB150_CH_ASYNC_BUF_DEP,
-	MLB150_CH_ISOC_BUF_DEP
-};
-
-#define MLB150_DBR_BUF_START 0x00000
-
-#define MLB150_CDT_LEN			(16)
-#define MLB150_ADT_LEN			(16)
-#define MLB150_CAT_LEN			(2)
-
-#define MLB150_CDT_SZ		(MLB150_CDT_LEN * MLB150_LOGIC_CH_NUM)
-#define MLB150_ADT_SZ		(MLB150_ADT_LEN * MLB150_LOGIC_CH_NUM)
-#define MLB150_CAT_SZ		(MLB150_CAT_LEN * MLB150_LOGIC_CH_NUM * 2)
-
-#define MLB150_CDT_BASE(base)		(base + MLB150_BUF_CDT_OFFSET)
-#define MLB150_ADT_BASE(base)		(base + MLB150_BUF_ADT_OFFSET)
-#define MLB150_CAT_MLB_BASE(base)	(base + MLB150_BUF_CAT_MLB_OFFSET)
-#define MLB150_CAT_HBI_BASE(base)	(base + MLB150_BUF_CAT_HBI_OFFSET)
-
-#define MLB150_CDTn_ADDR(base, n)	(base + MLB150_BUF_CDT_OFFSET + n * MLB150_CDT_LEN)
-#define MLB150_ADTn_ADDR(base, n)	(base + MLB150_BUF_ADT_OFFSET + n * MLB150_ADT_LEN)
-#define MLB150_CATn_MLB_ADDR(base, n)	(base + MLB150_BUF_CAT_MLB_OFFSET + n * MLB150_CAT_LEN)
-#define MLB150_CATn_HBI_ADDR(base, n)	(base + MLB150_BUF_CAT_HBI_OFFSET + n * MLB150_CAT_LEN)
+#define REG_MLBC0		0x0
+#define MLBC0_MLBEN		(0x1)
+#define MLBC0_MLBCLK_MASK	(0x7 << 2)
+#define MLBC0_MLBCLK_SHIFT	(2)
+#define MLBC0_MLBPEN		(0x1 << 5)
+#define MLBC0_MLBLK		(0x1 << 7)
+#define MLBC0_ASYRETRY		(0x1 << 12)
+#define MLBC0_CTLRETRY		(0x1 << 12)
+#define MLBC0_FCNT_MASK		(0x7 << 15)
+#define MLBC0_FCNT_SHIFT	(15)
+
+#define REG_MLBPC0		0x8
+#define MLBPC0_MCLKHYS		(0x1 << 11)
+
+#define REG_MS0			0xC
+#define REG_MS1			0x14
+
+#define REG_MSS			0x20
+#define MSS_RSTSYSCMD		(0x1)
+#define MSS_LKSYSCMD		(0x1 << 1)
+#define MSS_ULKSYSCMD		(0x1 << 2)
+#define MSS_CSSYSCMD		(0x1 << 3)
+#define MSS_SWSYSCMD		(0x1 << 4)
+#define MSS_SERVREQ		(0x1 << 5)
+
+#define REG_MSD			0x24
+
+#define REG_MIEN		0x2C
+#define MIEN_ISOC_PE		(0x1)
+#define MIEN_ISOC_BUFO		(0x1 << 1)
+#define MIEN_SYNC_PE		(0x1 << 16)
+#define MIEN_ARX_DONE		(0x1 << 17)
+#define MIEN_ARX_PE		(0x1 << 18)
+#define MIEN_ARX_BREAK		(0x1 << 19)
+#define MIEN_ATX_DONE		(0x1 << 20)
+#define MIEN_ATX_PE		(0x1 << 21)
+#define MIEN_ATX_BREAK		(0x1 << 22)
+#define MIEN_CRX_DONE		(0x1 << 24)
+#define MIEN_CRX_PE		(0x1 << 25)
+#define MIEN_CRX_BREAK		(0x1 << 26)
+#define MIEN_CTX_DONE		(0x1 << 27)
+#define MIEN_CTX_PE		(0x1 << 28)
+#define MIEN_CTX_BREAK		(0x1 << 29)
+
+#define REG_MLBPC2		0x34
+#define REG_MLBPC1		0x38
+#define MLBPC1_VAL		(0x00000888)
+
+#define REG_MLBC1		0x3C
+#define MLBC1_LOCK		(0x1 << 6)
+#define MLBC1_CLKM		(0x1 << 7)
+#define MLBC1_NDA_MASK		(0xFF << 8)
+#define MLBC1_NDA_SHIFT		(8)
+
+#define REG_HCTL		0x80
+#define HCTL_RST0		(0x1)
+#define HCTL_RST1		(0x1 << 1)
+#define HCTL_EN			(0x1 << 15)
+
+#define REG_HCMR0		0x88
+#define REG_HCMR1		0x8C
+#define REG_HCER0		0x90
+#define REG_HCER1		0x94
+#define REG_HCBR0		0x98
+#define REG_HCBR1		0x9C
+
+#define REG_MDAT0		0xC0
+#define REG_MDAT1		0xC4
+#define REG_MDAT2		0xC8
+#define REG_MDAT3		0xCC
+
+#define REG_MDWE0		0xD0
+#define REG_MDWE1		0xD4
+#define REG_MDWE2		0xD8
+#define REG_MDWE3		0xDC
+
+#define REG_MCTL		0xE0
+#define MCTL_XCMP		(0x1)
+
+#define REG_MADR		0xE4
+#define MADR_WNR		(0x1 << 31)
+#define MADR_TB			(0x1 << 30)
+#define MADR_ADDR_MASK		(0x7f << 8)
+#define MADR_ADDR_SHIFT		(0)
+
+#define REG_ACTL		0x3C0
+#define ACTL_MPB		(0x1 << 4)
+#define ACTL_DMAMODE		(0x1 << 2)
+#define ACTL_SMX		(0x1 << 1)
+#define ACTL_SCE		(0x1)
+
+#define REG_ACSR0		0x3D0
+#define REG_ACSR1		0x3D4
+#define REG_ACMR0		0x3D8
+#define REG_ACMR1		0x3DC
+
+#define REG_CAT_MDATn(ch) (REG_MDAT0 + ((ch % 8) >> 1) * 4)
+#define REG_CAT_MDWEn(ch) (REG_MDWE0 + ((ch % 8) >> 1) * 4)
+
+#define INT_AHB0_CH_START	(0)
+#define INT_AHB1_CH_START	(32)
+
+#define LOGIC_CH_NUM		(64)
+#define BUF_CDT_OFFSET		(0x0)
+#define BUF_ADT_OFFSET		(0x40)
+#define BUF_CAT_MLB_OFFSET	(0x80)
+#define BUF_CAT_HBI_OFFSET	(0x88)
+#define BUF_CTR_END_OFFSET	(0x8F)
+
+#define CAT_MODE_RX		(0x1 << 0)
+#define CAT_MODE_TX		(0x1 << 1)
+#define CAT_MODE_INBOUND_DMA	(0x1 << 8)
+#define CAT_MODE_OUTBOUND_DMA	(0x1 << 9)
+
+#define CH_SYNC_DEFAULT_QUAD	(1)
+#define CH_SYNC_MAX_QUAD	(15)
+#define CH_SYNC_CDT_BUF_DEP	(CH_SYNC_DEFAULT_QUAD * 4 * 4)
+#define CH_SYNC_ADT_BUF_MULTI	(4)
+#define CH_SYNC_ADT_BUF_DEP	(CH_SYNC_CDT_BUF_DEP * CH_SYNC_ADT_BUF_MULTI)
+#define CH_SYNC_BUF_SZ		(CH_SYNC_MAX_QUAD * 4 * 4 * CH_SYNC_ADT_BUF_MULTI)
+#define CH_CTRL_CDT_BUF_DEP	(64)
+#define CH_CTRL_ADT_BUF_DEP	(CH_CTRL_CDT_BUF_DEP)
+#define CH_CTRL_BUF_SZ		(CH_CTRL_ADT_BUF_DEP)
+#define CH_ASYNC_MDP_PACKET_LEN	(1024)
+#define CH_ASYNC_MEP_PACKET_LEN	(1536)
+#define CH_ASYNC_CDT_BUF_DEP	(CH_ASYNC_MEP_PACKET_LEN)
+#define CH_ASYNC_ADT_BUF_DEP	(CH_ASYNC_CDT_BUF_DEP)
+#define CH_ASYNC_BUF_SZ		(CH_ASYNC_ADT_BUF_DEP)
+#define CH_ISOC_BLK_SIZE_188	(188)
+#define CH_ISOC_BLK_SIZE_196	(196)
+#define CH_ISOC_BLK_SIZE	(CH_ISOC_BLK_SIZE_188)
+#define CH_ISOC_BLK_NUM		(5)
+#define CH_ISOC_CDT_BUF_DEP	(CH_ISOC_BLK_SIZE * CH_ISOC_BLK_NUM)
+#define CH_ISOC_ADT_BUF_DEP	(CH_ISOC_CDT_BUF_DEP)
+#define CH_ISOC_BUF_SZ		(1024)
+
+#define CH_SYNC_DBR_BUF_OFFSET	(0x0)
+#define CH_CTRL_DBR_BUF_OFFSET	(CH_SYNC_DBR_BUF_OFFSET + 2 * (CH_SYNC_MAX_QUAD * 4 * 4))
+#define CH_ASYNC_DBR_BUF_OFFSET	(CH_CTRL_DBR_BUF_OFFSET + 2 * CH_CTRL_CDT_BUF_DEP)
+#define CH_ISOC_DBR_BUF_OFFSET	(CH_ASYNC_DBR_BUF_OFFSET + 2 * CH_ASYNC_CDT_BUF_DEP)
+
+#define DBR_BUF_START 0x00000
+
+#define CDT_LEN			(16)
+#define ADT_LEN			(16)
+#define CAT_LEN			(2)
+
+#define CDT_SZ			(CDT_LEN * LOGIC_CH_NUM)
+#define ADT_SZ			(ADT_LEN * LOGIC_CH_NUM)
+#define CAT_SZ			(CAT_LEN * LOGIC_CH_NUM * 2)
+
+#define CDT_BASE(base)		(base + BUF_CDT_OFFSET)
+#define ADT_BASE(base)		(base + BUF_ADT_OFFSET)
+#define CAT_MLB_BASE(base)	(base + BUF_CAT_MLB_OFFSET)
+#define CAT_HBI_BASE(base)	(base + BUF_CAT_HBI_OFFSET)
+
+#define CDTn_ADDR(base, n)	(base + BUF_CDT_OFFSET + n * CDT_LEN)
+#define ADTn_ADDR(base, n)	(base + BUF_ADT_OFFSET + n * ADT_LEN)
+#define CATn_MLB_ADDR(base, n)	(base + BUF_CAT_MLB_OFFSET + n * CAT_LEN)
+#define CATn_HBI_ADDR(base, n)	(base + BUF_CAT_HBI_OFFSET + n * CAT_LEN)
 
 #define CAT_CL_SHIFT		(0x0)
 #define CAT_CT_SHIFT		(8)
@@ -246,11 +258,6 @@ static u32 mlb150_ch_packet_buf_size[4] = {
 #define ADT_MEP1		(0x1 << 11)
 #define ADT_MEP2		(0x1 << 27)
 
-#define MLB_CONTROL_TX_CHANN	(0 << 4)
-#define MLB_CONTROL_RX_CHANN	(1 << 4)
-#define MLB_ASYNC_TX_CHANN	(2 << 4)
-#define MLB_ASYNC_RX_CHANN	(3 << 4)
-
 #define MLB_MINOR_DEVICES	4
 #define MLB_CONTROL_DEV_NAME	"ctrl"
 #define MLB_ASYNC_DEV_NAME	"async"
@@ -259,16 +266,8 @@ static u32 mlb150_ch_packet_buf_size[4] = {
 
 #define TX_CHANNEL		0
 #define RX_CHANNEL		1
-#define PING_BUF_MAX_SIZE	(2 * 1024)
-#define PONG_BUF_MAX_SIZE	(2 * 1024)
-/* max package data size */
-#define ASYNC_PACKET_SIZE	1024
-#define CTRL_PACKET_SIZE	64
-#define TRANS_RING_NODES	10
 
-#define MLB_IRAM_SIZE		(MLB_MINOR_DEVICES * (PING_BUF_MAX_SIZE + PONG_BUF_MAX_SIZE))
-#define _get_txchan(dev)	mlb_devinfo[dev].channels[TX_CHANNEL]
-#define _get_rxchan(dev)	mlb_devinfo[dev].channels[RX_CHANNEL]
+#define TRANS_RING_NODES	(1 << 3)
 
 enum MLB_CTYPE {
 	MLB_CTYPE_SYNC,
@@ -277,71 +276,34 @@ enum MLB_CTYPE {
 	MLB_CTYPE_ISOC,
 };
 
-enum MLB150_CLK_SPEED {
-	MLB150_CLK_256FS,
-	MLB150_CLK_512FS,
-	MLB150_CLK_1024FS,
-	MLB150_CLK_2048FS,
-	MLB150_CLK_3072FS,
-	MLB150_CLK_4096FS,
-	MLB150_CLK_6144FS,
-	MLB150_CLK_8192FS,
+enum CLK_SPEED {
+	CLK_256FS,
+	CLK_512FS,
+	CLK_1024FS,
+	CLK_2048FS,
+	CLK_3072FS,
+	CLK_4096FS,
+	CLK_6144FS,
+	CLK_8192FS,
 };
 
-/*!
- * Ring buffer
- */
-#define MLB_RING_BUF_INIT(r)	{	\
-	r->wpos = 0;	\
-	r->rpos = 0;	\
-}
-
-#define MLB_RING_BUF_IS_FULL(r) (((r->wpos + 1) % TRANS_RING_NODES) == r->rpos)
-#define MLB_RING_BUF_IS_EMPTY(r) (r->rpos == r->wpos)
-#define MLB_RING_BUF_ENQUE(r, buf) {	\
-	memcpy(r->node[r->wpos].data, buf, r->node.size);	\
-	r->wpos = (r->wpos + 1) % TRANS_RING_NODES;	\
-}
-#define MLB_RING_BUF_DEQUE(r, buf) {	\
-	memcpy(buf, r->node[r->rpos].data, r->node.size);	\
-	r->rpos = (r->rpos + 1) % TRANS_RING_NODES;	\
-}
-
 struct mlb_ringbuf {
-	u32 wpos;
-	u32 rpos;
-	u32 size;
-	/* Last buffer is for package drop */
-	u8  *virt_bufs[TRANS_RING_NODES + 1];
+	s8 *virt_bufs[TRANS_RING_NODES + 1];
 	u32 phy_addrs[TRANS_RING_NODES + 1];
+	s32 head;
+	s32 tail;
+	s32 unit_size;
+	s32 total_size;
+	rwlock_t rb_lock ____cacheline_aligned; /* ring index lock */
 };
 
 struct mlb_channel_info {
-
-	/* channel address */
-	s32 address;
+	/* Input MLB channel address */
+	u32 address;
+	/* Internal AHB channel label */
+	u32 cl;
 	/* DBR buf head */
 	u32 dbr_buf_head;
-	/* ping buffer head */
-	u32 ping_buf_head;
-	/* pong buffer head */
-	u32 pong_buf_head;
-	/* ping buffer physical head */
-	u32 ping_phy_head;
-	/* pong buffer physical head */
-	u32 pong_phy_head;
-	/* channel buffer size */
-	u32 buf_size;
-	/* channel buffer current ptr */
-	u32 buf_ptr;
-	/* channel buffer phy addr */
-	u32 buf_phy_addr;
-	/* packet start indicator */
-	u32 ps_ind;
-	/* packet remain size */
-	u32 pkt_remain_size;
-	/* buffer spin lock */
-	rwlock_t buf_lock;
 };
 
 struct mlb_dev_info {
@@ -351,11 +313,14 @@ struct mlb_dev_info {
 	/* channel type */
 	const unsigned int channel_type;
 	/* ch fps */
-	enum MLB150_CLK_SPEED fps;
+	enum CLK_SPEED fps;
 	/* channel info for tx/rx */
 	struct mlb_channel_info channels[2];
-	/* rx ring buffer */
-	struct mlb_ringbuf rx_bufs;
+	/* ring buffer */
+	u8 *rbuf_base_virt;
+	u32 rbuf_base_phy;
+	struct mlb_ringbuf rx_rbuf;
+	struct mlb_ringbuf tx_rbuf;
 	/* exception event */
 	unsigned long ex_event;
 	/* tx busy indicator */
@@ -365,37 +330,109 @@ struct mlb_dev_info {
 	/* device open count */
 	atomic_t opencnt;
 	/* wait queue head for channel */
-	wait_queue_head_t rd_wq;
-	wait_queue_head_t wt_wq;
+	wait_queue_head_t rx_wq;
+	wait_queue_head_t tx_wq;
+	/* TX OK */
+	s32 tx_ok;
 	/* spinlock for event access */
 	spinlock_t event_lock;
+	/* Block size for isoc mode
+	 * This variable can be configured in ioctl */
+	u32 isoc_blksz;
+	/* Quads number for sync mode
+	 * This variable can be confifured in ioctl */
+	u32 sync_quad;
+	/* Buffer depth in cdt */
+	u32 cdt_buf_dep;
+	/* Buffer depth in adt */
+	u32 adt_buf_dep;
+	/* Buffer size to hold data */
+	u32 buf_size;
+};
+
+struct mlb_data {
+	struct mlb_dev_info *devinfo;
+	struct clk *clk_mlb3p;
+	struct clk *clk_mlb6p;
+	struct cdev cdev;
+	struct class *class;	/* device class */
+	dev_t firstdev;
+#ifdef CONFIG_REGULATOR
+	struct regulator *nvcc;
+#endif
+	u32 membase;	/* mlb module base address */
+	u32 irq_ahb0;
+	u32 irq_ahb1;
+	u32 irq_mlb;
 };
 
+/* For optimization, we use fixed channel label for
+ * input channels of each mode */
+/* SYNC: CL = 0 for RX, CL = 64 for TX
+ * CTRL: CL = 1 for RX, CL = 65 for TX
+ * ASYNC: CL = 2 for RX, CL = 66 for TX
+ * ISOC: CL = 3 for RX, CL = 67 for TX
+ * */
+#define SYNC_RX_CL_AHB0		0
+#define CTRL_RX_CL_AHB0		1
+#define ASYNC_RX_CL_AHB0	2
+#define ISOC_RX_CL_AHB0		3
+#define SYNC_TX_CL_AHB0		4
+#define CTRL_TX_CL_AHB0		5
+#define ASYNC_TX_CL_AHB0	6
+#define ISOC_TX_CL_AHB0		7
+
+#define SYNC_RX_CL_AHB1		32
+#define CTRL_RX_CL_AHB1		33
+#define ASYNC_RX_CL_AHB1	34
+#define ISOC_RX_CL_AHB1		35
+#define SYNC_TX_CL_AHB1		36
+#define CTRL_TX_CL_AHB1		37
+#define ASYNC_TX_CL_AHB1	38
+#define ISOC_TX_CL_AHB1		39
+
+#define SYNC_RX_CL	SYNC_RX_CL_AHB0
+#define CTRL_RX_CL	CTRL_RX_CL_AHB0
+#define ASYNC_RX_CL	ASYNC_RX_CL_AHB0
+#define ISOC_RX_CL	ISOC_RX_CL_AHB0
+
+#define SYNC_TX_CL	SYNC_TX_CL_AHB0
+#define CTRL_TX_CL	CTRL_TX_CL_AHB0
+#define ASYNC_TX_CL	ASYNC_TX_CL_AHB0
+#define ISOC_TX_CL	ISOC_TX_CL_AHB0
+
 static struct mlb_dev_info mlb_devinfo[MLB_MINOR_DEVICES] = {
 	{
 	.dev_name = MLB_SYNC_DEV_NAME,
 	.channel_type = MLB_CTYPE_SYNC,
 	.channels = {
 		[0] = {
-			.buf_size = MLB150_CH_SYNC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_SYNC_DBR_BUF_OFFSET,
-			.buf_lock =
-				__RW_LOCK_UNLOCKED(mlb_devinfo[0].channels[0].
-					buf_lock),
+			.cl = SYNC_TX_CL,
+			.dbr_buf_head = CH_SYNC_DBR_BUF_OFFSET,
 		},
 		[1] = {
-			.buf_size = MLB150_CH_SYNC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_SYNC_DBR_BUF_OFFSET
-					+ MLB150_CH_SYNC_BUF_DEP,
-			.buf_lock =
-				__RW_LOCK_UNLOCKED(mlb_devinfo[0].channels[1].
-					buf_lock),
+			.cl = SYNC_RX_CL,
+			.dbr_buf_head = CH_SYNC_DBR_BUF_OFFSET
+					+ CH_SYNC_BUF_SZ,
 		},
 	},
+	.rx_rbuf = {
+		.unit_size = CH_SYNC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[0].rx_rbuf.rb_lock),
+	},
+	.tx_rbuf = {
+		.unit_size = CH_SYNC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[0].tx_rbuf.rb_lock),
+	},
+	.cdt_buf_dep = CH_SYNC_CDT_BUF_DEP,
+	.adt_buf_dep = CH_SYNC_ADT_BUF_DEP,
+	.buf_size = CH_SYNC_BUF_SZ,
 	.on = ATOMIC_INIT(0),
 	.opencnt = ATOMIC_INIT(0),
-	.rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].rd_wq),
-	.wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].wt_wq),
+	.rx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].rx_wq),
+	.tx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[0].tx_wq),
 	.event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[0].event_lock),
 	},
 	{
@@ -403,25 +440,32 @@ static struct mlb_dev_info mlb_devinfo[MLB_MINOR_DEVICES] = {
 	.channel_type = MLB_CTYPE_CTRL,
 	.channels = {
 		[0] = {
-			.buf_size = MLB150_CH_CTRL_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_CTRL_DBR_BUF_OFFSET,
-			.buf_lock =
-			__RW_LOCK_UNLOCKED(mlb_devinfo[1].channels[0].
-					buf_lock),
+			.cl = CTRL_TX_CL,
+			.dbr_buf_head = CH_CTRL_DBR_BUF_OFFSET,
 		},
 		[1] = {
-			.buf_size = MLB150_CH_CTRL_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_CTRL_DBR_BUF_OFFSET
-					+ MLB150_CH_CTRL_BUF_DEP,
-			.buf_lock =
-			__RW_LOCK_UNLOCKED(mlb_devinfo[1].channels[1].
-					buf_lock),
+			.cl = CTRL_RX_CL,
+			.dbr_buf_head = CH_CTRL_DBR_BUF_OFFSET
+					+ CH_CTRL_BUF_SZ,
 		},
 	},
+	.rx_rbuf = {
+		.unit_size = CH_CTRL_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[1].rx_rbuf.rb_lock),
+	},
+	.tx_rbuf = {
+		.unit_size = CH_CTRL_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[1].tx_rbuf.rb_lock),
+	},
+	.cdt_buf_dep = CH_CTRL_CDT_BUF_DEP,
+	.adt_buf_dep = CH_CTRL_ADT_BUF_DEP,
+	.buf_size = CH_CTRL_BUF_SZ,
 	.on = ATOMIC_INIT(0),
 	.opencnt = ATOMIC_INIT(0),
-	.rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].rd_wq),
-	.wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].wt_wq),
+	.rx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].rx_wq),
+	.tx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[1].tx_wq),
 	.event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[1].event_lock),
 	},
 	{
@@ -429,25 +473,32 @@ static struct mlb_dev_info mlb_devinfo[MLB_MINOR_DEVICES] = {
 	.channel_type = MLB_CTYPE_ASYNC,
 	.channels = {
 		[0] = {
-			.buf_size = MLB150_CH_ASYNC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_ASYNC_DBR_BUF_OFFSET,
-			.buf_lock =
-			__RW_LOCK_UNLOCKED(mlb_devinfo[2].channels[0].
-					buf_lock),
+			.cl = ASYNC_TX_CL,
+			.dbr_buf_head = CH_ASYNC_DBR_BUF_OFFSET,
 		},
 		[1] = {
-			.buf_size = MLB150_CH_ASYNC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_ASYNC_DBR_BUF_OFFSET
-					+ MLB150_CH_ASYNC_BUF_DEP,
-			.buf_lock =
-				__RW_LOCK_UNLOCKED(mlb_devinfo[2].channels[1].
-					buf_lock),
+			.cl = ASYNC_RX_CL,
+			.dbr_buf_head = CH_ASYNC_DBR_BUF_OFFSET
+					+ CH_ASYNC_BUF_SZ,
 		},
 	},
+	.rx_rbuf = {
+		.unit_size = CH_ASYNC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[2].rx_rbuf.rb_lock),
+	},
+	.tx_rbuf = {
+		.unit_size = CH_ASYNC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[2].tx_rbuf.rb_lock),
+	},
+	.cdt_buf_dep = CH_ASYNC_CDT_BUF_DEP,
+	.adt_buf_dep = CH_ASYNC_ADT_BUF_DEP,
+	.buf_size = CH_ASYNC_BUF_SZ,
 	.on = ATOMIC_INIT(0),
 	.opencnt = ATOMIC_INIT(0),
-	.rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[2].rd_wq),
-	.wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[2].wt_wq),
+	.rx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[2].rx_wq),
+	.tx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[2].tx_wq),
 	.event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[2].event_lock),
 	},
 	{
@@ -455,171 +506,98 @@ static struct mlb_dev_info mlb_devinfo[MLB_MINOR_DEVICES] = {
 	.channel_type = MLB_CTYPE_ISOC,
 	.channels = {
 		[0] = {
-			.buf_size = MLB150_CH_ISOC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_ISOC_DBR_BUF_OFFSET,
-			.buf_lock =
-				__RW_LOCK_UNLOCKED(mlb_devinfo[3].channels[0].
-					buf_lock),
+			.cl = ISOC_TX_CL,
+			.dbr_buf_head = CH_ISOC_DBR_BUF_OFFSET,
 		},
 		[1] = {
-			.buf_size = MLB150_CH_ISOC_BUF_DEP,
-			.dbr_buf_head = MLB150_CH_ISOC_DBR_BUF_OFFSET
-					+ MLB150_CH_ISOC_BUF_DEP,
-			.buf_lock =
-				__RW_LOCK_UNLOCKED(mlb_devinfo[3].channels[1].
-					buf_lock),
+			.cl = ISOC_RX_CL,
+			.dbr_buf_head = CH_ISOC_DBR_BUF_OFFSET
+					+ CH_ISOC_BUF_SZ,
 		},
 	},
+	.rx_rbuf = {
+		.unit_size = CH_ISOC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[3].rx_rbuf.rb_lock),
+	},
+	.tx_rbuf = {
+		.unit_size = CH_ISOC_BUF_SZ,
+		.rb_lock =
+			__RW_LOCK_UNLOCKED(mlb_devinfo[3].tx_rbuf.rb_lock),
+	},
+	.cdt_buf_dep = CH_ISOC_CDT_BUF_DEP,
+	.adt_buf_dep = CH_ISOC_ADT_BUF_DEP,
+	.buf_size = CH_ISOC_BUF_SZ,
 	.on = ATOMIC_INIT(0),
 	.opencnt = ATOMIC_INIT(0),
-	.rd_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[3].rd_wq),
-	.wt_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[3].wt_wq),
+	.rx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[3].rx_wq),
+	.tx_wq = __WAIT_QUEUE_HEAD_INITIALIZER(mlb_devinfo[3].tx_wq),
 	.event_lock = __SPIN_LOCK_UNLOCKED(mlb_devinfo[3].event_lock),
+	.isoc_blksz = CH_ISOC_BLK_SIZE_188,
 	},
 };
 
-static struct regulator *reg_nvcc;	/* NVCC_MLB regulator */
-static struct clk *mlb_clk;
-static struct clk *mlb_pll_clk;
-static dev_t dev;
-static struct class *mlb_class;	/* device class */
-static struct device *class_dev;
-static u32 mlb_base;	/* mlb module base address */
-static u32 ahb0_irq, ahb1_irq, mlb_irq;
+static u32 mlb_base;
 
 DEFINE_SPINLOCK(ctr_lock);
 
 #ifdef DEBUG
-
 #define DUMP_REG(reg) pr_debug(#reg": 0x%08x\n", __raw_readl(mlb_base + reg))
 
 static void mlb150_dev_dump_reg(void)
 {
 	pr_debug("mxc_mlb150: Dump registers:\n");
-	DUMP_REG(MLB150_REG_MLBC0);
-	DUMP_REG(MLB150_REG_MLBPC0);
-	DUMP_REG(MLB150_REG_MS0);
-	DUMP_REG(MLB150_REG_MS1);
-	DUMP_REG(MLB150_REG_MSS);
-	DUMP_REG(MLB150_REG_MSD);
-	DUMP_REG(MLB150_REG_MIEN);
-	DUMP_REG(MLB150_REG_MLBPC2);
-	DUMP_REG(MLB150_REG_MLBPC1);
-	DUMP_REG(MLB150_REG_MLBC1);
-	DUMP_REG(MLB150_REG_HCTL);
-	DUMP_REG(MLB150_REG_HCMR0);
-	DUMP_REG(MLB150_REG_HCMR1);
-	DUMP_REG(MLB150_REG_HCER0);
-	DUMP_REG(MLB150_REG_HCER1);
-	DUMP_REG(MLB150_REG_HCBR0);
-	DUMP_REG(MLB150_REG_HCBR1);
-	DUMP_REG(MLB150_REG_MDAT0);
-	DUMP_REG(MLB150_REG_MDAT1);
-	DUMP_REG(MLB150_REG_MDAT2);
-	DUMP_REG(MLB150_REG_MDAT3);
-	DUMP_REG(MLB150_REG_MDWE0);
-	DUMP_REG(MLB150_REG_MDWE1);
-	DUMP_REG(MLB150_REG_MDWE2);
-	DUMP_REG(MLB150_REG_MDWE3);
-	DUMP_REG(MLB150_REG_MCTL);
-	DUMP_REG(MLB150_REG_MADR);
-	DUMP_REG(MLB150_REG_ACTL);
-	DUMP_REG(MLB150_REG_ACSR0);
-	DUMP_REG(MLB150_REG_ACSR1);
-	DUMP_REG(MLB150_REG_ACMR0);
-	DUMP_REG(MLB150_REG_ACMR1);
+	DUMP_REG(REG_MLBC0);
+	DUMP_REG(REG_MLBPC0);
+	DUMP_REG(REG_MS0);
+	DUMP_REG(REG_MS1);
+	DUMP_REG(REG_MSS);
+	DUMP_REG(REG_MSD);
+	DUMP_REG(REG_MIEN);
+	DUMP_REG(REG_MLBPC2);
+	DUMP_REG(REG_MLBPC1);
+	DUMP_REG(REG_MLBC1);
+	DUMP_REG(REG_HCTL);
+	DUMP_REG(REG_HCMR0);
+	DUMP_REG(REG_HCMR1);
+	DUMP_REG(REG_HCER0);
+	DUMP_REG(REG_HCER1);
+	DUMP_REG(REG_HCBR0);
+	DUMP_REG(REG_HCBR1);
+	DUMP_REG(REG_MDAT0);
+	DUMP_REG(REG_MDAT1);
+	DUMP_REG(REG_MDAT2);
+	DUMP_REG(REG_MDAT3);
+	DUMP_REG(REG_MDWE0);
+	DUMP_REG(REG_MDWE1);
+	DUMP_REG(REG_MDWE2);
+	DUMP_REG(REG_MDWE3);
+	DUMP_REG(REG_MCTL);
+	DUMP_REG(REG_MADR);
+	DUMP_REG(REG_ACTL);
+	DUMP_REG(REG_ACSR0);
+	DUMP_REG(REG_ACSR1);
+	DUMP_REG(REG_ACMR0);
+	DUMP_REG(REG_ACMR1);
 }
 
 static void mlb150_dev_dump_hex(const u8 *buf, u32 len)
 {
-	u32 i, remain, round_len;
-
-	pr_debug("buf: 0x%08x, len: %d\n", (u32)buf, len);
-	remain = len & 0x7;
-	round_len = len - remain;
-	for (i = 0; i < round_len; i += 8) {
-		pr_debug("%02x %02x %02x %02x %02x %02x %02x %02x\n",
-			*(buf + i),
-			*(buf + i + 1),
-			*(buf + i + 2),
-			*(buf + i + 3),
-			*(buf + i + 4),
-			*(buf + i + 5),
-			*(buf + i + 6),
-			*(buf + i + 7));
-	}
-
-	if (remain) {
-		i = round_len;
-		switch (remain) {
-		case 1:
-			pr_debug("%02x\n",
-				*(buf + i));
-			break;
-		case 2:
-			pr_debug("%02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1));
-			break;
-		case 3:
-			pr_debug("%02x %02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1),
-				*(buf + i + 2));
-			break;
-		case 4:
-			pr_debug("%02x %02x %02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1),
-				*(buf + i + 2),
-				*(buf + i + 3));
-			break;
-		case 5:
-			pr_debug("%02x %02x %02x %02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1),
-				*(buf + i + 2),
-				*(buf + i + 3),
-				*(buf + i + 4));
-			break;
-		case 6:
-			pr_debug("%02x %02x %02x %02x %02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1),
-				*(buf + i + 2),
-				*(buf + i + 3),
-				*(buf + i + 4),
-				*(buf + i + 5));
-			break;
-		case 7:
-			pr_debug("%02x %02x %02x %02x %02x %02x %02x\n",
-				*(buf + i),
-				*(buf + i + 1),
-				*(buf + i + 2),
-				*(buf + i + 3),
-				*(buf + i + 4),
-				*(buf + i + 5),
-				*(buf + i + 6));
-			break;
-		default:
-			break;
-		}
-	}
-
-	if (i % 8 != 0)
-		pr_debug("\n");
+	print_hex_dump(KERN_DEBUG, "CTR DUMP:",
+			DUMP_PREFIX_OFFSET, 8, 1, buf, len, 0);
 }
 #endif
 
 static inline void mlb150_dev_enable_ctr_write(u32 mdat0_bits_en,
 		u32 mdat1_bits_en, u32 mdat2_bits_en, u32 mdat3_bits_en)
 {
-	__raw_writel(mdat0_bits_en, mlb_base + MLB150_REG_MDWE0);
-	__raw_writel(mdat1_bits_en, mlb_base + MLB150_REG_MDWE1);
-	__raw_writel(mdat2_bits_en, mlb_base + MLB150_REG_MDWE2);
-	__raw_writel(mdat3_bits_en, mlb_base + MLB150_REG_MDWE3);
+	__raw_writel(mdat0_bits_en, mlb_base + REG_MDWE0);
+	__raw_writel(mdat1_bits_en, mlb_base + REG_MDWE1);
+	__raw_writel(mdat2_bits_en, mlb_base + REG_MDWE2);
+	__raw_writel(mdat3_bits_en, mlb_base + REG_MDWE3);
 }
 
+#ifdef DEBUG
 static inline u8 mlb150_dev_dbr_read(u32 dbr_addr)
 {
 	s32 timeout = 1000;
@@ -627,20 +605,22 @@ static inline u8 mlb150_dev_dbr_read(u32 dbr_addr)
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctr_lock, flags);
-	__raw_writel(MLB150_MADR_TB | dbr_addr,
-		mlb_base + MLB150_REG_MADR);
+	__raw_writel(MADR_TB | dbr_addr,
+		mlb_base + REG_MADR);
 
-	while ((!(__raw_readl(mlb_base + MLB150_REG_MCTL)
-			& MLB150_MCTL_XCMP)) &&
+	while ((!(__raw_readl(mlb_base + REG_MCTL)
+			& MCTL_XCMP)) &&
 			timeout--)
 		;
 
-	if (unlikely(0 == timeout))
+	if (0 == timeout) {
+		spin_unlock_irqrestore(&ctr_lock, flags);
 		return -ETIME;
+	}
 
-	dbr_val = __raw_readl(mlb_base + MLB150_REG_MDAT0) & 0x000000ff;
+	dbr_val = __raw_readl(mlb_base + REG_MDAT0) & 0x000000ff;
 
-	__raw_writel(0, mlb_base + MLB150_REG_MCTL);
+	__raw_writel(0, mlb_base + REG_MCTL);
 	spin_unlock_irqrestore(&ctr_lock, flags);
 
 	return dbr_val;
@@ -653,49 +633,76 @@ static inline s32 mlb150_dev_dbr_write(u32 dbr_addr, u8 dbr_val)
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctr_lock, flags);
-	__raw_writel(mdat0, mlb_base + MLB150_REG_MDAT0);
+	__raw_writel(mdat0, mlb_base + REG_MDAT0);
 
-	__raw_writel(MLB150_MADR_WNR | MLB150_MADR_TB | dbr_addr,
-			mlb_base + MLB150_REG_MADR);
+	__raw_writel(MADR_WNR | MADR_TB | dbr_addr,
+			mlb_base + REG_MADR);
 
-	while ((!(__raw_readl(mlb_base + MLB150_REG_MCTL)
-			& MLB150_MCTL_XCMP)) &&
+	while ((!(__raw_readl(mlb_base + REG_MCTL)
+			& MCTL_XCMP)) &&
 			timeout--)
 		;
 
-	if (unlikely(timeout <= 0))
+	if (timeout <= 0) {
+		spin_unlock_irqrestore(&ctr_lock, flags);
 		return -ETIME;
+	}
 
-	__raw_writel(0, mlb_base + MLB150_REG_MCTL);
+	__raw_writel(0, mlb_base + REG_MCTL);
 	spin_unlock_irqrestore(&ctr_lock, flags);
 
 	return 0;
 }
 
+static inline s32 mlb150_dev_dbr_dump(u32 addr, u32 size)
+{
+	u8 *dump_buf = NULL;
+	u8 *buf_ptr = NULL;
+	s32 i;
+
+	dump_buf = kzalloc(size, GFP_KERNEL);
+	if (!dump_buf) {
+		pr_err("can't allocate enough memory\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0, buf_ptr = dump_buf;
+			i < size; ++i, ++buf_ptr)
+		*buf_ptr = mlb150_dev_dbr_read(addr + i);
+
+	mlb150_dev_dump_hex(dump_buf, size);
+
+	kfree(dump_buf);
+
+	return 0;
+}
+#endif
+
 static s32 mlb150_dev_ctr_read(u32 ctr_offset, u32 *ctr_val)
 {
 	s32 timeout = 1000;
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctr_lock, flags);
-	__raw_writel(ctr_offset, mlb_base + MLB150_REG_MADR);
+	__raw_writel(ctr_offset, mlb_base + REG_MADR);
 
-	while ((!(__raw_readl(mlb_base + MLB150_REG_MCTL)
-			& MLB150_MCTL_XCMP)) &&
+	while ((!(__raw_readl(mlb_base + REG_MCTL)
+			& MCTL_XCMP)) &&
 			timeout--)
 		;
 
-	if (unlikely(timeout <= 0)) {
+	if (timeout <= 0) {
+		spin_unlock_irqrestore(&ctr_lock, flags);
 		pr_debug("mxc_mlb150: Read CTR timeout\n");
 		return -ETIME;
 	}
 
-	ctr_val[0] = __raw_readl(mlb_base + MLB150_REG_MDAT0);
-	ctr_val[1] = __raw_readl(mlb_base + MLB150_REG_MDAT1);
-	ctr_val[2] = __raw_readl(mlb_base + MLB150_REG_MDAT2);
-	ctr_val[3] = __raw_readl(mlb_base + MLB150_REG_MDAT3);
+	ctr_val[0] = __raw_readl(mlb_base + REG_MDAT0);
+	ctr_val[1] = __raw_readl(mlb_base + REG_MDAT1);
+	ctr_val[2] = __raw_readl(mlb_base + REG_MDAT2);
+	ctr_val[3] = __raw_readl(mlb_base + REG_MDAT3);
 
-	__raw_writel(0, mlb_base + MLB150_REG_MCTL);
+	__raw_writel(0, mlb_base + REG_MCTL);
 
 	spin_unlock_irqrestore(&ctr_lock, flags);
 
@@ -709,25 +716,26 @@ static s32 mlb150_dev_ctr_write(u32 ctr_offset, const u32 *ctr_val)
 
 	spin_lock_irqsave(&ctr_lock, flags);
 
-	__raw_writel(ctr_val[0], mlb_base + MLB150_REG_MDAT0);
-	__raw_writel(ctr_val[1], mlb_base + MLB150_REG_MDAT1);
-	__raw_writel(ctr_val[2], mlb_base + MLB150_REG_MDAT2);
-	__raw_writel(ctr_val[3], mlb_base + MLB150_REG_MDAT3);
+	__raw_writel(ctr_val[0], mlb_base + REG_MDAT0);
+	__raw_writel(ctr_val[1], mlb_base + REG_MDAT1);
+	__raw_writel(ctr_val[2], mlb_base + REG_MDAT2);
+	__raw_writel(ctr_val[3], mlb_base + REG_MDAT3);
 
-	__raw_writel(MLB150_MADR_WNR | ctr_offset,
-			mlb_base + MLB150_REG_MADR);
+	__raw_writel(MADR_WNR | ctr_offset,
+			mlb_base + REG_MADR);
 
-	while ((!(__raw_readl(mlb_base + MLB150_REG_MCTL)
-			& MLB150_MCTL_XCMP)) &&
+	while ((!(__raw_readl(mlb_base + REG_MCTL)
+			& MCTL_XCMP)) &&
 			timeout--)
 		;
 
-	if (unlikely(timeout <= 0)) {
+	if (timeout <= 0) {
+		spin_unlock_irqrestore(&ctr_lock, flags);
 		pr_debug("mxc_mlb150: Write CTR timeout\n");
 		return -ETIME;
 	}
 
-	__raw_writel(0, mlb_base + MLB150_REG_MCTL);
+	__raw_writel(0, mlb_base + REG_MCTL);
 
 	spin_unlock_irqrestore(&ctr_lock, flags);
 
@@ -743,6 +751,13 @@ static s32 mlb150_dev_ctr_write(u32 ctr_offset, const u32 *ctr_val)
 				return 0;
 			else {
 				pr_debug("mxc_mlb150: ctr write failed\n");
+				pr_debug("offset: 0x%x\n", ctr_offset);
+				pr_debug("Write: 0x%x 0x%x 0x%x 0x%x\n",
+						ctr_val[3], ctr_val[2],
+						ctr_val[1], ctr_val[0]);
+				pr_debug("Read: 0x%x 0x%x 0x%x 0x%x\n",
+						ctr_rd[3], ctr_rd[2],
+						ctr_rd[1], ctr_rd[0]);
 				return -EBADE;
 			}
 		} else {
@@ -755,69 +770,88 @@ static s32 mlb150_dev_ctr_write(u32 ctr_offset, const u32 *ctr_val)
 	return 0;
 }
 
-static s32 mlb150_dev_get_adt_sts(u32 ch)
+#ifdef DEBUG
+static s32 mlb150_dev_cat_read(u32 ctr_offset, u32 ch, u16 *cat_val)
 {
-	s32 timeout = 1000;
-	unsigned long flags;
-	u32 reg;
-
-	spin_lock_irqsave(&ctr_lock, flags);
-	__raw_writel(MLB150_BUF_ADT_OFFSET + ch,
-			mlb_base + MLB150_REG_MADR);
-
-	while ((!(__raw_readl(mlb_base + MLB150_REG_MCTL)
-			& MLB150_MCTL_XCMP)) &&
-			timeout--)
-		;
+	u16 ctr_val[8] = { 0 };
 
-	if (unlikely(timeout <= 0)) {
-		pr_debug("mxc_mlb150: Read CTR timeout\n");
+	if (mlb150_dev_ctr_read(ctr_offset, (u32 *)ctr_val))
 		return -ETIME;
-	}
-
-	reg = __raw_readl(mlb_base + MLB150_REG_MDAT1);
-
-	__raw_writel(0, mlb_base + MLB150_REG_MCTL);
-	spin_unlock_irqrestore(&ctr_lock, flags);
 
-#ifdef DEBUG_ADT
-	pr_debug("mxc_mlb150: Get ch %d adt sts: 0x%08x\n", ch, reg);
-#endif
+	/*
+	 * Use u16 array to get u32 array value,
+	 * need to convert
+	 * */
+	cat_val = ctr_val[ch % 8];
 
-	return reg;
+	 return 0;
 }
+#endif
 
 static s32 mlb150_dev_cat_write(u32 ctr_offset, u32 ch, const u16 cat_val)
 {
 	u16 ctr_val[8] = { 0 };
 
-	if (unlikely(mlb150_dev_ctr_read(ctr_offset, (u32 *)ctr_val)))
+	if (mlb150_dev_ctr_read(ctr_offset, (u32 *)ctr_val))
 		return -ETIME;
 
 	ctr_val[ch % 8] = cat_val;
-	if (unlikely(mlb150_dev_ctr_write(ctr_offset, (u32 *)ctr_val)))
+	if (mlb150_dev_ctr_write(ctr_offset, (u32 *)ctr_val))
 		return -ETIME;
 
 	return 0;
 }
 
 #define mlb150_dev_cat_mlb_read(ch, cat_val)	\
-	mlb150_dev_cat_read(MLB150_BUF_CAT_MLB_OFFSET + (ch >> 3), ch, cat_val)
+	mlb150_dev_cat_read(BUF_CAT_MLB_OFFSET + (ch >> 3), ch, cat_val)
 #define mlb150_dev_cat_mlb_write(ch, cat_val)	\
-	mlb150_dev_cat_write(MLB150_BUF_CAT_MLB_OFFSET + (ch >> 3), ch, cat_val)
+	mlb150_dev_cat_write(BUF_CAT_MLB_OFFSET + (ch >> 3), ch, cat_val)
 #define mlb150_dev_cat_hbi_read(ch, cat_val)	\
-	mlb150_dev_cat_read(MLB150_BUF_CAT_HBI_OFFSET + (ch >> 3), ch, cat_val)
+	mlb150_dev_cat_read(BUF_CAT_HBI_OFFSET + (ch >> 3), ch, cat_val)
 #define mlb150_dev_cat_hbi_write(ch, cat_val)	\
-	mlb150_dev_cat_write(MLB150_BUF_CAT_HBI_OFFSET + (ch >> 3), ch, cat_val)
+	mlb150_dev_cat_write(BUF_CAT_HBI_OFFSET + (ch >> 3), ch, cat_val)
 
 #define mlb150_dev_cdt_read(ch, cdt_val)	\
-	mlb150_dev_ctr_read(MLB150_BUF_CDT_OFFSET + ch, cdt_val)
+	mlb150_dev_ctr_read(BUF_CDT_OFFSET + ch, cdt_val)
 #define mlb150_dev_cdt_write(ch, cdt_val)	\
-	mlb150_dev_ctr_write(MLB150_BUF_CDT_OFFSET + ch, cdt_val)
+	mlb150_dev_ctr_write(BUF_CDT_OFFSET + ch, cdt_val)
 #define mlb150_dev_adt_read(ch, adt_val)	\
-	mlb150_dev_ctr_read(MLB150_BUF_ADT_OFFSET + ch, adt_val)
+	mlb150_dev_ctr_read(BUF_ADT_OFFSET + ch, adt_val)
 #define mlb150_dev_adt_write(ch, adt_val)	\
-	mlb150_dev_ctr_write(MLB150_BUF_ADT_OFFSET + ch, adt_val)
+	mlb150_dev_ctr_write(BUF_ADT_OFFSET + ch, adt_val)
+
+static s32 mlb150_dev_get_adt_sts(u32 ch)
+{
+	s32 timeout = 1000;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&ctr_lock, flags);
+	__raw_writel(BUF_ADT_OFFSET + ch,
+			mlb_base + REG_MADR);
+
+	while ((!(__raw_readl(mlb_base + REG_MCTL)
+			& MCTL_XCMP)) &&
+			timeout--)
+		;
+
+	if (timeout <= 0) {
+		spin_unlock_irqrestore(&ctr_lock, flags);
+		pr_debug("mxc_mlb150: Read CTR timeout\n");
+		return -ETIME;
+	}
+
+	reg = __raw_readl(mlb_base + REG_MDAT1);
+
+	__raw_writel(0, mlb_base + REG_MCTL);
+	spin_unlock_irqrestore(&ctr_lock, flags);
+
+#ifdef DEBUG_ADT
+	pr_debug("mxc_mlb150: Get ch %d adt sts: 0x%08x\n", ch, reg);
+#endif
+
+	return reg;
+}
 
 #ifdef DEBUG
 static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
@@ -826,8 +860,8 @@ static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
 	u32 ctr_val[4] = { 0 };
 
 	pr_debug("mxc_mlb150: CDT Table");
-	for (i = MLB150_BUF_CDT_OFFSET + ch_start;
-			i < MLB150_BUF_CDT_OFFSET + ch_end;
+	for (i = BUF_CDT_OFFSET + ch_start;
+			i < BUF_CDT_OFFSET + ch_end;
 			++i) {
 		mlb150_dev_ctr_read(i, ctr_val);
 		pr_debug("CTR 0x%02x: 0x%08x, 0x%08x, 0x%08x, 0x%08x\n",
@@ -835,8 +869,8 @@ static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
 	}
 
 	pr_debug("mxc_mlb150: ADT Table");
-	for (i = MLB150_BUF_ADT_OFFSET + ch_start;
-			i < MLB150_BUF_ADT_OFFSET + ch_end;
+	for (i = BUF_ADT_OFFSET + ch_start;
+			i < BUF_ADT_OFFSET + ch_end;
 			++i) {
 		mlb150_dev_ctr_read(i, ctr_val);
 		pr_debug("CTR 0x%02x: 0x%08x, 0x%08x, 0x%08x, 0x%08x\n",
@@ -844,8 +878,8 @@ static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
 	}
 
 	pr_debug("mxc_mlb150: CAT MLB Table");
-	for (i = MLB150_BUF_CAT_MLB_OFFSET + (ch_start >> 3);
-			i < MLB150_BUF_CAT_MLB_OFFSET + (ch_end >> 3) + 1;
+	for (i = BUF_CAT_MLB_OFFSET + (ch_start >> 3);
+			i <= BUF_CAT_MLB_OFFSET + ((ch_end + 8) >> 3);
 			++i) {
 		mlb150_dev_ctr_read(i, ctr_val);
 		pr_debug("CTR 0x%02x: 0x%08x, 0x%08x, 0x%08x, 0x%08x\n",
@@ -853,8 +887,8 @@ static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
 	}
 
 	pr_debug("mxc_mlb150: CAT HBI Table");
-	for (i = MLB150_BUF_CAT_HBI_OFFSET + (ch_start >> 3);
-			i < MLB150_BUF_CAT_HBI_OFFSET + (ch_end >> 3) + 1;
+	for (i = BUF_CAT_HBI_OFFSET + (ch_start >> 3);
+			i <= BUF_CAT_HBI_OFFSET + ((ch_end + 8) >> 3);
 			++i) {
 		mlb150_dev_ctr_read(i, ctr_val);
 		pr_debug("CTR 0x%02x: 0x%08x, 0x%08x, 0x%08x, 0x%08x\n",
@@ -868,12 +902,25 @@ static void mlb150_dev_dump_ctr_tbl(u32 ch_start, u32 ch_end)
  */
 static inline s32 mlb150_dev_enable_dma_irq(u32 enable)
 {
+	u32 ch_rx_mask = (1 << SYNC_RX_CL_AHB0) | (1 << CTRL_RX_CL_AHB0)
+			| (1 << ASYNC_RX_CL_AHB0) | (1 << ISOC_RX_CL_AHB0)
+			| (1 << SYNC_TX_CL_AHB0) | (1 << CTRL_TX_CL_AHB0)
+			| (1 << ASYNC_TX_CL_AHB0) | (1 << ISOC_TX_CL_AHB0);
+	u32 ch_tx_mask = (1 << (SYNC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (CTRL_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ASYNC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ISOC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (SYNC_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (CTRL_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ASYNC_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ISOC_TX_CL_AHB1 - INT_AHB1_CH_START));
+
 	if (enable) {
-		__raw_writel(0xffffffff, mlb_base + MLB150_REG_ACMR0);
-		__raw_writel(0xffffffff, mlb_base + MLB150_REG_ACMR1);
+		__raw_writel(ch_rx_mask, mlb_base + REG_ACMR0);
+		__raw_writel(ch_tx_mask, mlb_base + REG_ACMR1);
 	} else {
-		__raw_writel(0x0, mlb_base + MLB150_REG_ACMR0);
-		__raw_writel(0x0, mlb_base + MLB150_REG_ACMR1);
+		__raw_writel(0x0, mlb_base + REG_ACMR0);
+		__raw_writel(0x0, mlb_base + REG_ACMR1);
 	}
 
 	return 0;
@@ -892,10 +939,10 @@ static s32 mlb150_dev_init_ir_amba_ahb(void)
 	 * ACTL.SCE = 0, hardware clears on read
 	 * ACTL.SCE = 1, software writes a '1' to clear */
 	/* We only support DMA MODE 1 */
-	reg = __raw_readl(mlb_base + MLB150_REG_ACTL);
-	reg |= MLB150_ACTL_DMAMODE;
-#ifdef MLB150_MULTIPLE_PACKAGE_MODE
-	reg |= MLB150_REG_ACTL_MPB;
+	reg = __raw_readl(mlb_base + REG_ACTL);
+	reg |= ACTL_DMAMODE;
+#ifdef MULTIPLE_PACKAGE_MODE
+	reg |= REG_ACTL_MPB;
 #endif
 
 	/* Step 3. Select 1 or 2 interrupt signals:
@@ -903,11 +950,9 @@ static s32 mlb150_dev_init_ir_amba_ahb(void)
 	 *	and another interrupt for channels 32 - 63 on ahb_init[1]
 	 * ACTL.SMX = 1: singel interrupt all channels on ahb_init[0]
 	 * */
-	/*
-	reg |= MLB150_ACTL_SMX;
-	*/
+	reg &= ~ACTL_SMX;
 
-	__raw_writel(reg, mlb_base + MLB150_REG_ACTL);
+	__raw_writel(reg, mlb_base + REG_ACTL);
 
 	return 0;
 }
@@ -916,155 +961,111 @@ static inline s32 mlb150_dev_enable_ir_mlb(u32 enable)
 {
 	/* Step 1, Select the MSn to be cleared by software,
 	 * writing a '0' to the appropriate bits */
-	__raw_writel(0, mlb_base + MLB150_REG_MS0);
-	__raw_writel(0, mlb_base + MLB150_REG_MS1);
+	__raw_writel(0, mlb_base + REG_MS0);
+	__raw_writel(0, mlb_base + REG_MS1);
 
 	/* Step 1, Program MIEN to enable protocol error
 	 * interrupts for all active MLB channels */
 	if (enable)
-		__raw_writel(MLB150_MIEN_CTX_PE |
-			MLB150_MIEN_CRX_PE | MLB150_MIEN_ATX_PE |
-			MLB150_MIEN_ARX_PE | MLB150_MIEN_SYNC_PE |
-			MLB150_MIEN_ISOC_PE,
-			mlb_base + MLB150_REG_MIEN);
+		__raw_writel(MIEN_CTX_PE |
+			MIEN_CRX_PE | MIEN_ATX_PE |
+			MIEN_ARX_PE | MIEN_SYNC_PE |
+			MIEN_ISOC_PE,
+			mlb_base + REG_MIEN);
 	else
-		__raw_writel(0, mlb_base + MLB150_REG_MIEN);
+		__raw_writel(0, mlb_base + REG_MIEN);
 
 	return 0;
 }
 
-static inline int mlb150_enable_pll(void)
+static inline int mlb150_enable_pll(struct mlb_data *drvdata)
 {
 	u32 c0_val;
 
-	__raw_writel(MLB150_MLBPC1_VAL,
-			mlb_base + MLB150_REG_MLBPC1);
+	__raw_writel(MLBPC1_VAL,
+			drvdata->membase + REG_MLBPC1);
 
-	c0_val = __raw_readl(mlb_base + MLB150_REG_MLBC0);
-	if (c0_val & MLB150_MLBC0_MLBPEN) {
-		c0_val &= ~MLB150_MLBC0_MLBPEN;
+	c0_val = __raw_readl(drvdata->membase + REG_MLBC0);
+	if (c0_val & MLBC0_MLBPEN) {
+		c0_val &= ~MLBC0_MLBPEN;
 		__raw_writel(c0_val,
-				mlb_base + MLB150_REG_MLBC0);
+				drvdata->membase + REG_MLBC0);
 	}
 
-	clk_enable(mlb_pll_clk);
+	clk_enable(drvdata->clk_mlb6p);
 
-	c0_val |= (MLB150_MLBC0_MLBPEN);
-	__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
+	c0_val |= (MLBC0_MLBPEN);
+	__raw_writel(c0_val, drvdata->membase + REG_MLBC0);
 
 	return 0;
 }
 
-static inline int mlb150_disable_pll(void)
+static inline int mlb150_disable_pll(struct mlb_data *drvdata)
 {
 	u32 c0_val;
 
-	clk_disable(mlb_pll_clk);
+	clk_disable(drvdata->clk_mlb6p);
 
-	c0_val = __raw_readl(mlb_base + MLB150_REG_MLBC0);
+	c0_val = __raw_readl(drvdata->membase + REG_MLBC0);
 
-	__raw_writel(0x0, mlb_base + MLB150_REG_MLBPC1);
+	__raw_writel(0x0, drvdata->membase + REG_MLBPC1);
 
-	c0_val &= ~MLB150_MLBC0_MLBPEN;
-	__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
+	c0_val &= ~MLBC0_MLBPEN;
+	__raw_writel(c0_val, drvdata->membase + REG_MLBC0);
 
 	return 0;
 }
 
-static void mlb150_dev_init(void)
-{
-	u32 c0_val, hctl_val;
-
-	/* Disable EN bits */
-	c0_val = __raw_readl(mlb_base + MLB150_REG_MLBC0);
-	c0_val &= ~MLB150_MLBC0_MLBEN;
-	__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
-
-	hctl_val = __raw_readl(mlb_base + MLB150_REG_HCTL);
-	hctl_val &= ~MLB150_HCTL_EN;
-	__raw_writel(hctl_val, mlb_base + MLB150_REG_HCTL);
-
-	/* Step 1, Configure the MediaLB interface */
-	/* Select pin mode and clock, 3-pin and 256fs */
-	c0_val = __raw_readl(mlb_base + MLB150_REG_MLBC0);
-	c0_val &= ~(MLB150_MLBC0_MLBPEN | MLB150_MLBC0_MLBCLK_MASK);
-	__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
-
-	c0_val |= MLB150_MLBC0_MLBEN;
-	__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
-
-	/* Step 2, Configure the HBI interface */
-	__raw_writel(0xffffffff, mlb_base + MLB150_REG_HCMR0);
-	__raw_writel(0xffffffff, mlb_base + MLB150_REG_HCMR1);
-	__raw_writel(MLB150_HCTL_EN, mlb_base + MLB150_REG_HCTL);
-
-	mlb150_dev_init_ir_amba_ahb();
-
-	mlb150_dev_enable_ir_mlb(1);
-}
-
 static s32 mlb150_dev_reset_cdt(void)
 {
 	int i = 0;
 	u32 ctr_val[4] = { 0 };
 
-	for (i = 0; i < (MLB150_LOGIC_CH_NUM); ++i)
-		mlb150_dev_ctr_write(MLB150_BUF_CDT_OFFSET + i, ctr_val);
+	mlb150_dev_enable_ctr_write(0xffffffff, 0xffffffff,
+			0xffffffff, 0xffffffff);
+
+	for (i = 0; i < (LOGIC_CH_NUM); ++i)
+		mlb150_dev_ctr_write(BUF_CDT_OFFSET + i, ctr_val);
 
 	return 0;
 }
 
-static s32 mlb150_dev_init_ch_cdt(u32 ch, enum MLB_CTYPE ctype, u32 ch_func)
+static s32 mlb150_dev_init_ch_cdt(struct mlb_dev_info *pdevinfo, u32 ch,
+		enum MLB_CTYPE ctype, u32 ch_func)
 {
 	u32 cdt_val[4] = { 0 };
 
 	/* a. Set the 14-bit base address (BA) */
 	pr_debug("mxc_mlb150: ctype: %d, ch: %d, dbr_buf_head: 0x%08x",
-		ctype, ch, mlb_devinfo[ctype].channels[ch_func].dbr_buf_head);
-	cdt_val[3] = (mlb_devinfo[ctype].channels[ch_func].dbr_buf_head)
+		ctype, ch, pdevinfo->channels[ch_func].dbr_buf_head);
+	cdt_val[3] = (pdevinfo->channels[ch_func].dbr_buf_head)
 			<< CDT_BA_SHIFT;
 
 	/* b. Set the 12-bit or 13-bit buffer depth (BD)
 	 * BD = buffer depth in bytes - 1 */
-	switch (ctype) {
-	case MLB_CTYPE_SYNC:
-		/* For synchronous channels: (BD + 1) = 4 * m * bpf */
-		cdt_val[3] |= (MLB150_CH_SYNC_BUF_DEP - 1) << CDT_BD_SHIFT;
-		break;
-	case MLB_CTYPE_CTRL:
-		/* For control channels: (BD + 1) >= max packet length (64) */
-		/* BD */
-		cdt_val[3] |= ((MLB150_CH_CTRL_BUF_DEP - 1) << CDT_BD_SHIFT);
-		break;
-	case MLB_CTYPE_ASYNC:
-		/* For asynchronous channels: (BD + 1) >= max packet length
-		 * 1024 for a MOST Data packet (MDP);
-		 * 1536 for a MOST Ethernet Packet (MEP) */
-		cdt_val[3] |= ((MLB150_CH_ASYNC_BUF_DEP - 1) << CDT_BD_SHIFT);
-		break;
-	case MLB_CTYPE_ISOC:
-		/* For isochronous channels: (BD + 1) mod (BS + 1) = 0 */
-		/* BS */
-		cdt_val[1] |= (MLB150_CH_ISOC_BLK_SIZE - 1);
-		/* BD */
-		cdt_val[3] |= (MLB150_CH_ISOC_BUF_DEP - 1)
-				<< CDT_BD_SHIFT;
-		break;
-	default:
-		break;
-	}
+	/* For synchronous channels: (BD + 1) = 4 * m * bpf */
+	/* For control channels: (BD + 1) >= max packet length (64) */
+	/* For asynchronous channels: (BD + 1) >= max packet length
+	 * 1024 for a MOST Data packet (MDP);
+	 * 1536 for a MOST Ethernet Packet (MEP) */
+	/* For isochronous channels: (BD + 1) mod (BS + 1) = 0 */
+	/* BS */
+	if (MLB_CTYPE_ISOC == ctype)
+		cdt_val[1] |= (pdevinfo->isoc_blksz - 1);
+	/* BD */
+	cdt_val[3] |= (pdevinfo->cdt_buf_dep - 1) << CDT_BD_SHIFT;
 
 	pr_debug("mxc_mlb150: Set CDT val of channel %d, type: %d: "
 		"0x%08x 0x%08x 0x%08x 0x%08x\n",
 		ch, ctype, cdt_val[3], cdt_val[2], cdt_val[1], cdt_val[0]);
 
-	if (unlikely(mlb150_dev_cdt_write(ch, cdt_val)))
+	if (mlb150_dev_cdt_write(ch, cdt_val))
 		return -ETIME;
 
 #ifdef DEBUG_CTR
 	{
 		u32 cdt_rd[4] = { 0 };
-		if (likely(!mlb150_dev_cdt_read(ch, cdt_rd))) {
+		if (!mlb150_dev_cdt_read(ch, cdt_rd)) {
 			pr_debug("mxc_mlb150: CDT val of channel %d: "
 				"0x%08x 0x%08x 0x%08x 0x%08x\n",
 				ch, cdt_rd[3], cdt_rd[2], cdt_rd[1], cdt_rd[0]);
@@ -1089,31 +1090,32 @@ static s32 mlb150_dev_init_ch_cdt(u32 ch, enum MLB_CTYPE ctype, u32 ch_func)
 	return 0;
 }
 
-static s32 mlb150_dev_init_ch_cat(u32 ch, u32 cat_mode, enum MLB_CTYPE ctype)
+static s32 mlb150_dev_init_ch_cat(u32 ch, u32 cl,
+		u32 cat_mode, enum MLB_CTYPE ctype)
 {
 	u16 cat_val = 0;
 #ifdef DEBUG_CTR
 	u16 cat_rd = 0;
 #endif
 
-	cat_val = CAT_CE | (ctype << CAT_CT_SHIFT) | ch;
+	cat_val = CAT_CE | (ctype << CAT_CT_SHIFT) | cl;
 
-	if (cat_mode & MLB150_CAT_MODE_OUTBOUND_DMA)
+	if (cat_mode & CAT_MODE_OUTBOUND_DMA)
 		cat_val |= CAT_RNW;
 
 	if (MLB_CTYPE_SYNC == ctype)
 		cat_val |= CAT_MT;
 
-	pr_debug("mxc_mlb150: set CAT val of channel %d, type: %d: 0x%04x\n",
+	switch (cat_mode) {
+	case CAT_MODE_RX | CAT_MODE_INBOUND_DMA:
+	case CAT_MODE_TX | CAT_MODE_OUTBOUND_DMA:
+		pr_debug("mxc_mlb150: set CAT val of channel %d, type: %d: 0x%04x\n",
 			ch, ctype, cat_val);
 
-	switch (cat_mode) {
-	case MLB150_CAT_MODE_RX | MLB150_CAT_MODE_INBOUND_DMA:
-	case MLB150_CAT_MODE_TX | MLB150_CAT_MODE_OUTBOUND_DMA:
-		if (unlikely(mlb150_dev_cat_mlb_write(ch, cat_val)))
+		if (mlb150_dev_cat_mlb_write(ch, cat_val))
 			return -ETIME;
 #ifdef DEBUG_CTR
-		if (likely(!mlb150_dev_cat_mlb_read(ch, &cat_rd)))
+		if (!mlb150_dev_cat_mlb_read(ch, &cat_rd))
 			pr_debug("mxc_mlb150: CAT val of mlb channel %d: 0x%04x",
 					ch, cat_rd);
 		else {
@@ -1123,17 +1125,20 @@ static s32 mlb150_dev_init_ch_cat(u32 ch, u32 cat_mode, enum MLB_CTYPE ctype)
 		}
 #endif
 		break;
-	case MLB150_CAT_MODE_TX | MLB150_CAT_MODE_INBOUND_DMA:
-	case MLB150_CAT_MODE_RX | MLB150_CAT_MODE_OUTBOUND_DMA:
-		if (unlikely(mlb150_dev_cat_hbi_write(ch, cat_val)))
+	case CAT_MODE_TX | CAT_MODE_INBOUND_DMA:
+	case CAT_MODE_RX | CAT_MODE_OUTBOUND_DMA:
+		pr_debug("mxc_mlb150: set CAT val of channel %d, type: %d: 0x%04x\n",
+			cl, ctype, cat_val);
+
+		if (mlb150_dev_cat_hbi_write(cl, cat_val))
 			return -ETIME;
 #ifdef DEBUG_CTR
-		if (likely(!mlb150_dev_cat_hbi_read(ch, &cat_rd)))
+		if (!mlb150_dev_cat_hbi_read(cl, &cat_rd))
 			pr_debug("mxc_mlb150: CAT val of hbi channel %d: 0x%04x",
-					ch, cat_rd);
+					cl, cat_rd);
 		else {
 			pr_debug("mxc_mlb150: Read CAT of hbi channel %d failed\n",
-					ch);
+					cl);
 				return -EBADE;
 		}
 #endif
@@ -1161,37 +1166,43 @@ static s32 mlb150_dev_reset_cat(void)
 	int i = 0;
 	u32 ctr_val[4] = { 0 };
 
-	for (i = 0; i < (MLB150_LOGIC_CH_NUM >> 3); ++i) {
-		mlb150_dev_ctr_write(MLB150_BUF_CAT_MLB_OFFSET + i, ctr_val);
-		mlb150_dev_ctr_write(MLB150_BUF_CAT_HBI_OFFSET + i, ctr_val);
+	mlb150_dev_enable_ctr_write(0xffffffff, 0xffffffff,
+			0xffffffff, 0xffffffff);
+
+	for (i = 0; i < (LOGIC_CH_NUM >> 3); ++i) {
+		mlb150_dev_ctr_write(BUF_CAT_MLB_OFFSET + i, ctr_val);
+		mlb150_dev_ctr_write(BUF_CAT_HBI_OFFSET + i, ctr_val);
 	}
 
 	return 0;
 }
 
-static s32 mlb150_dev_init_rfb(u32 rx_ch, u32 tx_ch, enum MLB_CTYPE ctype)
+static s32 mlb150_dev_init_rfb(struct mlb_dev_info *pdevinfo, u32 rx_ch,
+		u32 tx_ch, enum MLB_CTYPE ctype)
 {
+	u32 rx_cl = pdevinfo->channels[RX_CHANNEL].cl;
+	u32 tx_cl = pdevinfo->channels[TX_CHANNEL].cl;
 	/* Step 1, Initialize all bits of CAT to '0' */
 	mlb150_dev_reset_cat();
 	mlb150_dev_reset_cdt();
 
 	/* Step 2, Initialize logical channel */
 	/* Step 3, Program the CDT for channel N */
-	mlb150_dev_init_ch_cdt(rx_ch, ctype, RX_CHANNEL);
-	mlb150_dev_init_ch_cdt(tx_ch, ctype, TX_CHANNEL);
+	mlb150_dev_init_ch_cdt(pdevinfo, rx_cl, ctype, RX_CHANNEL);
+	mlb150_dev_init_ch_cdt(pdevinfo, tx_cl, ctype, TX_CHANNEL);
 
 	/* Step 4&5, Program the CAT for the inbound and outbound DMA */
-	mlb150_dev_init_ch_cat(rx_ch,
-			MLB150_CAT_MODE_RX | MLB150_CAT_MODE_INBOUND_DMA,
+	mlb150_dev_init_ch_cat(rx_ch, rx_cl,
+			CAT_MODE_RX | CAT_MODE_INBOUND_DMA,
 			ctype);
-	mlb150_dev_init_ch_cat(rx_ch,
-			MLB150_CAT_MODE_RX | MLB150_CAT_MODE_OUTBOUND_DMA,
+	mlb150_dev_init_ch_cat(rx_ch, rx_cl,
+			CAT_MODE_RX | CAT_MODE_OUTBOUND_DMA,
 			ctype);
-	mlb150_dev_init_ch_cat(tx_ch,
-			MLB150_CAT_MODE_TX | MLB150_CAT_MODE_INBOUND_DMA,
+	mlb150_dev_init_ch_cat(tx_ch, tx_cl,
+			CAT_MODE_TX | CAT_MODE_INBOUND_DMA,
 			ctype);
-	mlb150_dev_init_ch_cat(tx_ch,
-			MLB150_CAT_MODE_TX | MLB150_CAT_MODE_OUTBOUND_DMA,
+	mlb150_dev_init_ch_cat(tx_ch, tx_cl,
+			CAT_MODE_TX | CAT_MODE_OUTBOUND_DMA,
 			ctype);
 
 	return 0;
@@ -1202,14 +1213,82 @@ static s32 mlb150_dev_reset_adt(void)
 	int i = 0;
 	u32 ctr_val[4] = { 0 };
 
-	for (i = 0; i < (MLB150_LOGIC_CH_NUM); ++i)
-		mlb150_dev_ctr_write(MLB150_BUF_ADT_OFFSET + i, ctr_val);
+	mlb150_dev_enable_ctr_write(0xffffffff, 0xffffffff,
+			0xffffffff, 0xffffffff);
+
+	for (i = 0; i < (LOGIC_CH_NUM); ++i)
+		mlb150_dev_ctr_write(BUF_ADT_OFFSET + i, ctr_val);
+
+	return 0;
+}
+
+static s32 mlb150_dev_reset_whole_ctr(void)
+{
+	mlb150_dev_enable_ctr_write(0xffffffff, 0xffffffff,
+			0xffffffff, 0xffffffff);
+	mlb150_dev_reset_cdt();
+	mlb150_dev_reset_adt();
+	mlb150_dev_reset_cat();
+
+	return 0;
+}
+
+#define CLR_REG(reg)  __raw_writel(0x0, mlb_base + reg)
+
+static s32 mlb150_dev_reset_all_regs(void)
+{
+	CLR_REG(REG_MLBC0);
+	CLR_REG(REG_MLBPC0);
+	CLR_REG(REG_MS0);
+	CLR_REG(REG_MS1);
+	CLR_REG(REG_MSS);
+	CLR_REG(REG_MSD);
+	CLR_REG(REG_MIEN);
+	CLR_REG(REG_MLBPC2);
+	CLR_REG(REG_MLBPC1);
+	CLR_REG(REG_MLBC1);
+	CLR_REG(REG_HCTL);
+	CLR_REG(REG_HCMR0);
+	CLR_REG(REG_HCMR1);
+	CLR_REG(REG_HCER0);
+	CLR_REG(REG_HCER1);
+	CLR_REG(REG_HCBR0);
+	CLR_REG(REG_HCBR1);
+	CLR_REG(REG_MDAT0);
+	CLR_REG(REG_MDAT1);
+	CLR_REG(REG_MDAT2);
+	CLR_REG(REG_MDAT3);
+	CLR_REG(REG_MDWE0);
+	CLR_REG(REG_MDWE1);
+	CLR_REG(REG_MDWE2);
+	CLR_REG(REG_MDWE3);
+	CLR_REG(REG_MCTL);
+	CLR_REG(REG_MADR);
+	CLR_REG(REG_ACTL);
+	CLR_REG(REG_ACSR0);
+	CLR_REG(REG_ACSR1);
+	CLR_REG(REG_ACMR0);
+	CLR_REG(REG_ACMR1);
 
 	return 0;
 }
 
-static inline s32 mlb150_dev_set_ch_amba_ahb(u32 ch, enum MLB_CTYPE ctype,
-					u32 dne_sts, u32 buf_addr)
+static inline s32 mlb150_dev_pipo_start(struct mlb_ringbuf *rbuf,
+						u32 ahb_ch, u32 buf_addr)
+{
+	u32 ctr_val[4] = { 0 };
+
+	ctr_val[1] |= ADT_RDY1;
+	ctr_val[2] = buf_addr;
+
+	if (mlb150_dev_adt_write(ahb_ch, ctr_val))
+		return -ETIME;
+
+	return 0;
+}
+
+static inline s32 mlb150_dev_pipo_next(u32 ahb_ch, enum MLB_CTYPE ctype,
+				u32 dne_sts, u32 buf_addr)
 {
 	u32 ctr_val[4] = { 0 };
 
@@ -1229,54 +1308,38 @@ static inline s32 mlb150_dev_set_ch_amba_ahb(u32 ch, enum MLB_CTYPE ctype,
 		ctr_val[2] = buf_addr;
 	}
 
-#ifdef DEBUG_ADT
-	pr_debug("mxc_mlb150: Set ADT val of channel %d, ctype: %d: "
-		"0x%08x 0x%08x 0x%08x 0x%08x\n",
-		ch, ctype, ctr_val[3], ctr_val[2], ctr_val[1], ctr_val[0]);
-#endif
-
-	if (unlikely(mlb150_dev_adt_write(ch, ctr_val)))
+	if (mlb150_dev_adt_write(ahb_ch, ctr_val))
 		return -ETIME;
 
-#ifdef DEBUG_ADT_N
-	{
-		u32 ctr_rd[4] = { 0 };
-		if (likely(!mlb150_dev_adt_read(ch, ctr_rd))) {
-			pr_debug("mxc_mlb150: ADT val of channel %d: "
-				"0x%08x 0x%08x 0x%08x 0x%08x\n",
-				ch, ctr_rd[3], ctr_rd[2],
-				ctr_rd[1], ctr_rd[0]);
-			if (ctr_rd[3] == ctr_val[3] &&
-				ctr_rd[2] == ctr_val[2] &&
-				ctr_rd[1] == ctr_val[1] &&
-				ctr_rd[0] == ctr_val[0]) {
-				pr_debug("mxc_mlb150: set adt succeed!\n");
-				return 0;
-			} else {
-				pr_debug("mxc_mlb150: set adt failed!\n");
-				return -EBADE;
-			}
-		} else {
-			pr_debug("mxc_mlb150: Read ADT val of channel %d failed\n",
-					ch);
-			return -EBADE;
-		}
-	}
-#endif
+	return 0;
+}
+
+static inline s32 mlb150_dev_pipo_stop(struct mlb_ringbuf *rbuf, u32 ahb_ch)
+{
+	u32 ctr_val[4] = { 0 };
+	unsigned long flags;
+
+	write_lock_irqsave(&rbuf->rb_lock, flags);
+	rbuf->head = rbuf->tail = 0;
+	write_unlock_irqrestore(&rbuf->rb_lock, flags);
 
-      return 0;
+	if (mlb150_dev_adt_write(ahb_ch, ctr_val))
+		return -ETIME;
+
+	return 0;
 }
 
-static s32 mlb150_dev_init_ch_amba_ahb(struct mlb_channel_info *chinfo,
+static s32 mlb150_dev_init_ch_amba_ahb(struct mlb_dev_info *pdevinfo,
+					struct mlb_channel_info *chinfo,
 					enum MLB_CTYPE ctype)
 {
 	u32 ctr_val[4] = { 0 };
 
 	/* a. Set the 32-bit base address (BA1) */
-	ctr_val[3] = chinfo->pong_phy_head;
-	ctr_val[2] = chinfo->ping_phy_head;
-	ctr_val[1] = (chinfo->buf_size - 1) << ADT_BD1_SHIFT;
-	ctr_val[1] |= (chinfo->buf_size - 1) << ADT_BD2_SHIFT;
+	ctr_val[3] = 0;
+	ctr_val[2] = 0;
+	ctr_val[1] = (pdevinfo->adt_buf_dep - 1) << ADT_BD1_SHIFT;
+	ctr_val[1] |= (pdevinfo->adt_buf_dep - 1) << ADT_BD2_SHIFT;
 	if (MLB_CTYPE_ASYNC == ctype ||
 		MLB_CTYPE_CTRL == ctype) {
 		ctr_val[1] |= ADT_PS1;
@@ -1287,19 +1350,19 @@ static s32 mlb150_dev_init_ch_amba_ahb(struct mlb_channel_info *chinfo,
 
 	pr_debug("mxc_mlb150: Set ADT val of channel %d, ctype: %d: "
 		"0x%08x 0x%08x 0x%08x 0x%08x\n",
-		chinfo->address, ctype, ctr_val[3], ctr_val[2],
+		chinfo->cl, ctype, ctr_val[3], ctr_val[2],
 		ctr_val[1], ctr_val[0]);
 
-	if (unlikely(mlb150_dev_adt_write(chinfo->address, ctr_val)))
+	if (mlb150_dev_adt_write(chinfo->cl, ctr_val))
 		return -ETIME;
 
 #ifdef DEBUG_CTR
 	{
 		u32 ctr_rd[4] = { 0 };
-		if (likely(!mlb150_dev_adt_read(chinfo->address, ctr_rd))) {
+		if (!mlb150_dev_adt_read(chinfo->cl, ctr_rd)) {
 			pr_debug("mxc_mlb150: ADT val of channel %d: "
 				"0x%08x 0x%08x 0x%08x 0x%08x\n",
-				chinfo->address, ctr_rd[3], ctr_rd[2],
+				chinfo->cl, ctr_rd[3], ctr_rd[2],
 				ctr_rd[1], ctr_rd[0]);
 			if (ctr_rd[3] == ctr_val[3] &&
 				ctr_rd[2] == ctr_val[2] &&
@@ -1313,7 +1376,7 @@ static s32 mlb150_dev_init_ch_amba_ahb(struct mlb_channel_info *chinfo,
 			}
 		} else {
 			pr_debug("mxc_mlb150: Read ADT val of channel %d failed\n",
-					chinfo->address);
+					chinfo->cl);
 			return -EBADE;
 		}
 	}
@@ -1322,263 +1385,359 @@ static s32 mlb150_dev_init_ch_amba_ahb(struct mlb_channel_info *chinfo,
 	return 0;
 }
 
-static s32 mlb150_dev_init_amba_ahb(struct mlb_channel_info *rx_chinfo,
-		struct mlb_channel_info *tx_chinfo, enum MLB_CTYPE ctype)
+static s32 mlb150_dev_init_amba_ahb(struct mlb_dev_info *pdevinfo,
+					enum MLB_CTYPE ctype)
 {
+	struct mlb_channel_info *tx_chinfo = &pdevinfo->channels[TX_CHANNEL];
+	struct mlb_channel_info *rx_chinfo = &pdevinfo->channels[RX_CHANNEL];
+
 	/* Step 1, Initialize all bits of the ADT to '0' */
 	mlb150_dev_reset_adt();
 
 	/* Step 2, Select a logic channel */
 	/* Step 3, Program the AMBA AHB block ping page for channel N */
 	/* Step 4, Program the AMBA AHB block pong page for channel N */
-	mlb150_dev_init_ch_amba_ahb(rx_chinfo, ctype);
-	mlb150_dev_init_ch_amba_ahb(tx_chinfo, ctype);
+	mlb150_dev_init_ch_amba_ahb(pdevinfo, rx_chinfo, ctype);
+	mlb150_dev_init_ch_amba_ahb(pdevinfo, tx_chinfo, ctype);
 
 	return 0;
 }
 
-static s32 mlb150_dev_unmute_syn_ch(u32 rx_ch, u32 tx_ch)
+static void mlb150_dev_exit(void)
+{
+	u32 c0_val, hctl_val;
+
+	/* Disable EN bits */
+	c0_val = __raw_readl(mlb_base + REG_MLBC0);
+	c0_val &= ~(MLBC0_MLBEN | MLBC0_MLBPEN);
+	__raw_writel(c0_val, mlb_base + REG_MLBC0);
+
+	hctl_val = __raw_readl(mlb_base + REG_HCTL);
+	hctl_val &= ~HCTL_EN;
+	__raw_writel(hctl_val, mlb_base + REG_HCTL);
+
+	__raw_writel(0x0, mlb_base + REG_HCMR0);
+	__raw_writel(0x0, mlb_base + REG_HCMR1);
+
+	mlb150_dev_enable_dma_irq(0);
+	mlb150_dev_enable_ir_mlb(0);
+}
+
+static void mlb150_dev_init(void)
+{
+	u32 c0_val;
+	u32 ch_rx_mask = (1 << SYNC_RX_CL_AHB0) | (1 << CTRL_RX_CL_AHB0)
+			| (1 << ASYNC_RX_CL_AHB0) | (1 << ISOC_RX_CL_AHB0)
+			| (1 << SYNC_TX_CL_AHB0) | (1 << CTRL_TX_CL_AHB0)
+			| (1 << ASYNC_TX_CL_AHB0) | (1 << ISOC_TX_CL_AHB0);
+	u32 ch_tx_mask = (1 << (SYNC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (CTRL_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ASYNC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ISOC_RX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (SYNC_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (CTRL_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ASYNC_TX_CL_AHB1 - INT_AHB1_CH_START)) |
+			(1 << (ISOC_TX_CL_AHB1 - INT_AHB1_CH_START));
+
+	/* Disable EN bits */
+	mlb150_dev_exit();
+
+	/* Step 1. Initialize CTR and registers
+	 * a. Set all bit of the CTR (CAT, CDT, and ADT) to 0. */
+	mlb150_dev_reset_whole_ctr();
+
+	/* a. Set all bit of the CTR (CAT, CDT, and ADT) to 0. */
+	mlb150_dev_reset_all_regs();
+
+	/* Step 2, Configure the MediaLB interface */
+	/* Select pin mode and clock, 3-pin and 256fs */
+	c0_val = __raw_readl(mlb_base + REG_MLBC0);
+	c0_val &= ~(MLBC0_MLBPEN | MLBC0_MLBCLK_MASK);
+	__raw_writel(c0_val, mlb_base + REG_MLBC0);
+
+	c0_val |= MLBC0_MLBEN;
+	__raw_writel(c0_val, mlb_base + REG_MLBC0);
+
+	/* Step 3, Configure the HBI interface */
+	__raw_writel(ch_rx_mask, mlb_base + REG_HCMR0);
+	__raw_writel(ch_tx_mask, mlb_base + REG_HCMR1);
+	__raw_writel(HCTL_EN, mlb_base + REG_HCTL);
+
+	mlb150_dev_init_ir_amba_ahb();
+
+	mlb150_dev_enable_ir_mlb(1);
+}
+
+static s32 mlb150_dev_unmute_syn_ch(u32 rx_ch, u32 rx_cl, u32 tx_ch, u32 tx_cl)
 {
 	u32 timeout = 10000;
 
 	/* Check that MediaLB clock is running (MLBC1.CLKM = 0)
 	 * If MLBC1.CLKM = 1, clear the register bit, wait one
 	 * APB or I/O clock cycle and repeat the check */
-	while ((__raw_readl(mlb_base + MLB150_REG_MLBC1) & MLB150_MLBC1_CLKM)
+	while ((__raw_readl(mlb_base + REG_MLBC1) & MLBC1_CLKM)
 			|| timeout--)
-		__raw_writel(~MLB150_MLBC1_CLKM, mlb_base + MLB150_REG_MLBC1);
+		__raw_writel(~MLBC1_CLKM, mlb_base + REG_MLBC1);
 
-	if (unlikely(0 == timeout))
+	if (0 == timeout)
 		return -ETIME;
 
 	timeout = 10000;
 	/* Poll for MLB lock (MLBC0.MLBLK = 1) */
-	while (!(__raw_readl(mlb_base + MLB150_REG_MLBC0) & MLB150_MLBC0_MLBLK)
+	while (!(__raw_readl(mlb_base + REG_MLBC0) & MLBC0_MLBLK)
 			|| timeout--)
 		;
 
-	if (unlikely(0 == timeout))
+	if (0 == timeout)
 		return -ETIME;
 
 	/* Unmute synchronous channel(s) */
-	mlb150_dev_cat_mlb_write(rx_ch, CAT_CE | rx_ch);
+	mlb150_dev_cat_mlb_write(rx_ch, CAT_CE | rx_cl);
 	mlb150_dev_cat_mlb_write(tx_ch,
-			CAT_CE | tx_ch | CAT_RNW);
-	mlb150_dev_cat_hbi_write(rx_ch,
-			CAT_CE | rx_ch | CAT_RNW);
-	mlb150_dev_cat_hbi_write(tx_ch, CAT_CE | tx_ch);
+			CAT_CE | tx_cl | CAT_RNW);
+	mlb150_dev_cat_hbi_write(rx_cl,
+			CAT_CE | rx_cl | CAT_RNW);
+	mlb150_dev_cat_hbi_write(tx_cl, CAT_CE | tx_cl);
 
 	return 0;
 }
 
-static void mlb150_dev_exit(void)
+/* In case the user calls channel shutdown, but rx or tx is not completed yet */
+static s32 mlb150_trans_complete_check(struct mlb_dev_info *pdevinfo)
 {
-	mlb150_dev_enable_dma_irq(0);
-	mlb150_dev_enable_ir_mlb(0);
+	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_rbuf;
+	struct mlb_ringbuf *tx_rbuf = &pdevinfo->tx_rbuf;
+	s32 timeout = 1024;
+
+	while (timeout--) {
+		read_lock(&tx_rbuf->rb_lock);
+		if (!CIRC_CNT(tx_rbuf->head, tx_rbuf->tail, TRANS_RING_NODES)) {
+			read_unlock(&tx_rbuf->rb_lock);
+			break;
+		} else
+			read_unlock(&tx_rbuf->rb_lock);
+	}
 
-	__raw_writel(0, mlb_base + MLB150_REG_HCTL);
-	__raw_writel(0, mlb_base + MLB150_REG_MLBC0);
-}
+	if (timeout <= 0) {
+		pr_debug("TX complete check timeout!\n");
+		return -ETIME;
+	}
 
-/*!
- * MLB receive start function
- *
- * load phy_head to next buf register to start next rx
- * here use single-packet buffer, set start=end
- */
-static inline void mlb_start_rx(u32 ch, s32 ctype, u32 dne_sts, u32 buf_addr)
-{
-	/*  Set ADT for RX */
-	mlb150_dev_set_ch_amba_ahb(ch, ctype, dne_sts, buf_addr);
-}
+	timeout = 1024;
+	while (timeout--) {
+		read_lock(&rx_rbuf->rb_lock);
+		if (!CIRC_CNT(rx_rbuf->head, rx_rbuf->tail, TRANS_RING_NODES)) {
+			read_unlock(&rx_rbuf->rb_lock);
+			break;
+		} else
+			read_unlock(&rx_rbuf->rb_lock);
+	}
 
-/*!
- * MLB transmit start function
- * make sure aquiring the rw buf_lock, when calling this
- */
-static inline void mlb_start_tx(u32 ch, s32 ctype, u32 dne_sts, u32 buf_addr)
-{
-	/*  Set ADT for TX */
-	mlb150_dev_set_ch_amba_ahb(ch, ctype, dne_sts, buf_addr);
+	if (timeout <= 0) {
+		pr_debug("RX complete check timeout!\n");
+		return -ETIME;
+	}
+
+	return 0;
 }
 
 /*!
  * Enable the MLB channel
  */
-static void mlb_channel_enable(int chan_dev_id, int on)
+static void mlb_channel_enable(struct mlb_data *drvdata,
+				int chan_dev_id, int on)
 {
-	struct mlb_dev_info *pdevinfo = &mlb_devinfo[chan_dev_id];
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
+	struct mlb_channel_info *tx_chinfo = &pdevinfo->channels[TX_CHANNEL];
+	struct mlb_channel_info *rx_chinfo = &pdevinfo->channels[RX_CHANNEL];
+	u32 tx_ch = tx_chinfo->address;
+	u32 rx_ch = rx_chinfo->address;
+	u32 tx_cl = tx_chinfo->cl;
+	u32 rx_cl = rx_chinfo->cl;
+
 	/*!
 	 * setup the direction, enable, channel type,
 	 * mode select, channel address and mask buf start
 	 */
 	if (on) {
 		u32 ctype = pdevinfo->channel_type;
-		struct mlb_channel_info *tx_chinfo = &_get_txchan(chan_dev_id);
-		struct mlb_channel_info *rx_chinfo = &_get_rxchan(chan_dev_id);
-		u32 tx_ch = tx_chinfo->address;
-		u32 rx_ch = rx_chinfo->address;
 
 		mlb150_dev_enable_ctr_write(0xffffffff, 0xffffffff,
 				0xffffffff, 0xffffffff);
-		mlb150_dev_init_rfb(rx_ch, tx_ch, ctype);
+		mlb150_dev_init_rfb(pdevinfo, rx_ch, tx_ch, ctype);
 
-		mlb150_dev_init_amba_ahb(rx_chinfo, tx_chinfo, ctype);
+		mlb150_dev_init_amba_ahb(pdevinfo, ctype);
 
+#ifdef DEBUG
+		mlb150_dev_dump_ctr_tbl(0, tx_chinfo->cl + 1);
+#endif
 		/* Synchronize and unmute synchrouous channel */
 		if (MLB_CTYPE_SYNC == ctype)
-			mlb150_dev_unmute_syn_ch(rx_ch, tx_ch);
+			mlb150_dev_unmute_syn_ch(rx_ch, rx_cl, tx_ch, tx_cl);
 
 		mlb150_dev_enable_ctr_write(0x0, ADT_RDY1 | ADT_DNE1 |
 				ADT_ERR1 | ADT_PS1 |
-				ADT_MEP1 | ADT_RDY2 | ADT_DNE2 | ADT_ERR2 |
-				ADT_PS2 | ADT_MEP2,
+				ADT_RDY2 | ADT_DNE2 | ADT_ERR2 | ADT_PS2,
 				0xffffffff, 0xffffffff);
 
-		if (pdevinfo->fps >= MLB150_CLK_2048FS)
-			mlb150_enable_pll();
+		if (pdevinfo->fps >= CLK_2048FS)
+			mlb150_enable_pll(drvdata);
 
 		atomic_set(&pdevinfo->on, 1);
 
 #ifdef DEBUG
 		mlb150_dev_dump_reg();
-		mlb150_dev_dump_ctr_tbl(0, tx_chinfo->address + 1);
+		mlb150_dev_dump_ctr_tbl(0, tx_chinfo->cl + 1);
 #endif
-		mlb_start_rx(rx_ch, ctype, ADT_DNE2,
-				pdevinfo->rx_bufs.phy_addrs[0]);
+		/* Init RX ADT */
+		mlb150_dev_pipo_start(&pdevinfo->rx_rbuf, rx_cl,
+					pdevinfo->rx_rbuf.phy_addrs[0]);
 	} else {
+		mlb150_dev_pipo_stop(&pdevinfo->rx_rbuf, rx_cl);
+
 		mlb150_dev_enable_dma_irq(0);
 		mlb150_dev_enable_ir_mlb(0);
 
 		mlb150_dev_reset_cat();
 
-		atomic_set(&mlb_devinfo[chan_dev_id].on, 0);
+		atomic_set(&pdevinfo->on, 0);
 
-		if (mlb_devinfo[chan_dev_id].fps >= MLB150_CLK_2048FS)
-			mlb150_disable_pll();
+		if (pdevinfo->fps >= CLK_2048FS)
+			mlb150_disable_pll(drvdata);
 	}
 }
 
 /*!
  * MLB interrupt handler
  */
-static void mlb_tx_isr(int minor)
-{
-	struct mlb_dev_info *pdevinfo = &mlb_devinfo[minor];
-
-	pdevinfo->tx_busy = 0;
-
-	wake_up_interruptible(&pdevinfo->wt_wq);
-}
-
-static void mlb_rx_isr(int minor)
+static void mlb_rx_isr(s32 ctype, u32 ahb_ch, struct mlb_dev_info *pdevinfo)
 {
-	struct mlb_dev_info *pdevinfo = &mlb_devinfo[minor];
-	struct mlb_channel_info *pchinfo = &_get_rxchan(minor);
-	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_bufs;
-	s32 wpos, rpos, adt_sts;
-	u32 rx_ring_buf = 0;
-	s32 ctype = pdevinfo->channel_type;
-	u32 ch_addr = pchinfo->address;
+	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_rbuf;
+	s32 head, tail, adt_sts;
+	unsigned long flags;
+	u32 rx_buf_ptr;
 
 #ifdef DEBUG_RX
 	pr_debug("mxc_mlb150: mlb_rx_isr\n");
 #endif
 
-	rpos = rx_rbuf->rpos;
-	wpos = rx_rbuf->wpos;
+	write_lock_irqsave(&rx_rbuf->rb_lock, flags);
 
-#ifdef DEBUG_RX
-	pr_debug("adt_buf_ptr: 0x%08x\n", (u32)adt_buf_ptr);
-#endif
+	head = (rx_rbuf->head + 1) & (TRANS_RING_NODES - 1);
+	tail = ACCESS_ONCE(rx_rbuf->tail);
 
-	/*!
-	 * Copy packet from IRAM buf to ring buf.
-	 * if the wpos++ == rpos, drop this packet
-	 */
-	if (((wpos + 1) % TRANS_RING_NODES) != rpos) {
-		rx_ring_buf = rx_rbuf->phy_addrs[(wpos + 1) % TRANS_RING_NODES];
-#ifdef DEBUG_RX
-		if (len > mlb150_ch_packet_buf_size[ctype])
-			pr_debug("mxc_mlb150: packet overflow, "
-				"packet type: %d\n", ctype);
-#endif
+	if (CIRC_SPACE(head, tail, TRANS_RING_NODES) >= 1) {
+		rx_buf_ptr = rx_rbuf->phy_addrs[head];
 
-		/* update the ring wpos */
-		rx_rbuf->wpos = (wpos + 1) % TRANS_RING_NODES;
+		/* commit the item before incrementing the head */
+		smp_wmb();
 
-		/* wake up the reader */
-		wake_up_interruptible(&pdevinfo->rd_wq);
+		rx_rbuf->head = head;
 
-#ifdef DEBUG_RX
-		pr_debug("recv package, len:%d, rx_rdpos: %d, rx_wtpos: %d\n",
-			 len, rpos, pdevinfo->rx_bufs.wpos);
-#endif
-	} else {
-		rx_ring_buf = pdevinfo->rx_bufs.phy_addrs[TRANS_RING_NODES];
+		write_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
 
-		pr_debug
-		    ("drop package, due to no space, (%d,%d)\n",
-		     rpos, pdevinfo->rx_bufs.wpos);
+		/* wake up the reader */
+		wake_up_interruptible(&pdevinfo->rx_wq);
+	} else {
+		rx_buf_ptr = rx_rbuf->phy_addrs[head];
+		write_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
+		pr_debug("drop RX package, due to no space, (%d,%d)\n",
+				head, tail);
 	}
 
-	adt_sts = mlb150_dev_get_adt_sts(ch_addr);
-	mlb_start_rx(ch_addr, ctype, adt_sts, rx_ring_buf);
+	adt_sts = mlb150_dev_get_adt_sts(ahb_ch);
+	/*  Set ADT for RX */
+	mlb150_dev_pipo_next(ahb_ch, ctype, adt_sts, rx_buf_ptr);
+}
+
+static void mlb_tx_isr(s32 ctype, u32 ahb_ch, struct mlb_dev_info *pdevinfo)
+{
+	struct mlb_ringbuf *tx_rbuf = &pdevinfo->tx_rbuf;
+	s32 head, tail, adt_sts;
+	u32 tx_buf_ptr;
+	unsigned long flags;
+
+	write_lock_irqsave(&tx_rbuf->rb_lock, flags);
+
+	head = ACCESS_ONCE(tx_rbuf->head);
+	tail = (tx_rbuf->tail + 1) & (TRANS_RING_NODES - 1);
+	smp_mb();
+	tx_rbuf->tail = tail;
+
+	/* check the current tx buffer is available or not */
+	if (CIRC_CNT(head, tail, TRANS_RING_NODES) >= 1) {
+		/* read index before reading contents at that index */
+		smp_read_barrier_depends();
+
+		tx_buf_ptr = tx_rbuf->phy_addrs[tail];
+
+		write_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+
+		wake_up_interruptible(&pdevinfo->tx_wq);
+
+		adt_sts = mlb150_dev_get_adt_sts(ahb_ch);
+		/*  Set ADT for TX */
+		mlb150_dev_pipo_next(ahb_ch, ctype, adt_sts, tx_buf_ptr);
+	} else
+		write_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
 }
 
 static irqreturn_t mlb_ahb_isr(int irq, void *dev_id)
 {
-	u32 rx_int_sts, tx_int_sts, acsr0,
-		acsr1, rx_err, tx_err, hcer0, hcer1;
-	struct mlb_dev_info *pdev = NULL;
-	struct mlb_channel_info *ptxchinfo = NULL, *prxchinfo = NULL;
-	int minor;
+	u32 acsr0, hcer0;
+	u32 ch_mask = (1 << SYNC_RX_CL) | (1 << CTRL_RX_CL)
+			| (1 << ASYNC_RX_CL) | (1 << ISOC_RX_CL)
+			| (1 << SYNC_TX_CL) | (1 << CTRL_TX_CL)
+			| (1 << ASYNC_TX_CL) | (1 << ISOC_TX_CL);
 
 	/* Step 5, Read the ACSRn registers to determine which channel or
 	 * channels are causing the interrupt */
-	acsr0 = __raw_readl(mlb_base + MLB150_REG_ACSR0);
-	acsr1 = __raw_readl(mlb_base + MLB150_REG_ACSR1);
+	acsr0 = __raw_readl(mlb_base + REG_ACSR0);
 
-	hcer0 = __raw_readl(mlb_base + MLB150_REG_HCER0);
-	hcer1 = __raw_readl(mlb_base + MLB150_REG_HCER1);
+	hcer0 = __raw_readl(mlb_base + REG_HCER0);
 
 	/* Step 6, If ACTL.SCE = 1, write the result of step 5 back to ACSR0
 	 * and ACSR1 to clear the interrupt */
-	if (MLB150_ACTL_SCE & __raw_readl(mlb_base + MLB150_REG_ACTL)) {
-		__raw_writel(acsr0, mlb_base + MLB150_REG_ACSR0);
-		__raw_writel(acsr1, mlb_base + MLB150_REG_ACSR1);
-	}
+	/* We'll not set ACTL_SCE */
+	/*
+	if (ACTL_SCE & __raw_readl(mlb_base + REG_ACTL))
+		__raw_writel(acsr0, mlb_base + REG_ACSR0);
+	*/
 
-	for (minor = 0; minor < MLB_MINOR_DEVICES; minor++) {
-		pdev = &mlb_devinfo[minor];
-		prxchinfo = &_get_rxchan(minor);
-		ptxchinfo = &_get_txchan(minor);
-
-		rx_int_sts = (prxchinfo->address < 31) ? acsr0 : acsr1;
-		tx_int_sts = (ptxchinfo->address < 31) ? acsr0 : acsr1;
-		rx_err = (prxchinfo->address < 31) ? hcer0 : hcer1;
-		tx_err = (ptxchinfo->address < 31) ? hcer0 : hcer1;
-
-		/* get tx channel interrupt status */
-		if (tx_int_sts & (1 << (ptxchinfo->address % 32))) {
-			if (!(tx_err & (1 << (ptxchinfo->address % 32))))
-				mlb_tx_isr(minor);
-			else {
-				pr_debug("tx channel %d encountered an AHB error!\n",
-					ptxchinfo->address);
-			}
-		}
+	if (ch_mask & hcer0)
+		pr_err("CH encounters an AHB error: 0x%x\n", hcer0);
 
-		/* get rx channel interrupt status */
-		if (rx_int_sts & (1 << (prxchinfo->address % 32))) {
-			if (!(rx_err & (1 << (prxchinfo->address % 32))))
-				mlb_rx_isr(minor);
-			else {
-				pr_debug("rx channel %d encountered an AHB error!\n",
-					prxchinfo->address);
-			}
-		}
-	}
+	if ((1 << SYNC_RX_CL) & acsr0)
+		mlb_rx_isr(MLB_CTYPE_SYNC, SYNC_RX_CL,
+				&mlb_devinfo[MLB_CTYPE_SYNC]);
+
+	if ((1 << CTRL_RX_CL) & acsr0)
+		mlb_rx_isr(MLB_CTYPE_CTRL, CTRL_RX_CL,
+				&mlb_devinfo[MLB_CTYPE_CTRL]);
+
+	if ((1 << ASYNC_RX_CL) & acsr0)
+		mlb_rx_isr(MLB_CTYPE_ASYNC, ASYNC_RX_CL,
+				&mlb_devinfo[MLB_CTYPE_ASYNC]);
+
+	if ((1 << ISOC_RX_CL) & acsr0)
+		mlb_rx_isr(MLB_CTYPE_ISOC, ISOC_RX_CL,
+				&mlb_devinfo[MLB_CTYPE_ISOC]);
+
+	if ((1 << SYNC_TX_CL) & acsr0)
+		mlb_tx_isr(MLB_CTYPE_SYNC, SYNC_TX_CL,
+				&mlb_devinfo[MLB_CTYPE_SYNC]);
+
+	if ((1 << CTRL_TX_CL) & acsr0)
+		mlb_tx_isr(MLB_CTYPE_CTRL, CTRL_TX_CL,
+				&mlb_devinfo[MLB_CTYPE_CTRL]);
+
+	if ((1 << ASYNC_TX_CL) & acsr0)
+		mlb_tx_isr(MLB_CTYPE_ASYNC, ASYNC_TX_CL,
+				&mlb_devinfo[MLB_CTYPE_ASYNC]);
+
+	if ((1 << ISOC_TX_CL) & acsr0)
+		mlb_tx_isr(MLB_CTYPE_ASYNC, ISOC_TX_CL,
+				&mlb_devinfo[MLB_CTYPE_ISOC]);
 
 	return IRQ_HANDLED;
 }
@@ -1587,38 +1746,41 @@ static irqreturn_t mlb_isr(int irq, void *dev_id)
 {
 	u32 rx_int_sts, tx_int_sts, ms0,
 		ms1, tx_cis, rx_cis, ctype;
-	struct mlb_dev_info *pdev;
 	int minor;
 	u32 cdt_val[4] = { 0 };
 
 	/* Step 4, Read the MSn register to determine which channel(s)
 	 * are causing the interrupt */
-	ms0 = __raw_readl(mlb_base + MLB150_REG_MS0);
-	ms1 = __raw_readl(mlb_base + MLB150_REG_MS1);
+	ms0 = __raw_readl(mlb_base + REG_MS0);
+	ms1 = __raw_readl(mlb_base + REG_MS1);
 	pr_debug("mxc_mlb150: mlb interrupt:0x%08x 0x%08x\n",
 			(u32)ms0, (u32)ms1);
 
 	for (minor = 0; minor < MLB_MINOR_DEVICES; minor++) {
-		pdev = &mlb_devinfo[minor];
+		struct mlb_dev_info *pdevinfo = &mlb_devinfo[minor];
+		u32 rx_mlb_ch = pdevinfo->channels[RX_CHANNEL].address;
+		u32 tx_mlb_ch = pdevinfo->channels[TX_CHANNEL].address;
+		u32 rx_mlb_cl = pdevinfo->channels[RX_CHANNEL].cl;
+		u32 tx_mlb_cl = pdevinfo->channels[TX_CHANNEL].cl;
+
 		tx_cis = rx_cis = 0;
 
-		ctype = pdev->channel_type;
-		rx_int_sts = (_get_rxchan(minor).address < 31) ? ms0 : ms1;
-		tx_int_sts = (_get_txchan(minor).address < 31) ? ms0 : ms1;
+		ctype = pdevinfo->channel_type;
+		rx_int_sts = (rx_mlb_ch < 31) ? ms0 : ms1;
+		tx_int_sts = (tx_mlb_ch < 31) ? ms0 : ms1;
 
 		pr_debug("mxc_mlb150: channel interrupt: "
-				"tx: 0x%08x, rx: 0x%08x\n",
-			(u32)tx_int_sts, (u32)rx_int_sts);
+				"tx %d: 0x%08x, rx %d: 0x%08x\n",
+			tx_mlb_ch, (u32)tx_int_sts, rx_mlb_ch, (u32)rx_int_sts);
 
 		/* Get tx channel interrupt status */
-		if (tx_int_sts & (1 << (_get_txchan(minor).address % 32))) {
-			mlb150_dev_cdt_read(_get_txchan(minor).address,
-					cdt_val);
-			pr_debug("mxc_mlb150: cdt_val[3]: 0x%08x, "
+		if (tx_int_sts & (1 << (tx_mlb_ch % 32))) {
+			mlb150_dev_cdt_read(tx_mlb_cl, cdt_val);
+			pr_debug("mxc_mlb150: TX_CH: %d, cdt_val[3]: 0x%08x, "
 					"cdt_val[2]: 0x%08x, "
 					"cdt_val[1]: 0x%08x, "
 					"cdt_val[0]: 0x%08x\n",
-					cdt_val[3], cdt_val[2],
+					tx_mlb_ch, cdt_val[3], cdt_val[2],
 					cdt_val[1], cdt_val[0]);
 			switch (ctype) {
 			case MLB_CTYPE_SYNC:
@@ -1652,14 +1814,18 @@ static irqreturn_t mlb_isr(int irq, void *dev_id)
 			default:
 				break;
 			}
-			mlb150_dev_cdt_write(_get_txchan(minor).address,
-					cdt_val);
+			mlb150_dev_cdt_write(tx_mlb_ch, cdt_val);
 		}
 
 		/* Get rx channel interrupt status */
-		if (rx_int_sts & (1 << (_get_rxchan(minor).address % 32))) {
-			mlb150_dev_cdt_read(_get_rxchan(minor).address,
-					cdt_val);
+		if (rx_int_sts & (1 << (rx_mlb_ch % 32))) {
+			mlb150_dev_cdt_read(rx_mlb_cl, cdt_val);
+			pr_debug("mxc_mlb150: RX_CH: %d, cdt_val[3]: 0x%08x, "
+					"cdt_val[2]: 0x%08x, "
+					"cdt_val[1]: 0x%08x, "
+					"cdt_val[0]: 0x%08x\n",
+					rx_mlb_ch, cdt_val[3], cdt_val[2],
+					cdt_val[1], cdt_val[0]);
 			switch (ctype) {
 			case MLB_CTYPE_SYNC:
 				tx_cis = (cdt_val[2] & ~CDT_SYNC_RSTS_MASK)
@@ -1685,17 +1851,16 @@ static irqreturn_t mlb_isr(int irq, void *dev_id)
 			default:
 				break;
 			}
-			mlb150_dev_cdt_write(_get_rxchan(minor).address,
-					cdt_val);
+			mlb150_dev_cdt_write(rx_mlb_ch, cdt_val);
 		}
 
 		if (!tx_cis && !rx_cis)
 			continue;
 
 		/* fill exception event */
-		spin_lock(&pdev->event_lock);
-		pdev->ex_event |= (rx_cis << 16) | tx_cis;
-		spin_unlock(&pdev->event_lock);
+		spin_lock(&pdevinfo->event_lock);
+		pdevinfo->ex_event |= (rx_cis << 16) | tx_cis;
+		spin_unlock(&pdevinfo->event_lock);
 	}
 
 	return IRQ_HANDLED;
@@ -1705,64 +1870,70 @@ static int mxc_mlb150_open(struct inode *inode, struct file *filp)
 {
 	int minor, ring_buf_size, buf_size, j, ret;
 	void __iomem *buf_addr;
-	ulong phyaddr;
-	struct mxc_mlb_platform_data *plat_data;
+	ulong phy_addr;
 	struct mlb_dev_info *pdevinfo = NULL;
 	struct mlb_channel_info *pchinfo = NULL;
-
-	plat_data = container_of(inode->i_cdev, struct mxc_mlb_platform_data,
-				cdev);
-	filp->private_data = plat_data;
+	struct mlb_data *drvdata;
 
 	minor = MINOR(inode->i_rdev);
 
-	if (unlikely(minor < 0 || minor >= MLB_MINOR_DEVICES))
+	if (minor < 0 || minor >= MLB_MINOR_DEVICES) {
+		pr_err("no device\n");
 		return -ENODEV;
+	}
 
 	/* open for each channel device */
-	if (unlikely(atomic_cmpxchg(&mlb_devinfo[minor].opencnt, 0, 1) != 0))
+	if (atomic_cmpxchg(&mlb_devinfo[minor].opencnt, 0, 1) != 0) {
+		pr_err("busy\n");
 		return -EBUSY;
+	}
+
+	/* initial MLB module */
+	mlb150_dev_init();
 
 	pdevinfo = &mlb_devinfo[minor];
-	pchinfo = &_get_txchan(minor);
+	pchinfo = &pdevinfo->channels[TX_CHANNEL];
 
-	ring_buf_size = mlb150_ch_packet_buf_size[minor];
-	buf_size = ring_buf_size * (TRANS_RING_NODES + 1) + PING_BUF_MAX_SIZE;
-	buf_addr = iram_alloc(buf_size, &phyaddr);
-	memset(buf_addr, 0, buf_size);
-	if (unlikely(buf_addr == NULL)) {
+	ring_buf_size = pdevinfo->buf_size;
+	buf_size = ring_buf_size * (TRANS_RING_NODES * 2);
+	buf_addr = iram_alloc(buf_size, &phy_addr);
+	if (buf_addr == NULL) {
 		ret = -ENOMEM;
-		dev_err(plat_data->dev, "can not alloc rx buffers\n");
+		pr_err("can not alloc rx/tx buffers: %d\n", buf_size);
 		return ret;
 	}
+	pdevinfo->rbuf_base_virt = buf_addr;
+	pdevinfo->rbuf_base_phy = phy_addr;
+	memset(buf_addr, 0, buf_size);
 
-	dev_dbg(plat_data->dev, "ch_type: %d, RX ring buf virt base: 0x%08x "
-			"phy base: 0x%08x\n",
-			pdevinfo->channel_type, (u32)buf_addr, (u32)phyaddr);
-
-	for (j = 0; j < TRANS_RING_NODES + 1;
-		++j, buf_addr += ring_buf_size, phyaddr += ring_buf_size) {
-		pdevinfo->rx_bufs.virt_bufs[j] = buf_addr;
-		pdevinfo->rx_bufs.phy_addrs[j] = phyaddr;
-		pdevinfo->rx_bufs.size = pchinfo->buf_size;
+	for (j = 0; j < (TRANS_RING_NODES + 1);
+		++j, buf_addr += ring_buf_size, phy_addr += ring_buf_size) {
+		pdevinfo->rx_rbuf.virt_bufs[j] = buf_addr;
+		pdevinfo->rx_rbuf.phy_addrs[j] = phy_addr;
+	}
+	pdevinfo->rx_rbuf.unit_size = ring_buf_size;
+	pdevinfo->rx_rbuf.total_size = buf_size;
+
+	buf_addr += ring_buf_size;
+	phy_addr += ring_buf_size;
+	for (j = 0; j < (TRANS_RING_NODES);
+		++j, buf_addr += ring_buf_size, phy_addr += ring_buf_size) {
+		pdevinfo->tx_rbuf.virt_bufs[j] = buf_addr;
+		pdevinfo->tx_rbuf.phy_addrs[j] = phy_addr;
 	}
 
-	/* set the virtual and physical buf head address */
-	pchinfo->ping_buf_head = pchinfo->pong_buf_head = (u32)buf_addr;
-	pchinfo->ping_phy_head = pchinfo->pong_phy_head = phyaddr;
-
-	pchinfo->buf_ptr = (u32)buf_addr;
-	pchinfo->buf_phy_addr = phyaddr;
-
-	dev_dbg(plat_data->dev, "ctype: %d, tx phy_head: 0x%08x, "
-		"buf_head: 0x%08x\n",
-		pchinfo->address,
-		(u32)pchinfo->buf_phy_addr,
-		(u32)pchinfo->buf_ptr);
+	pdevinfo->tx_rbuf.unit_size = ring_buf_size;
+	pdevinfo->tx_rbuf.total_size = buf_size;
 
 	/* reset the buffer read/write ptr */
-	pdevinfo->rx_bufs.rpos = pdevinfo->rx_bufs.wpos = 0;
+	pdevinfo->rx_rbuf.head = pdevinfo->rx_rbuf.tail = 0;
+	pdevinfo->tx_rbuf.head = pdevinfo->tx_rbuf.tail = 0;
 	pdevinfo->ex_event = 0;
+	pdevinfo->tx_ok = 0;
+
+	drvdata = container_of(inode->i_cdev, struct mlb_data, cdev);
+	drvdata->devinfo = pdevinfo;
+	filp->private_data = drvdata;
 
 	return 0;
 }
@@ -1770,26 +1941,24 @@ static int mxc_mlb150_open(struct inode *inode, struct file *filp)
 static int mxc_mlb150_release(struct inode *inode, struct file *filp)
 {
 	int minor;
-	u32 buf_size;
+	struct mlb_data *drvdata = filp->private_data;
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
+	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_rbuf;
 
 	minor = MINOR(inode->i_rdev);
 
 #ifdef DEBUG
 	mlb150_dev_dump_reg();
-	mlb150_dev_dump_ctr_tbl(0, _get_txchan(minor).address + 1);
-	mlb150_dev_dump_hex((const u8 *)mlb_devinfo[minor].rx_bufs.virt_bufs[0],
-			mlb_devinfo[minor].rx_bufs.size);
+	mlb150_dev_dump_ctr_tbl(0, pdevinfo->channels[TX_CHANNEL].cl + 1);
 #endif
 
 	/* clear channel settings and info */
-	mlb_channel_enable(minor, 0);
+	mlb_channel_enable(drvdata, minor, 0);
 
-	buf_size = mlb150_ch_packet_buf_size[minor] *
-			(TRANS_RING_NODES + 1) + PING_BUF_MAX_SIZE;
-	iram_free(mlb_devinfo[minor].rx_bufs.phy_addrs[0], buf_size);
+	iram_free(pdevinfo->rbuf_base_phy, rx_rbuf->total_size);
 
 	/* decrease the open count */
-	atomic_set(&mlb_devinfo[minor].opencnt, 0);
+	atomic_set(&pdevinfo->opencnt, 0);
 
 	return 0;
 }
@@ -1798,10 +1967,11 @@ static long mxc_mlb150_ioctl(struct file *filp,
 			 unsigned int cmd, unsigned long arg)
 {
 	struct inode *inode = filp->f_dentry->d_inode;
+	struct mlb_data *drvdata = filp->private_data;
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
 	void __user *argp = (void __user *)arg;
 	unsigned long flags, event;
 	int minor;
-	struct mxc_mlb_platform_data *plat_data = filp->private_data;
 
 	minor = MINOR(inode->i_rdev);
 
@@ -1814,46 +1984,75 @@ static long mxc_mlb150_ioctl(struct file *filp,
 				pr_err("mxc_mlb150: copy from user failed\n");
 				return -EFAULT;
 			}
-			_get_txchan(minor).address = (caddr >> 16) & 0xFFFF;
-			_get_rxchan(minor).address = caddr & 0xFFFF;
+			pdevinfo->channels[TX_CHANNEL].address =
+							(caddr >> 16) & 0xFFFF;
+			pdevinfo->channels[RX_CHANNEL].address = caddr & 0xFFFF;
 			pr_debug("mxc_mlb150: set ch addr, tx: %d, rx: %d\n",
-					_get_txchan(minor).address,
-					_get_rxchan(minor).address);
+					pdevinfo->channels[TX_CHANNEL].address,
+					pdevinfo->channels[RX_CHANNEL].address);
 			break;
 		}
 
 	case MLB_CHAN_STARTUP:
-		if (unlikely(atomic_read(&mlb_devinfo[minor].on))) {
-			pr_debug("mxc_mlb150: channel areadly startup\n");
+		if (atomic_read(&pdevinfo->on)) {
+			pr_debug("mxc_mlb150: channel alreadly startup\n");
 			break;
 		}
-		pr_debug("mxc_mlb150: start channel\n");
-		mlb_channel_enable(minor, 1);
+		mlb_channel_enable(drvdata, minor, 1);
 		break;
 	case MLB_CHAN_SHUTDOWN:
-		if (unlikely(atomic_read(&mlb_devinfo[minor].on) == 0)) {
+		if (atomic_read(&pdevinfo->on) == 0) {
 			pr_debug("mxc_mlb150: channel areadly shutdown\n");
 			break;
 		}
-		pr_debug("mxc_mlb150: shutdown channel\n");
-		mlb_channel_enable(minor, 0);
+		mlb150_trans_complete_check(pdevinfo);
+		mlb_channel_enable(drvdata, minor, 0);
 		break;
 	case MLB_CHAN_GETEVENT:
 		/* get and clear the ex_event */
-		spin_lock_irqsave(&mlb_devinfo[minor].event_lock, flags);
-		event = mlb_devinfo[minor].ex_event;
-		mlb_devinfo[minor].ex_event = 0;
-		spin_unlock_irqrestore(&mlb_devinfo[minor].event_lock, flags);
+		spin_lock_irqsave(&pdevinfo->event_lock, flags);
+		event = pdevinfo->ex_event;
+		pdevinfo->ex_event = 0;
+		spin_unlock_irqrestore(&pdevinfo->event_lock, flags);
 
-		pr_debug("mxc_mlb150: get event\n");
 		if (event) {
 			if (copy_to_user(argp, &event, sizeof(event))) {
 				pr_err("mxc_mlb150: copy to user failed\n");
 				return -EFAULT;
 			}
-		} else {
-			pr_debug("mxc_mlb150: no exception event now\n");
+		} else
 			return -EAGAIN;
+		break;
+	case MLB_SET_ISOC_BLKSIZE_188:
+		pdevinfo->isoc_blksz = 188;
+		pdevinfo->cdt_buf_dep = pdevinfo->adt_buf_dep =
+					pdevinfo->isoc_blksz * CH_ISOC_BLK_NUM;
+		break;
+	case MLB_SET_ISOC_BLKSIZE_196:
+		pdevinfo->isoc_blksz = 196;
+		pdevinfo->cdt_buf_dep = pdevinfo->adt_buf_dep =
+					pdevinfo->isoc_blksz * CH_ISOC_BLK_NUM;
+		break;
+	case MLB_SET_SYNC_QUAD:
+		{
+			u32 quad;
+
+			if (copy_from_user(&quad, argp, sizeof(quad))) {
+				pr_err("mxc_mlb150: get quad number "
+						"from user failed\n");
+				return -EFAULT;
+			}
+			if (quad <= 0 || quad > 3) {
+				pr_err("mxc_mlb150: Invalid Quadlets!"
+					"Quadlets in Sync mode can "
+					"only be 1, 2, 3\n");
+				return -EINVAL;
+			}
+			pdevinfo->sync_quad = quad;
+			/* Each quadlets is 4 bytes */
+			pdevinfo->cdt_buf_dep = quad * 4 * 4;
+			pdevinfo->adt_buf_dep =
+				pdevinfo->cdt_buf_dep * CH_SYNC_ADT_BUF_MULTI;
 		}
 		break;
 	case MLB_SET_FPS:
@@ -1861,43 +2060,47 @@ static long mxc_mlb150_ioctl(struct file *filp,
 			u32 fps, c0_val;
 
 			/* get fps from user space */
-			if (unlikely(copy_from_user(&fps, argp, sizeof(fps)))) {
+			if (copy_from_user(&fps, argp, sizeof(fps))) {
 				pr_err("mxc_mlb150: copy from user failed\n");
 				return -EFAULT;
 			}
 
-			if (plat_data->fps_sel)
-				plat_data->fps_sel(fps);
-
-			c0_val = __raw_readl(mlb_base + MLB150_REG_MLBC0);
-			c0_val &= ~MLB150_MLBC0_MLBCLK_MASK;
+			c0_val = __raw_readl(mlb_base + REG_MLBC0);
+			c0_val &= ~MLBC0_MLBCLK_MASK;
 
 			/* check fps value */
 			switch (fps) {
 			case 256:
 			case 512:
 			case 1024:
-				mlb_devinfo[minor].fps = fps >> 9;
-				c0_val &= ~MLB150_MLBC0_MLBPEN;
+				pdevinfo->fps = fps >> 9;
+				c0_val &= ~MLBC0_MLBPEN;
 				c0_val |= (fps >> 9)
-					<< MLB150_MLBC0_MLBCLK_SHIFT;
+					<< MLBC0_MLBCLK_SHIFT;
+
+				if (1024 == fps) {
+					/* Invert output clock phase
+					 * in 1024 fps */
+					__raw_writel(0x1,
+						mlb_base + REG_MLBPC2);
+				}
 				break;
 			case 2048:
 			case 3072:
 			case 4096:
-				mlb_devinfo[minor].fps = (fps >> 10) + 1;
+				pdevinfo->fps = (fps >> 10) + 1;
 				c0_val |= ((fps >> 10) + 1)
-					<< MLB150_MLBC0_MLBCLK_SHIFT;
+					<< MLBC0_MLBCLK_SHIFT;
 				break;
 			case 6144:
-				mlb_devinfo[minor].fps = fps >> 10;
+				pdevinfo->fps = fps >> 10;
 				c0_val |= ((fps >> 10) + 1)
-					<< MLB150_MLBC0_MLBCLK_SHIFT;
+					<< MLBC0_MLBCLK_SHIFT;
 				break;
 			case 8192:
-				mlb_devinfo[minor].fps = (fps >> 10) - 1;
+				pdevinfo->fps = (fps >> 10) - 1;
 				c0_val |= ((fps >> 10) - 1)
-						<< MLB150_MLBC0_MLBCLK_SHIFT;
+						<< MLBC0_MLBCLK_SHIFT;
 				break;
 			default:
 				pr_debug("mxc_mlb150: invalid fps argument: %d\n",
@@ -1905,11 +2108,11 @@ static long mxc_mlb150_ioctl(struct file *filp,
 				return -EINVAL;
 			}
 
-			__raw_writel(c0_val, mlb_base + MLB150_REG_MLBC0);
+			__raw_writel(c0_val, mlb_base + REG_MLBC0);
 
 			pr_debug("mxc_mlb150: set fps to %d, MLBC0: 0x%08x\n",
 				fps,
-				(u32)__raw_readl(mlb_base + MLB150_REG_MLBC0));
+				(u32)__raw_readl(mlb_base + REG_MLBC0));
 
 			break;
 		}
@@ -1937,19 +2140,19 @@ static long mxc_mlb150_ioctl(struct file *filp,
 			u8 devaddr;
 
 			/* get MLB device address from user space */
-			if (unlikely(copy_from_user
-				(&devaddr, argp, sizeof(unsigned char)))) {
+			if (copy_from_user
+				(&devaddr, argp, sizeof(unsigned char))) {
 				pr_err("mxc_mlb150: copy from user failed\n");
 				return -EFAULT;
 			}
 
-			c1_val = __raw_readl(mlb_base + MLB150_REG_MLBC1);
-			c1_val &= ~MLB150_MLBC1_NDA_MASK;
-			c1_val |= devaddr << MLB150_MLBC1_NDA_SHIFT;
-			__raw_writel(c1_val, mlb_base + MLB150_REG_MLBC1);
+			c1_val = __raw_readl(mlb_base + REG_MLBC1);
+			c1_val &= ~MLBC1_NDA_MASK;
+			c1_val |= devaddr << MLBC1_NDA_SHIFT;
+			__raw_writel(c1_val, mlb_base + REG_MLBC1);
 			pr_debug("mxc_mlb150: set dev addr, dev addr: %d, "
 				"MLBC1: 0x%08x\n", devaddr,
-				(u32)__raw_readl(mlb_base + MLB150_REG_MLBC1));
+				(u32)__raw_readl(mlb_base + REG_MLBC1));
 
 			break;
 		}
@@ -1970,35 +2173,59 @@ static long mxc_mlb150_ioctl(struct file *filp,
 static ssize_t mxc_mlb150_read(struct file *filp, char __user *buf,
 			    size_t count, loff_t *f_pos)
 {
-	int minor, ret;
-	int size, rdpos;
-	struct mlb_ringbuf *rx_rbuf = NULL;
-	struct mlb_dev_info *pdevinfo = NULL;
-
-#ifdef DEBUG_RX
-	pr_debug("mxc_mlb150: mxc_mlb150_read\n");
-#endif
+	int size;
+	struct mlb_data *drvdata = filp->private_data;
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
+	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_rbuf;
+	int head, tail;
+	unsigned long flags;
 
-	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
+	read_lock_irqsave(&rx_rbuf->rb_lock, flags);
 
-	pdevinfo = &mlb_devinfo[minor];
-
-	rdpos = pdevinfo->rx_bufs.rpos;
-	rx_rbuf = &pdevinfo->rx_bufs;
+	head = ACCESS_ONCE(rx_rbuf->head);
+	tail = rx_rbuf->tail;
 
 	/* check the current rx buffer is available or not */
-	if (rdpos == rx_rbuf->wpos) {
+	if (0 == CIRC_CNT(head, tail, TRANS_RING_NODES)) {
+		read_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
+
 		if (filp->f_flags & O_NONBLOCK)
 			return -EAGAIN;
-		/* if !O_NONBLOCK, we wait for recv packet */
-		ret = wait_event_interruptible(pdevinfo->rd_wq,
-						(rx_rbuf->wpos != rdpos));
-		if (ret < 0)
-			return ret;
+
+		do {
+			DEFINE_WAIT(__wait);
+
+			for (;;) {
+				prepare_to_wait(&pdevinfo->rx_wq,
+						&__wait, TASK_INTERRUPTIBLE);
+
+				read_lock_irqsave(&rx_rbuf->rb_lock, flags);
+				if (CIRC_CNT(rx_rbuf->head, rx_rbuf->tail,
+						TRANS_RING_NODES) > 0) {
+					read_unlock_irqrestore(&rx_rbuf->rb_lock,
+								flags);
+					break;
+				}
+				read_unlock_irqrestore(&rx_rbuf->rb_lock,
+							flags);
+
+				if (!signal_pending(current)) {
+					schedule();
+					continue;
+				}
+				return -ERESTARTSYS;
+			}
+			finish_wait(&pdevinfo->rx_wq, &__wait);
+		} while (0);
+		read_lock_irqsave(&rx_rbuf->rb_lock, flags);
 	}
+	read_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
+
+	/* read index before reading contents at that index */
+	smp_read_barrier_depends();
 
-	size = mlb150_ch_packet_buf_size[minor];
-	if (unlikely(size > count)) {
+	size = pdevinfo->adt_buf_dep;
+	if (size > count) {
 		/* the user buffer is too small */
 		pr_warning
 			("mxc_mlb150: received data size is bigger than "
@@ -2006,14 +2233,18 @@ static ssize_t mxc_mlb150_read(struct file *filp, char __user *buf,
 		return -EINVAL;
 	}
 
-	/* copy rx buffer data to user buffer */
-	if (likely(copy_to_user(buf, rx_rbuf->virt_bufs[rdpos], size))) {
+	/* extract one item from the buffer */
+	if (copy_to_user(buf, rx_rbuf->virt_bufs[tail], size)) {
 		pr_err("mxc_mlb150: copy from user failed\n");
 		return -EFAULT;
 	}
 
-	/* update the read ptr */
-	rx_rbuf->rpos = (rdpos + 1) % TRANS_RING_NODES;
+	/* finish reading descriptor before incrementing tail */
+	smp_mb();
+
+	write_lock_irqsave(&rx_rbuf->rb_lock, flags);
+	rx_rbuf->tail = (tail + 1) & (TRANS_RING_NODES - 1);
+	write_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
 
 	*f_pos = 0;
 
@@ -2029,16 +2260,20 @@ static ssize_t mxc_mlb150_read(struct file *filp, char __user *buf,
 static ssize_t mxc_mlb150_write(struct file *filp, const char __user *buf,
 			     size_t count, loff_t *f_pos)
 {
-	s32 minor = 0, ret = 0;
+	s32 ret = 0;
 	struct mlb_channel_info *pchinfo = NULL;
-	struct mlb_dev_info *pdevinfo = NULL;
-	u32 adt_sts = 0;
+	struct mlb_data *drvdata = filp->private_data;
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
+	struct mlb_ringbuf *tx_rbuf = &pdevinfo->tx_rbuf;
+	int head, tail;
+	unsigned long flags;
 
+	/*
 	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
-	pchinfo = &_get_txchan(minor);
-	pdevinfo = &mlb_devinfo[minor];
+	*/
+	pchinfo = &pdevinfo->channels[TX_CHANNEL];
 
-	if (unlikely(count > pchinfo->buf_size)) {
+	if (count > pdevinfo->buf_size) {
 		/* too many data to write */
 		pr_warning("mxc_mlb150: overflow write data\n");
 		return -EFBIG;
@@ -2046,31 +2281,77 @@ static ssize_t mxc_mlb150_write(struct file *filp, const char __user *buf,
 
 	*f_pos = 0;
 
-	/* check the current tx buffer is used or not */
-	if (1 == pdevinfo->tx_busy) {
-		if (filp->f_flags & O_NONBLOCK)
-			return -EAGAIN;
+	read_lock_irqsave(&tx_rbuf->rb_lock, flags);
 
-		ret = wait_event_interruptible(pdevinfo->wt_wq,
-				0 == pdevinfo->tx_busy);
+	head = tx_rbuf->head;
+	tail = ACCESS_ONCE(tx_rbuf->tail);
 
-		if (ret < 0)
-			goto out;
+	if (0 == CIRC_SPACE(head, tail, TRANS_RING_NODES)) {
+		read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+		if (filp->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+		do {
+			DEFINE_WAIT(__wait);
+
+			for (;;) {
+				prepare_to_wait(&pdevinfo->tx_wq,
+						&__wait, TASK_INTERRUPTIBLE);
+
+				read_lock_irqsave(&tx_rbuf->rb_lock, flags);
+				if (CIRC_SPACE(tx_rbuf->head, tx_rbuf->tail,
+							TRANS_RING_NODES) > 0) {
+					read_unlock_irqrestore(&tx_rbuf->rb_lock,
+							flags);
+					break;
+				}
+				read_unlock_irqrestore(&tx_rbuf->rb_lock,
+								flags);
+
+				if (!signal_pending(current)) {
+					schedule();
+					continue;
+				}
+				return -ERESTARTSYS;
+			}
+			finish_wait(&pdevinfo->tx_wq, &__wait);
+		} while (0);
 	}
 
-	if (copy_from_user((void *)pchinfo->buf_ptr, buf, count)) {
+	if (copy_from_user((void *)tx_rbuf->virt_bufs[head], buf, count)) {
+		read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
 		pr_err("mxc_mlb: copy from user failed\n");
 		ret = -EFAULT;
 		goto out;
 	}
 
-	adt_sts = mlb150_dev_get_adt_sts(pchinfo->address);
-	pdevinfo->tx_busy = 1;
-	mlb_start_tx(pchinfo->address, pdevinfo->channel_type,
-			adt_sts, pchinfo->buf_phy_addr);
+	read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+	write_lock_irqsave(&tx_rbuf->rb_lock, flags);
+	smp_wmb();
+	tx_rbuf->head = (head + 1) & (TRANS_RING_NODES - 1);
+	write_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
 
-	ret = count;
+	read_lock_irqsave(&tx_rbuf->rb_lock, flags);
+	if (0 == CIRC_CNT(head, tail, TRANS_RING_NODES)) {
+		u32 tx_buf_ptr, ahb_ch;
+		s32 adt_sts;
+		u32 ctype = pdevinfo->channel_type;
+
+		/* read index before reading contents at that index */
+		smp_read_barrier_depends();
 
+		tx_buf_ptr = tx_rbuf->phy_addrs[tail];
+		read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+
+		ahb_ch = pdevinfo->channels[TX_CHANNEL].cl;
+		adt_sts = mlb150_dev_get_adt_sts(ahb_ch);
+
+		/*  Set ADT for TX */
+		mlb150_dev_pipo_next(ahb_ch, ctype, adt_sts, tx_buf_ptr);
+	} else {
+		read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+	}
+
+	ret = count;
 out:
 	return ret;
 }
@@ -2080,23 +2361,41 @@ static unsigned int mxc_mlb150_poll(struct file *filp,
 {
 	int minor;
 	unsigned int ret = 0;
-	struct mlb_dev_info *pdevinfo = NULL;
+	struct mlb_data *drvdata = filp->private_data;
+	struct mlb_dev_info *pdevinfo = drvdata->devinfo;
+	struct mlb_ringbuf *tx_rbuf = &pdevinfo->tx_rbuf;
+	struct mlb_ringbuf *rx_rbuf = &pdevinfo->rx_rbuf;
+	int head, tail;
+	unsigned long flags;
+
 
 	minor = MINOR(filp->f_dentry->d_inode->i_rdev);
 
-	pdevinfo = &mlb_devinfo[minor];
+	poll_wait(filp, &pdevinfo->rx_wq, wait);
+	poll_wait(filp, &pdevinfo->tx_wq, wait);
 
-	poll_wait(filp, &pdevinfo->rd_wq, wait);
-	poll_wait(filp, &pdevinfo->wt_wq, wait);
+	read_lock_irqsave(&tx_rbuf->rb_lock, flags);
+
+	head = tx_rbuf->head;
+	tail = tx_rbuf->tail;
 
 	/* check the tx buffer is avaiable or not */
-	if (0 == pdevinfo->tx_busy)
+	if (CIRC_SPACE(head, tail, TRANS_RING_NODES) >= 1)
 		ret |= POLLOUT | POLLWRNORM;
 
+	read_unlock_irqrestore(&tx_rbuf->rb_lock, flags);
+
+	read_lock_irqsave(&rx_rbuf->rb_lock, flags);
+
+	head = rx_rbuf->head;
+	tail = rx_rbuf->tail;
+
 	/* check the rx buffer filled or not */
-	if (pdevinfo->rx_bufs.rpos != pdevinfo->rx_bufs.wpos)
+	if (CIRC_CNT(head, tail, TRANS_RING_NODES) >= 1)
 		ret |= POLLIN | POLLRDNORM;
 
+	read_unlock_irqrestore(&rx_rbuf->rb_lock, flags);
+
 	/* check the exception event */
 	if (pdevinfo->ex_event)
 		ret |= POLLIN | POLLRDNORM;
@@ -2124,175 +2423,180 @@ static const struct file_operations mxc_mlb150_fops = {
 static int __devinit mxc_mlb150_probe(struct platform_device *pdev)
 {
 	int ret, mlb_major, i;
-	struct mxc_mlb_platform_data *plat_data;
+	struct mlb_data *drvdata;
 	struct resource *res;
-	void __iomem *base;
+	struct mxc_mlb_platform_data *plat_data;
+
 
 	plat_data =
 		(struct mxc_mlb_platform_data *)pdev->dev.platform_data;
 	plat_data->dev = &pdev->dev;
+	drvdata = kzalloc(sizeof(struct mlb_data), GFP_KERNEL);
+	if (!drvdata) {
+		dev_err(&pdev->dev, "can't allocate enough memory\n");
+		return -ENOMEM;
+	}
 
 	/**
 	 * Register MLB lld as four character devices
 	 */
-	ret = alloc_chrdev_region(&dev, 0, MLB_MINOR_DEVICES, "mxc_mlb150");
-	mlb_major = MAJOR(dev);
-	dev_dbg(plat_data->dev, "MLB device major: %d\n", mlb_major);
-
-	if (unlikely(ret < 0)) {
-		dev_err(plat_data->dev, "can't get major %d\n", mlb_major);
-		goto err2;
+	ret = alloc_chrdev_region(&drvdata->firstdev, 0,
+			MLB_MINOR_DEVICES, "mxc_mlb150");
+	mlb_major = MAJOR(drvdata->firstdev);
+	dev_dbg(&pdev->dev, "MLB device major: %d\n", mlb_major);
+
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't get major %d\n", mlb_major);
+		goto err_reg;
 	}
 
-	cdev_init(&plat_data->cdev, &mxc_mlb150_fops);
-	plat_data->cdev.owner = THIS_MODULE;
+	cdev_init(&drvdata->cdev, &mxc_mlb150_fops);
+	drvdata->cdev.owner = THIS_MODULE;
 
-	ret = cdev_add(&plat_data->cdev, dev, MLB_MINOR_DEVICES);
-	if (unlikely(ret)) {
-		dev_err(plat_data->dev, "can't add cdev\n");
-		goto err2;
+	ret = cdev_add(&drvdata->cdev, drvdata->firstdev, MLB_MINOR_DEVICES);
+	if (ret) {
+		dev_err(&pdev->dev, "can't add cdev\n");
+		goto err_reg;
 	}
 
 	/* create class and device for udev information */
-	mlb_class = class_create(THIS_MODULE, "mlb150");
-	if (unlikely(IS_ERR(mlb_class))) {
-		dev_err(plat_data->dev, "failed to create mlb150 class\n");
+	drvdata->class = class_create(THIS_MODULE, "mlb150");
+	if (IS_ERR(drvdata->class)) {
+		dev_err(&pdev->dev, "failed to create device class\n");
 		ret = -ENOMEM;
-		goto err2;
+		goto err_reg;
 	}
 
 	for (i = 0; i < MLB_MINOR_DEVICES; i++) {
-		class_dev = device_create(mlb_class, NULL, MKDEV(mlb_major, i),
-					  NULL, mlb_devinfo[i].dev_name);
-		if (unlikely(IS_ERR(class_dev))) {
-			dev_err(plat_data->dev, "failed to create mlb150 %s"
+		struct device *class_dev;
+
+		class_dev = device_create(drvdata->class, NULL,
+				MKDEV(mlb_major, i),
+				NULL, mlb_devinfo[i].dev_name);
+		if (IS_ERR(class_dev)) {
+			dev_err(&pdev->dev, "failed to create mlb150 %s"
 				" class device\n", mlb_devinfo[i].dev_name);
 			ret = -ENOMEM;
-			goto err1;
+			goto err_dev;
 		}
 	}
 
-	/* get irq line */
-	/* AHB0 IRQ */
+	/* get irq */
+	/* ahb0 irq */
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
 	if (unlikely(res == NULL)) {
-		dev_err(plat_data->dev, "No mlb150 ahb0 irq line provided\n");
-		goto err0;
+		dev_err(&pdev->dev, "No ahb0 irq line provided\n");
+		goto err_irq;
 	}
-
-	ahb0_irq = res->start;
-	dev_dbg(plat_data->dev, "ahb0_irq: %d\n", ahb0_irq);
-	if (request_irq(ahb0_irq, mlb_ahb_isr, 0, "mlb_ahb0", NULL)) {
-		dev_err(plat_data->dev, "failed to request irq\n");
-		ret = -EBUSY;
-		goto err0;
+	drvdata->irq_ahb0 = res->start;
+	dev_dbg(&pdev->dev, "ahb0_irq: %d\n", drvdata->irq_ahb0);
+	if (request_irq(drvdata->irq_ahb0, mlb_ahb_isr, 0, "mlb_ahb0", NULL)) {
+		dev_err(&pdev->dev, "can't claim irq %d\n", drvdata->irq_ahb0);
+		goto err_irq;
 	}
 
-	/* AHB1 IRQ */
+	/* ahb1 irq */
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 2);
 	if (unlikely(res == NULL)) {
-		dev_err(plat_data->dev, "No mlb150 ahb0 irq line provided\n");
-		goto err0;
+		dev_err(&pdev->dev, "No ahb1 irq line provided\n");
+		goto err_irq;
 	}
-
-	ahb1_irq = res->start;
-	dev_dbg(plat_data->dev, "ahb1_irq: %d\n", ahb1_irq);
-	if (request_irq(ahb1_irq, mlb_ahb_isr, 0, "mlb_ahb1", NULL)) {
-		dev_err(plat_data->dev, "failed to request irq\n");
-		ret = -EBUSY;
-		goto err0;
+	drvdata->irq_ahb1 = res->start;
+	dev_dbg(&pdev->dev, "ahb1_irq: %d\n", drvdata->irq_ahb1);
+	if (request_irq(drvdata->irq_ahb1, mlb_ahb_isr, 0, "mlb_ahb1", NULL)) {
+		dev_err(&pdev->dev, "can't claim irq %d\n", drvdata->irq_ahb1);
+		goto err_irq;
 	}
 
-	/* MLB IRQ */
+	/* mlb irq */
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		dev_err(plat_data->dev, "No mlb150 irq line provided\n");
-		goto err0;
+	if (unlikely(res == NULL)) {
+		dev_err(&pdev->dev, "No mlb irq line provided\n");
+		goto err_irq;
 	}
-
-	mlb_irq = res->start;
-	dev_dbg(plat_data->dev, "mlb_irq: %d\n", mlb_irq);
-	if (request_irq(mlb_irq, mlb_isr, 0, "mlb", NULL)) {
-		dev_err(plat_data->dev, "failed to request irq\n");
-		ret = -EBUSY;
-		goto err0;
+	drvdata->irq_mlb  = res->start;
+	dev_dbg(&pdev->dev, "mlb_irq: %d\n", drvdata->irq_mlb);
+	if (request_irq(drvdata->irq_mlb, mlb_isr, 0, "mlb", NULL)) {
+		dev_err(&pdev->dev, "can't claim irq %d\n", drvdata->irq_mlb);
+		goto err_irq;
 	}
 
 	/* ioremap from phy mlb to kernel space */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (unlikely(res == NULL)) {
-		dev_err(plat_data->dev, "No mlb150 base address provided\n");
-		goto err0;
+	if (!res) {
+		dev_err(&pdev->dev, "can't get device resources\n");
+		ret = -ENOENT;
+		goto err_unmap;
 	}
-
-	base = ioremap(res->start, res->end - res->start);
-	dev_dbg(plat_data->dev, "mapped mlb150 base address: 0x%08x\n",
-		(u32)base);
-
-	if (unlikely(base == NULL)) {
-		dev_err(plat_data->dev,
-				"failed to do ioremap with mlb150 base\n");
-		goto err0;
+	mlb_base = (u32)ioremap(res->start, res->end - res->start);
+	dev_dbg(&pdev->dev, "mapped base address: 0x%08x\n", (u32)mlb_base);
+	if (!mlb_base) {
+		dev_err(&pdev->dev,
+			"failed to get ioremap base\n");
+		goto err_irq;
 	}
-	mlb_base = (u32)base;
-
-	dev_dbg(plat_data->dev, "mlb reg base: 0x%08x\n", mlb_base);
+	drvdata->membase = mlb_base;
 
+#ifdef CONFIG_REGULATOR
 	if (plat_data->reg_nvcc) {
 		/* power on MLB */
-		reg_nvcc = regulator_get(plat_data->dev, plat_data->reg_nvcc);
-		if (unlikely(!IS_ERR(reg_nvcc))) {
+		drvdata->nvcc = regulator_get(&pdev->dev, plat_data->reg_nvcc);
+		if (!IS_ERR(drvdata->nvcc)) {
 			/* set MAX LDO6 for NVCC to 2.5V */
-			regulator_set_voltage(reg_nvcc, 2500000, 2500000);
-			regulator_enable(reg_nvcc);
+			regulator_set_voltage(drvdata->nvcc, 2500000, 2500000);
+			regulator_enable(drvdata->nvcc);
 		}
 	}
+#endif
 
 	/* enable clock */
-	if (likely(plat_data->mlb_clk)) {
-		mlb_clk = clk_get(plat_data->dev, plat_data->mlb_clk);
-		if (unlikely(IS_ERR(mlb_clk))) {
+	if (plat_data->mlb_clk) {
+		drvdata->clk_mlb3p = clk_get(&pdev->dev, plat_data->mlb_clk);
+		if (IS_ERR(drvdata->clk_mlb3p)) {
 			dev_err(&pdev->dev, "unable to get mlb clock\n");
-			ret = PTR_ERR(mlb_clk);
-			goto err0;
+			ret = PTR_ERR(drvdata->clk_mlb3p);
+			goto err_clk;
 		}
-		clk_enable(mlb_clk);
+		clk_enable(drvdata->clk_mlb3p);
 	}
 
-	if (likely(plat_data->mlb_pll_clk)) {
-		mlb_pll_clk = clk_get(plat_data->dev, plat_data->mlb_pll_clk);
-		if (unlikely(IS_ERR(mlb_pll_clk))) {
+	if (plat_data->mlb_pll_clk) {
+		drvdata->clk_mlb6p = clk_get(&pdev->dev,
+				plat_data->mlb_pll_clk);
+		if (IS_ERR(drvdata->clk_mlb6p)) {
 			dev_err(&pdev->dev, "unable to get mlb pll clock\n");
-			ret = PTR_ERR(mlb_pll_clk);
-			goto err0;
+			ret = PTR_ERR(drvdata->clk_mlb6p);
+			goto err_clk;
 		}
 	}
 
-	/* initial MLB module */
-	mlb150_dev_init();
+	platform_set_drvdata(pdev, drvdata);
 
 	return 0;
 
-err0:
-	if (likely(ahb0_irq)) {
-		free_irq(ahb0_irq, NULL);
-		ahb0_irq = 0;
-	}
-	if (likely(ahb1_irq)) {
-		free_irq(ahb1_irq, NULL);
-		ahb1_irq = 0;
-	}
-	if (likely(mlb_irq)) {
-		free_irq(mlb_irq, NULL);
-		mlb_irq = 0;
-	}
-err1:
+err_clk:
+	if (plat_data->mlb_clk)
+		clk_disable(drvdata->clk_mlb3p);
+	if (plat_data->mlb_pll_clk)
+		clk_disable(drvdata->clk_mlb6p);
+err_irq:
+	if (drvdata->irq_ahb0)
+		free_irq(drvdata->irq_ahb0, NULL);
+	if (drvdata->irq_ahb1)
+		free_irq(drvdata->irq_ahb1, NULL);
+	if (drvdata->irq_mlb)
+		free_irq(drvdata->irq_mlb, NULL);
+err_dev:
 	for (--i; i >= 0; i--)
-		device_destroy(mlb_class, MKDEV(mlb_major, i));
+		device_destroy(drvdata->class, MKDEV(mlb_major, i));
+
+	class_destroy(drvdata->class);
+err_reg:
+	unregister_chrdev_region(drvdata->firstdev, MLB_MINOR_DEVICES);
+err_unmap:
+	iounmap((void __iomem *)drvdata->membase);
 
-	class_destroy(mlb_class);
-err2:
-	unregister_chrdev_region(dev, MLB_MINOR_DEVICES);
+	kfree(drvdata);
 
 	return ret;
 }
@@ -2301,6 +2605,7 @@ static int __devexit mxc_mlb150_remove(struct platform_device *pdev)
 {
 	int i;
 	struct mxc_mlb_platform_data *plat_data;
+	struct mlb_data *drvdata = platform_get_drvdata(pdev);
 
 	plat_data = (struct mxc_mlb_platform_data *)pdev->dev.platform_data;
 
@@ -2308,43 +2613,43 @@ static int __devexit mxc_mlb150_remove(struct platform_device *pdev)
 
 	/* disable mlb clock */
 	if (plat_data->mlb_clk) {
-		clk_disable(mlb_clk);
-		clk_put(mlb_clk);
+		clk_disable(drvdata->clk_mlb3p);
+		clk_put(drvdata->clk_mlb3p);
 	}
-
 	if (plat_data->mlb_pll_clk)
-		clk_put(mlb_pll_clk);
+		clk_put(drvdata->clk_mlb6p);
 
 	/* disable mlb power */
-	if (plat_data->reg_nvcc) {
-		regulator_disable(reg_nvcc);
-		regulator_put(reg_nvcc);
+#ifdef CONFIG_REGULATOR
+	if (drvdata->nvcc) {
+		regulator_disable(drvdata->nvcc);
+		regulator_put(drvdata->nvcc);
 	}
+#endif
 
 	/* inactive GPIO */
 	gpio_mlb_inactive();
 
 	/* iounmap */
-	if (mlb_base) {
-		iounmap((void *)mlb_base);
-		mlb_base = 0;
-	}
+	iounmap((void __iomem *)drvdata->membase);
 
-	if (ahb0_irq)
-		free_irq(ahb0_irq, NULL);
-	if (ahb1_irq)
-		free_irq(ahb1_irq, NULL);
-	if (mlb_irq)
-		free_irq(mlb_irq, NULL);
-	ahb0_irq = ahb1_irq = mlb_irq = 0;
+	if (drvdata->irq_ahb0)
+		free_irq(drvdata->irq_ahb0, NULL);
+	if (drvdata->irq_ahb1)
+		free_irq(drvdata->irq_ahb1, NULL);
+	if (drvdata->irq_mlb)
+		free_irq(drvdata->irq_mlb,  NULL);
 
 	/* destroy mlb device class */
 	for (i = MLB_MINOR_DEVICES - 1; i >= 0; i--)
-		device_destroy(mlb_class, MKDEV(MAJOR(dev), i));
-	class_destroy(mlb_class);
+		device_destroy(drvdata->class,
+				MKDEV(MAJOR(drvdata->firstdev), i));
+	class_destroy(drvdata->class);
 
 	/* Unregister the two MLB devices */
-	unregister_chrdev_region(dev, MLB_MINOR_DEVICES);
+	unregister_chrdev_region(drvdata->firstdev, MLB_MINOR_DEVICES);
+
+	kfree(drvdata);
 
 	return 0;
 }
@@ -2352,11 +2657,23 @@ static int __devexit mxc_mlb150_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int mxc_mlb150_suspend(struct platform_device *pdev, pm_message_t state)
 {
+	struct mlb_data *drvdata = platform_get_drvdata(pdev);
+
+	mlb150_dev_exit();
+
+	clk_disable(drvdata->clk_mlb3p);
+
 	return 0;
 }
 
 static int mxc_mlb150_resume(struct platform_device *pdev)
 {
+	struct mlb_data *drvdata = platform_get_drvdata(pdev);
+
+	mlb150_dev_init();
+
+	clk_enable(drvdata->clk_mlb6p);
+
 	return 0;
 }
 #else
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index d0f8c7e67e7d..f0123ef7d89d 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -315,16 +315,24 @@ EXPORT_SYMBOL_GPL(can_put_echo_skb);
  * is handled in the device driver. The driver must protect
  * access to priv->echo_skb, if necessary.
  */
-void can_get_echo_skb(struct net_device *dev, unsigned int idx)
+unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx)
 {
 	struct can_priv *priv = netdev_priv(dev);
 
 	BUG_ON(idx >= priv->echo_skb_max);
 
 	if (priv->echo_skb[idx]) {
+		struct sk_buff *skb = priv->echo_skb[idx];
+		struct can_frame *cf = (struct can_frame *)skb->data;
+		u8 dlc = cf->can_dlc;
+
 		netif_rx(priv->echo_skb[idx]);
 		priv->echo_skb[idx] = NULL;
+
+		return dlc;
 	}
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(can_get_echo_skb);
 
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 920b8a01f9cb..d3b1342ca727 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -126,7 +126,8 @@
 	(FLEXCAN_ESR_ERR_BUS | FLEXCAN_ESR_ERR_STATE)
 
 /* FLEXCAN interrupt flag register (IFLAG) bits */
-#define FLEXCAN_TX_BUF_ID		8
+#define FLEXCAN_RESERVED_BUF_ID		8
+#define FLEXCAN_TX_BUF_ID		13
 #define FLEXCAN_IFLAG_BUF(x)		BIT(x)
 #define FLEXCAN_IFLAG_RX_FIFO_OVERFLOW	BIT(7)
 #define FLEXCAN_IFLAG_RX_FIFO_WARN	BIT(6)
@@ -284,7 +285,6 @@ static int flexcan_get_berr_counter(const struct net_device *dev,
 static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	const struct flexcan_priv *priv = netdev_priv(dev);
-	struct net_device_stats *stats = &dev->stats;
 	struct flexcan_regs __iomem *regs = priv->base;
 	struct can_frame *cf = (struct can_frame *)skb->data;
 	u32 can_id;
@@ -314,13 +314,15 @@ static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		writel(data, &regs->cantxfg[FLEXCAN_TX_BUF_ID].data[1]);
 	}
 
+	can_put_echo_skb(skb, dev, 0);
+
 	writel(can_id, &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_id);
 	writel(ctrl, &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_ctrl);
 
-	kfree_skb(skb);
-
-	/* tx_packets is incremented in flexcan_irq */
-	stats->tx_bytes += cf->can_dlc;
+	if (priv->version == FLEXCAN_VER_10_0_12) {
+		writel(0x0, &regs->cantxfg[FLEXCAN_RESERVED_BUF_ID].can_ctrl);
+		writel(0x0, &regs->cantxfg[FLEXCAN_RESERVED_BUF_ID].can_ctrl);
+	}
 
 	return NETDEV_TX_OK;
 }
@@ -633,7 +635,7 @@ static irqreturn_t flexcan_irq(int irq, void *dev_id)
 
 	/* transmission complete interrupt */
 	if (reg_iflag1 & (1 << FLEXCAN_TX_BUF_ID)) {
-		/* tx_bytes is incremented in flexcan_start_xmit */
+		stats->tx_bytes += can_get_echo_skb(dev, 0);
 		stats->tx_packets++;
 		writel((1 << FLEXCAN_TX_BUF_ID), &regs->iflag1);
 		netif_wake_queue(dev);
@@ -722,13 +724,14 @@ static int flexcan_chip_start(struct net_device *dev)
 	 * enable warning int
 	 * choose format C
 	 * enable self wakeup
+	 * disable local echo
 	 *
 	 */
 	reg_mcr = readl(&regs->mcr);
 	reg_mcr |= FLEXCAN_MCR_FRZ | FLEXCAN_MCR_FEN | FLEXCAN_MCR_HALT |
 		FLEXCAN_MCR_SUPV | FLEXCAN_MCR_WRN_EN |
 		FLEXCAN_MCR_IDAM_C | FLEXCAN_MCR_WAK_MSK |
-		FLEXCAN_MCR_SLF_WAK;
+		FLEXCAN_MCR_SLF_WAK | FLEXCAN_MCR_SRX_DIS;
 	dev_dbg(dev->dev.parent, "%s: writing mcr=0x%08x", __func__, reg_mcr);
 	writel(reg_mcr, &regs->mcr);
 
@@ -1008,7 +1011,7 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 		goto failed_map;
 	}
 
-	dev = alloc_candev(sizeof(struct flexcan_priv), 0);
+	dev = alloc_candev(sizeof(struct flexcan_priv), 1);
 	if (!dev) {
 		err = -ENOMEM;
 		goto failed_alloc;
@@ -1016,7 +1019,7 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 
 	dev->netdev_ops = &flexcan_netdev_ops;
 	dev->irq = irq;
-	dev->flags |= IFF_ECHO; /* we support local echo in hardware */
+	dev->flags |= IFF_ECHO;
 
 	priv = netdev_priv(dev);
 	priv->can.clock.freq = clk_get_rate(clk);
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 69e4a57ed51b..ebb09eb9a6f2 100755
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -27,6 +27,7 @@
 #include <linux/string.h>
 #include <linux/ptrace.h>
 #include <linux/errno.h>
+#include <linux/gpio.h>
 #include <linux/ioport.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
@@ -1872,6 +1873,17 @@ fec_probe(struct platform_device *pdev)
 	if (pdata)
 		fep->phy_interface = pdata->phy;
 
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	gpio_request(pdata->gpio_irq, "gpio_enet_irq");
+	gpio_direction_input(pdata->gpio_irq);
+
+	irq = gpio_to_irq(pdata->gpio_irq);
+	ret = request_irq(irq, fec_enet_interrupt,
+			IRQF_TRIGGER_RISING,
+			 pdev->name, ndev);
+	if (ret)
+		goto failed_irq;
+#else
 	/* This device has up to three irqs on some platforms */
 	for (i = 0; i < 3; i++) {
 		irq = platform_get_irq(pdev, i);
@@ -1886,6 +1898,7 @@ fec_probe(struct platform_device *pdev)
 			goto failed_irq;
 		}
 	}
+#endif
 
 	fep->clk = clk_get(&pdev->dev, "fec_clk");
 	if (IS_ERR(fep->clk)) {
@@ -1936,11 +1949,15 @@ failed_init:
 	clk_disable(fep->clk);
 	clk_put(fep->clk);
 failed_clk:
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	free_irq(irq, ndev);
+#else
 	for (i = 0; i < 3; i++) {
 		irq = platform_get_irq(pdev, i);
 		if (irq > 0)
 			free_irq(irq, ndev);
 	}
+#endif
 failed_irq:
 	iounmap(fep->hwp);
 failed_ioremap:
diff --git a/include/linux/can/dev.h b/include/linux/can/dev.h
index cc0bb4961669..129dbee2c8d6 100644
--- a/include/linux/can/dev.h
+++ b/include/linux/can/dev.h
@@ -93,7 +93,7 @@ void can_bus_off(struct net_device *dev);
 
 void can_put_echo_skb(struct sk_buff *skb, struct net_device *dev,
 		      unsigned int idx);
-void can_get_echo_skb(struct net_device *dev, unsigned int idx);
+unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx);
 void can_free_echo_skb(struct net_device *dev, unsigned int idx);
 
 struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf);
diff --git a/include/linux/fec.h b/include/linux/fec.h
index 97a56d4add74..a9d659456eba 100644
--- a/include/linux/fec.h
+++ b/include/linux/fec.h
@@ -21,6 +21,9 @@ struct fec_platform_data {
 	int (*power_hibernate) (struct phy_device *);
 	phy_interface_t phy;
 	unsigned char mac[ETH_ALEN];
+#ifdef CONFIG_MX6_ENET_IRQ_TO_GPIO
+	unsigned int gpio_irq;
+#endif
 };
 
 #endif
diff --git a/include/linux/mxc_mlb.h b/include/linux/mxc_mlb.h
index 7ac953c84dd3..3a63647b61b3 100644
--- a/include/linux/mxc_mlb.h
+++ b/include/linux/mxc_mlb.h
@@ -1,7 +1,7 @@
 /*
  * mxc_mlb.h
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -21,6 +21,7 @@
 #define MLB_SET_FPS		_IOW('S', 0x10, unsigned int)
 #define MLB_GET_VER		_IOR('S', 0x11, unsigned long)
 #define MLB_SET_DEVADDR		_IOR('S', 0x12, unsigned char)
+
 /*!
  * set channel address for each logical channel
  * the MSB 16bits is for tx channel, the left LSB is for rx channel
@@ -30,6 +31,10 @@
 #define MLB_CHAN_SHUTDOWN	_IO('S', 0x15)
 #define MLB_CHAN_GETEVENT	_IOR('S', 0x16, unsigned long)
 
+#define MLB_SET_ISOC_BLKSIZE_188 _IO('S', 0x17)
+#define MLB_SET_ISOC_BLKSIZE_196 _IO('S', 0x18)
+#define MLB_SET_SYNC_QUAD	_IOW('S', 0x19, unsigned int)
+
 /*!
  * MLB event define
  */