--- a/arch/arm/plat-mxc/pwm.c
+++ b/arch/arm/plat-mxc/pwm.c
@@ -25,6 +25,11 @@
 #define MX1_PWMS    0x04   /* PWM Sample Register */
 #define MX1_PWMP    0x08   /* PWM Period Register */
 
+#define MX1_PWMC_EN (1 << 4)
+#define MX1_PWMC_PRESCALER_MASK (0x7f << 8)
+#define MX1_PWMC_PRESCALER(x) ((x & 0x7f) << 8)
+#define MX1_PWMC_CLKSEL_MASK 0x3
+#define MX1_PWMC_CLKSEL(x) ((x & 0x3))
 
 /* i.MX27, i.MX31, i.MX35 share the same PWM function block: */
 
@@ -54,26 +59,33 @@ struct pwm_device {
 
 int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 {
+	unsigned long long c;
+	unsigned long period_cycles, duty_cycles, prescale;
+
 	if (pwm == NULL || period_ns == 0 || duty_ns > period_ns)
 		return -EINVAL;
 
+	c = clk_get_rate(pwm->clk);
+
+	c = c * period_ns;
+
+	if (cpu_is_mx1() || cpu_is_mx2())
+		c >>= 1;
+
+	do_div(c, 1000000000);
+	period_cycles = c;
+
+	prescale = period_cycles / 0x10000 + 1;
+
+	period_cycles /= prescale;
+	c = (unsigned long long)period_cycles * duty_ns;
+	do_div(c, period_ns);
+	duty_cycles = c;
+
+
 	if (cpu_is_mx27() || cpu_is_mx3() || cpu_is_mx25()) {
-		unsigned long long c;
-		unsigned long period_cycles, duty_cycles, prescale;
 		u32 cr;
 
-		c = clk_get_rate(pwm->clk);
-		c = c * period_ns;
-		do_div(c, 1000000000);
-		period_cycles = c;
-
-		prescale = period_cycles / 0x10000 + 1;
-
-		period_cycles /= prescale;
-		c = (unsigned long long)period_cycles * duty_ns;
-		do_div(c, period_ns);
-		duty_cycles = c;
-
 		writel(duty_cycles, pwm->mmio_base + MX3_PWMSAR);
 		writel(period_cycles, pwm->mmio_base + MX3_PWMPR);
 
@@ -86,25 +98,28 @@ int pwm_config(struct pwm_device *pwm, i
 
 		writel(cr, pwm->mmio_base + MX3_PWMCR);
 	} else if (cpu_is_mx1() || cpu_is_mx21()) {
-		/* The PWM subsystem allows for exact frequencies. However,
-		 * I cannot connect a scope on my device to the PWM line and
-		 * thus cannot provide the program the PWM controller
-		 * exactly. Instead, I'm relying on the fact that the
-		 * Bootloader (u-boot or WinCE+haret) has programmed the PWM
-		 * function group already. So I'll just modify the PWM sample
-		 * register to follow the ratio of duty_ns vs. period_ns
-		 * accordingly.
-		 *
-		 * This is good enough for programming the brightness of
-		 * the LCD backlight.
-		 *
-		 * The real implementation would divide PERCLK[0] first by
-		 * both the prescaler (/1 .. /128) and then by CLKSEL
-		 * (/2 .. /16).
-		 */
-		u32 max = readl(pwm->mmio_base + MX1_PWMP);
-		u32 p = max * duty_ns / period_ns;
-		writel(max - p, pwm->mmio_base + MX1_PWMS);
+		unsigned long clksel = 0;
+		u32 ctrl;
+
+		while (prescale >= 0x80 && clksel < 4) {
+			prescale >>= 1;
+			++clksel;
+		}
+
+		if (clksel > 3)
+			return -EINVAL;
+
+		ctrl = readl(pwm->mmio_base + MX1_PWMC);
+		writel(ctrl & ~MX1_PWMC_EN, pwm->mmio_base + MX1_PWMC);
+
+		writel(duty_cycles, pwm->mmio_base + MX1_PWMS);
+		writel(period_cycles, pwm->mmio_base + MX1_PWMP);
+
+		ctrl &= ~(MX1_PWMC_PRESCALER_MASK | MX1_PWMC_CLKSEL_MASK);
+		ctrl |= MX1_PWMC_PRESCALER(prescale);
+		ctrl |= MX1_PWMC_CLKSEL(clksel);
+		writel(ctrl, pwm->mmio_base + MX1_PWMC);
+
 	} else {
 		BUG();
 	}
@@ -116,6 +131,11 @@ EXPORT_SYMBOL(pwm_config);
 int pwm_enable(struct pwm_device *pwm)
 {
 	int rc = 0;
+	if (cpu_is_mx1() || cpu_is_mx2()) {
+		u32 ctrl;
+		ctrl = readl(pwm->mmio_base + MX1_PWMC);
+		writel(ctrl | MX1_PWMC_EN, pwm->mmio_base + MX1_PWMC);
+	}
 
 	if (!pwm->clk_enabled) {
 		rc = clk_enable(pwm->clk);
@@ -128,7 +148,13 @@ EXPORT_SYMBOL(pwm_enable);
 
 void pwm_disable(struct pwm_device *pwm)
 {
-	writel(0, pwm->mmio_base + MX3_PWMCR);
+	if (cpu_is_mx27() || cpu_is_mx3() || cpu_is_mx25()) {
+		writel(0, pwm->mmio_base + MX3_PWMCR);
+	} else if (cpu_is_mx1() || cpu_is_mx2()) {
+		u32 ctrl;
+		ctrl = readl(pwm->mmio_base + MX1_PWMC);
+		writel(ctrl & ~MX1_PWMC_EN, pwm->mmio_base + MX1_PWMC);
+	}
 
 	if (pwm->clk_enabled) {
 		clk_disable(pwm->clk);