Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cooloney/blackfin-2.6

* 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cooloney/blackfin-2.6: (21 commits)
  Blackfin Serial Driver: abstract away DLAB differences into header
  Blackfin Serial Driver: macro away the IER differences between processors
  [Blackfin] arch: remove useless IRQ_SW_INT defines
  [Blackfin] arch: protect linux/usb/musb.h include until the driver gets mainlined
  [Blackfin] arch: protect linux/usb/isp1362.h include until the driver gets mainlined
  [Blackfin] arch: add EBIU supporting for BF54x EZKIT SMSC LAN911x/LAN921x families embedded ethernet driver
  [Blackfin] arch: Set spi flash partition on bf527 as like bf548.
  [Blackfin] arch: fix bug - Remove module will not free L1 memory used
  [Blackfin] arch: fix wrong header name in comment
  [Blackfin] arch: Fix BUG - spi flash on bf527 ezkit would fail at mount
  [Blackfin] arch: add twi_lcd and twi_keypad i2c board info to bf527-ezkit
  [Blackfin] arch: Add physmap partition for BF527-EZkit
  [Blackfin] arch: fix gdb testing regression
  [Blackfin] arch: disable single stepping when delivering a signal
  [Blackfin] arch: Delete unused (copied from m68k) entries in asm-offsets.c.
  [Blackfin] arch: In the double fault handler, set up the PT_RETI slot
  [Blackfin] arch: Support for CPU_FREQ and NOHZ
  [Blackfin] arch: Functional power management support: Add CPU and platform voltage scaling support
  [Blackfin] arch: fix bug -  breaking the atomic sections code.
  [Blackfin] arch: Equalize include files: Add VR_CTL masks
  ...

23 files changed, 682 insertions, 107 deletions

diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index 795d0ac67c2..fd5708523f2 100644
--- a/arch/blackfin/Kconfig
+++ b/arch/blackfin/Kconfig
@@ -832,6 +832,7 @@ config BANK_0
 config BANK_1
 	hex "Bank 1"
 	default 0x7BB0
+	default 0x5558 if BF54x
 
 config BANK_2
 	hex "Bank 2"
@@ -963,21 +964,22 @@ endchoice
 
 endmenu
 
-if (BF537 || BF533 || BF54x)
-
 menu "CPU Frequency scaling"
 
 source "drivers/cpufreq/Kconfig"
 
-config CPU_FREQ
-	bool
+config CPU_VOLTAGE
+	bool "CPU Voltage scaling"
+	depends on EXPERIMENTAL	
+	depends on CPU_FREQ
 	default n
 	help
-	  If you want to enable this option, you should select the
-	  DPMC driver from Character Devices.
-endmenu
+	  Say Y here if you want CPU voltage scaling according to the CPU frequency.
+	  This option violates the PLL BYPASS recommendation in the Blackfin Processor
+	  manuals. There is a theoretical risk that during VDDINT transitions 
+	  the PLL may unlock.
 
-endif
+endmenu
 
 source "net/Kconfig"
 
diff --git a/arch/blackfin/kernel/asm-offsets.c b/arch/blackfin/kernel/asm-offsets.c
index 721f15f3ceb..881afe9082c 100644
--- a/arch/blackfin/kernel/asm-offsets.c
+++ b/arch/blackfin/kernel/asm-offsets.c
@@ -56,9 +56,6 @@ int main(void)
 	/* offsets into the thread struct */
 	DEFINE(THREAD_KSP, offsetof(struct thread_struct, ksp));
 	DEFINE(THREAD_USP, offsetof(struct thread_struct, usp));
-	DEFINE(THREAD_SR, offsetof(struct thread_struct, seqstat));
-	DEFINE(PT_SR, offsetof(struct thread_struct, seqstat));
-	DEFINE(THREAD_ESP0, offsetof(struct thread_struct, esp0));
 	DEFINE(THREAD_PC, offsetof(struct thread_struct, pc));
 	DEFINE(KERNEL_STACK_SIZE, THREAD_SIZE);
 
diff --git a/arch/blackfin/kernel/fixed_code.S b/arch/blackfin/kernel/fixed_code.S
index 5ed47228a39..4b03ba02548 100644
--- a/arch/blackfin/kernel/fixed_code.S
+++ b/arch/blackfin/kernel/fixed_code.S
@@ -1,6 +1,6 @@
 /*
  * This file contains sequences of code that will be copied to a
- * fixed location, defined in <asm/atomic_seq.h>.  The interrupt
+ * fixed location, defined in <asm/fixed_code.h>.  The interrupt
  * handlers ensure that these sequences appear to be atomic when
  * executed from userspace.
  * These are aligned to 16 bytes, so that we have some space to replace
diff --git a/arch/blackfin/kernel/module.c b/arch/blackfin/kernel/module.c
index 8b9fe29d03f..14a42848f37 100644
--- a/arch/blackfin/kernel/module.c
+++ b/arch/blackfin/kernel/module.c
@@ -160,6 +160,13 @@ int
 module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 			  char *secstrings, struct module *mod)
 {
+	/*
+	 * XXX: sechdrs are vmalloced in kernel/module.c
+	 * and would be vfreed just after module is loaded,
+	 * so we hack to keep the only information we needed
+	 * in mod->arch to correctly free L1 I/D sram later.
+	 * NOTE: this breaks the semantic of mod->arch structure.
+	 */
 	Elf_Shdr *s, *sechdrs_end = sechdrs + hdr->e_shnum;
 	void *dest = NULL;
 
@@ -167,8 +174,8 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 		if ((strcmp(".l1.text", secstrings + s->sh_name) == 0) ||
 		    ((strcmp(".text", secstrings + s->sh_name) == 0) &&
 		     (hdr->e_flags & FLG_CODE_IN_L1) && (s->sh_size > 0))) {
-			mod->arch.text_l1 = s;
 			dest = l1_inst_sram_alloc(s->sh_size);
+			mod->arch.text_l1 = dest;
 			if (dest == NULL) {
 				printk(KERN_ERR
 				       "module %s: L1 instruction memory allocation failed\n",
@@ -182,8 +189,8 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 		if ((strcmp(".l1.data", secstrings + s->sh_name) == 0) ||
 		    ((strcmp(".data", secstrings + s->sh_name) == 0) &&
 		     (hdr->e_flags & FLG_DATA_IN_L1) && (s->sh_size > 0))) {
-			mod->arch.data_a_l1 = s;
 			dest = l1_data_sram_alloc(s->sh_size);
+			mod->arch.data_a_l1 = dest;
 			if (dest == NULL) {
 				printk(KERN_ERR
 					"module %s: L1 data memory allocation failed\n",
@@ -197,8 +204,8 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 		if (strcmp(".l1.bss", secstrings + s->sh_name) == 0 ||
 		    ((strcmp(".bss", secstrings + s->sh_name) == 0) &&
 		     (hdr->e_flags & FLG_DATA_IN_L1) && (s->sh_size > 0))) {
-			mod->arch.bss_a_l1 = s;
 			dest = l1_data_sram_alloc(s->sh_size);
+			mod->arch.bss_a_l1 = dest;
 			if (dest == NULL) {
 				printk(KERN_ERR
 					"module %s: L1 data memory allocation failed\n",
@@ -210,8 +217,8 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 			s->sh_addr = (unsigned long)dest;
 		}
 		if (strcmp(".l1.data.B", secstrings + s->sh_name) == 0) {
-			mod->arch.data_b_l1 = s;
 			dest = l1_data_B_sram_alloc(s->sh_size);
+			mod->arch.data_b_l1 = dest;
 			if (dest == NULL) {
 				printk(KERN_ERR
 					"module %s: L1 data memory allocation failed\n",
@@ -223,8 +230,8 @@ module_frob_arch_sections(Elf_Ehdr * hdr, Elf_Shdr * sechdrs,
 			s->sh_addr = (unsigned long)dest;
 		}
 		if (strcmp(".l1.bss.B", secstrings + s->sh_name) == 0) {
-			mod->arch.bss_b_l1 = s;
 			dest = l1_data_B_sram_alloc(s->sh_size);
+			mod->arch.bss_b_l1 = dest;
 			if (dest == NULL) {
 				printk(KERN_ERR
 					"module %s: L1 data memory allocation failed\n",
@@ -416,14 +423,14 @@ module_finalize(const Elf_Ehdr * hdr,
 
 void module_arch_cleanup(struct module *mod)
 {
-	if ((mod->arch.text_l1) && (mod->arch.text_l1->sh_addr))
-		l1_inst_sram_free((void *)mod->arch.text_l1->sh_addr);
-	if ((mod->arch.data_a_l1) && (mod->arch.data_a_l1->sh_addr))
-		l1_data_sram_free((void *)mod->arch.data_a_l1->sh_addr);
-	if ((mod->arch.bss_a_l1) && (mod->arch.bss_a_l1->sh_addr))
-		l1_data_sram_free((void *)mod->arch.bss_a_l1->sh_addr);
-	if ((mod->arch.data_b_l1) && (mod->arch.data_b_l1->sh_addr))
-		l1_data_B_sram_free((void *)mod->arch.data_b_l1->sh_addr);
-	if ((mod->arch.bss_b_l1) && (mod->arch.bss_b_l1->sh_addr))
-		l1_data_B_sram_free((void *)mod->arch.bss_b_l1->sh_addr);
+	if (mod->arch.text_l1)
+		l1_inst_sram_free((void *)mod->arch.text_l1);
+	if (mod->arch.data_a_l1)
+		l1_data_sram_free((void *)mod->arch.data_a_l1);
+	if (mod->arch.bss_a_l1)
+		l1_data_sram_free((void *)mod->arch.bss_a_l1);
+	if (mod->arch.data_b_l1)
+		l1_data_B_sram_free((void *)mod->arch.data_b_l1);
+	if (mod->arch.bss_b_l1)
+		l1_data_B_sram_free((void *)mod->arch.bss_b_l1);
 }
diff --git a/arch/blackfin/kernel/process.c b/arch/blackfin/kernel/process.c
index be9fdd00d7c..53c2cd25544 100644
--- a/arch/blackfin/kernel/process.c
+++ b/arch/blackfin/kernel/process.c
@@ -245,7 +245,7 @@ unsigned long get_wchan(struct task_struct *p)
 
 void finish_atomic_sections (struct pt_regs *regs)
 {
-	int __user *up0 = (int __user *)&regs->p0;
+	int __user *up0 = (int __user *)regs->p0;
 
 	if (regs->pc < ATOMIC_SEQS_START || regs->pc >= ATOMIC_SEQS_END)
 		return;
diff --git a/arch/blackfin/kernel/ptrace.c b/arch/blackfin/kernel/ptrace.c
index b4f062c172c..f51ab088098 100644
--- a/arch/blackfin/kernel/ptrace.c
+++ b/arch/blackfin/kernel/ptrace.c
@@ -185,8 +185,8 @@ void ptrace_disable(struct task_struct *child)
 {
 	unsigned long tmp;
 	/* make sure the single step bit is not set. */
-	tmp = get_reg(child, PT_SR) & ~(TRACE_BITS << 16);
-	put_reg(child, PT_SR, tmp);
+	tmp = get_reg(child, PT_SYSCFG) & ~TRACE_BITS;
+	put_reg(child, PT_SYSCFG, tmp);
 }
 
 long arch_ptrace(struct task_struct *child, long request, long addr, long data)
diff --git a/arch/blackfin/kernel/signal.c b/arch/blackfin/kernel/signal.c
index cb9d883d493..dbc3bbf846b 100644
--- a/arch/blackfin/kernel/signal.c
+++ b/arch/blackfin/kernel/signal.c
@@ -42,6 +42,9 @@
 
 #define _BLOCKABLE (~(sigmask(SIGKILL) | sigmask(SIGSTOP)))
 
+/* Location of the trace bit in SYSCFG. */
+#define TRACE_BITS 0x0001
+
 struct fdpic_func_descriptor {
 	unsigned long	text;
 	unsigned long	GOT;
@@ -225,6 +228,16 @@ setup_rt_frame(int sig, struct k_sigaction *ka, siginfo_t * info,
 	regs->r1 = (unsigned long)(&frame->info);
 	regs->r2 = (unsigned long)(&frame->uc);
 
+	/*
+	 * Clear the trace flag when entering the signal handler, but
+	 * notify any tracer that was single-stepping it. The tracer
+	 * may want to single-step inside the handler too.
+	 */
+	if (regs->syscfg & TRACE_BITS) {
+		regs->syscfg &= ~TRACE_BITS;
+		ptrace_notify(SIGTRAP);
+	}
+
 	return 0;
 
  give_sigsegv:
diff --git a/arch/blackfin/kernel/time-ts.c b/arch/blackfin/kernel/time-ts.c
index 4482c47c09e..e887efc86c2 100644
--- a/arch/blackfin/kernel/time-ts.c
+++ b/arch/blackfin/kernel/time-ts.c
@@ -60,7 +60,7 @@ static inline unsigned long long cycles_2_ns(cycle_t cyc)
 
 static cycle_t read_cycles(void)
 {
-	return get_cycles();
+	return __bfin_cycles_off + (get_cycles() << __bfin_cycles_mod);
 }
 
 unsigned long long sched_clock(void)
@@ -117,7 +117,7 @@ static void bfin_timer_set_mode(enum clock_event_mode mode,
 		break;
 	}
 	case CLOCK_EVT_MODE_ONESHOT:
-		bfin_write_TSCALE(0);
+		bfin_write_TSCALE(TIME_SCALE - 1);
 		bfin_write_TCOUNT(0);
 		bfin_write_TCNTL(TMPWR | TMREN);
 		CSYNC();
@@ -183,10 +183,14 @@ irqreturn_t timer_interrupt(int irq, void *dev_id)
 
 static int __init bfin_clockevent_init(void)
 {
+	unsigned long timer_clk;
+
+	timer_clk = get_cclk() / TIME_SCALE;
+
 	setup_irq(IRQ_CORETMR, &bfin_timer_irq);
 	bfin_timer_init();
 
-	clockevent_bfin.mult = div_sc(get_cclk(), NSEC_PER_SEC, clockevent_bfin.shift);
+	clockevent_bfin.mult = div_sc(timer_clk, NSEC_PER_SEC, clockevent_bfin.shift);
 	clockevent_bfin.max_delta_ns = clockevent_delta2ns(-1, &clockevent_bfin);
 	clockevent_bfin.min_delta_ns = clockevent_delta2ns(100, &clockevent_bfin);
 	clockevents_register_device(&clockevent_bfin);
diff --git a/arch/blackfin/mach-bf527/boards/ezkit.c b/arch/blackfin/mach-bf527/boards/ezkit.c
index 583d53811f0..8aa49f80422 100644
--- a/arch/blackfin/mach-bf527/boards/ezkit.c
+++ b/arch/blackfin/mach-bf527/boards/ezkit.c
@@ -32,12 +32,14 @@
 #include <linux/platform_device.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
+#include <linux/mtd/physmap.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 #include <linux/usb/isp1362.h>
 #endif
 #include <linux/ata_platform.h>
+#include <linux/i2c.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/usb/sl811.h>
@@ -50,6 +52,7 @@
 #include <asm/reboot.h>
 #include <asm/nand.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 #include <linux/spi/ad7877.h>
 
 /*
@@ -171,6 +174,46 @@ static struct platform_device bf52x_t350mcqb_device = {
 };
 #endif
 
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+static struct mtd_partition ezkit_partitions[] = {
+	{
+		.name       = "Bootloader",
+		.size       = 0x40000,
+		.offset     = 0,
+	}, {
+		.name       = "Kernel",
+		.size       = 0x1C0000,
+		.offset     = MTDPART_OFS_APPEND,
+	}, {
+		.name       = "RootFS",
+		.size       = MTDPART_SIZ_FULL,
+		.offset     = MTDPART_OFS_APPEND,
+	}
+};
+
+static struct physmap_flash_data ezkit_flash_data = {
+	.width      = 2,
+	.parts      = ezkit_partitions,
+	.nr_parts   = ARRAY_SIZE(ezkit_partitions),
+};
+
+static struct resource ezkit_flash_resource = {
+	.start = 0x20000000,
+	.end   = 0x203fffff,
+	.flags = IORESOURCE_MEM,
+};
+
+static struct platform_device ezkit_flash_device = {
+	.name          = "physmap-flash",
+	.id            = 0,
+	.dev = {
+		.platform_data = &ezkit_flash_data,
+	},
+	.num_resources = 1,
+	.resource      = &ezkit_flash_resource,
+};
+#endif
+
 #if defined(CONFIG_MTD_NAND_BF5XX) || defined(CONFIG_MTD_NAND_BF5XX_MODULE)
 static struct mtd_partition partition_info[] = {
 	{
@@ -420,11 +463,7 @@ static struct mtd_partition bfin_spi_flash_partitions[] = {
 		.offset = 0,
 		.mask_flags = MTD_CAP_ROM
 	}, {
-		.name = "kernel",
-		.size = 0xe0000,
-		.offset = MTDPART_OFS_APPEND,
-	}, {
-		.name = "file system",
+		.name = "linux kernel",
 		.size = MTDPART_SIZ_FULL,
 		.offset = MTDPART_OFS_APPEND,
 	}
@@ -434,7 +473,7 @@ static struct flash_platform_data bfin_spi_flash_data = {
 	.name = "m25p80",
 	.parts = bfin_spi_flash_partitions,
 	.nr_parts = ARRAY_SIZE(bfin_spi_flash_partitions),
-	.type = "m25p64",
+	.type = "m25p16",
 };
 
 /* SPI flash chip (m25p64) */
@@ -755,6 +794,24 @@ static struct platform_device i2c_bfin_twi_device = {
 };
 #endif
 
+#ifdef CONFIG_I2C_BOARDINFO
+static struct i2c_board_info __initdata bfin_i2c_board_info[] = {
+#if defined(CONFIG_TWI_LCD) || defined(CONFIG_TWI_LCD_MODULE)
+	{
+		I2C_BOARD_INFO("pcf8574_lcd", 0x22),
+		.type = "pcf8574_lcd",
+	},
+#endif
+#if defined(CONFIG_TWI_KEYPAD) || defined(CONFIG_TWI_KEYPAD_MODULE)
+	{
+		I2C_BOARD_INFO("pcf8574_keypad", 0x27),
+		.type = "pcf8574_keypad",
+		.irq = IRQ_PF8,
+	},
+#endif
+};
+#endif
+
 #if defined(CONFIG_SERIAL_BFIN_SPORT) || defined(CONFIG_SERIAL_BFIN_SPORT_MODULE)
 static struct platform_device bfin_sport0_uart_device = {
 	.name = "bfin-sport-uart",
@@ -839,7 +896,32 @@ static struct platform_device bfin_gpios_device = {
 	.resource = &bfin_gpios_resources,
 };
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_100, 400000000),
+	VRPAIR(VLEV_105, 426000000),
+	VRPAIR(VLEV_110, 500000000),
+	VRPAIR(VLEV_115, 533000000),
+	VRPAIR(VLEV_120, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *stamp_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_MTD_NAND_BF5XX) || defined(CONFIG_MTD_NAND_BF5XX_MODULE)
 	&bf5xx_nand_device,
 #endif
@@ -921,12 +1003,22 @@ static struct platform_device *stamp_devices[] __initdata = {
 	&bfin_device_gpiokeys,
 #endif
 
+#if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
+	&ezkit_flash_device,
+#endif
+
 	&bfin_gpios_device,
 };
 
 static int __init stamp_init(void)
 {
 	printk(KERN_INFO "%s(): registering device resources\n", __func__);
+
+#ifdef CONFIG_I2C_BOARDINFO
+	i2c_register_board_info(0, bfin_i2c_board_info,
+				ARRAY_SIZE(bfin_i2c_board_info));
+#endif
+
 	platform_add_devices(stamp_devices, ARRAY_SIZE(stamp_devices));
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
 	spi_register_board_info(bfin_spi_board_info,
diff --git a/arch/blackfin/mach-bf533/boards/cm_bf533.c b/arch/blackfin/mach-bf533/boards/cm_bf533.c
index a03149c7268..ed2b0b8f5dc 100644
--- a/arch/blackfin/mach-bf533/boards/cm_bf533.c
+++ b/arch/blackfin/mach-bf533/boards/cm_bf533.c
@@ -33,12 +33,15 @@
 #include <linux/mtd/partitions.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
+#if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 #include <linux/usb/isp1362.h>
+#endif
 #include <linux/ata_platform.h>
 #include <linux/irq.h>
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -341,7 +344,37 @@ static struct platform_device bfin_pata_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 376000000),
+	VRPAIR(VLEV_095, 426000000),
+	VRPAIR(VLEV_100, 426000000),
+	VRPAIR(VLEV_105, 476000000),
+	VRPAIR(VLEV_110, 476000000),
+	VRPAIR(VLEV_115, 476000000),
+	VRPAIR(VLEV_120, 600000000),
+	VRPAIR(VLEV_125, 600000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *cm_bf533_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_SERIAL_BFIN) || defined(CONFIG_SERIAL_BFIN_MODULE)
 	&bfin_uart_device,
 #endif
diff --git a/arch/blackfin/mach-bf533/boards/ezkit.c b/arch/blackfin/mach-bf533/boards/ezkit.c
index 08a7943949a..9d28415163e 100644
--- a/arch/blackfin/mach-bf533/boards/ezkit.c
+++ b/arch/blackfin/mach-bf533/boards/ezkit.c
@@ -42,6 +42,7 @@
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -350,7 +351,37 @@ static struct platform_device i2c_gpio_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 376000000),
+	VRPAIR(VLEV_095, 426000000),
+	VRPAIR(VLEV_100, 426000000),
+	VRPAIR(VLEV_105, 476000000),
+	VRPAIR(VLEV_110, 476000000),
+	VRPAIR(VLEV_115, 476000000),
+	VRPAIR(VLEV_120, 600000000),
+	VRPAIR(VLEV_125, 600000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *ezkit_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
diff --git a/arch/blackfin/mach-bf533/boards/stamp.c b/arch/blackfin/mach-bf533/boards/stamp.c
index 024f418ae54..7fd35fb32fd 100644
--- a/arch/blackfin/mach-bf533/boards/stamp.c
+++ b/arch/blackfin/mach-bf533/boards/stamp.c
@@ -45,6 +45,7 @@
 #include <asm/bfin5xx_spi.h>
 #include <asm/reboot.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -516,7 +517,37 @@ static struct i2c_board_info __initdata bfin_i2c_board_info[] = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 376000000),
+	VRPAIR(VLEV_095, 426000000),
+	VRPAIR(VLEV_100, 426000000),
+	VRPAIR(VLEV_105, 476000000),
+	VRPAIR(VLEV_110, 476000000),
+	VRPAIR(VLEV_115, 476000000),
+	VRPAIR(VLEV_120, 600000000),
+	VRPAIR(VLEV_125, 600000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *stamp_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 	&rtc_device,
 #endif
diff --git a/arch/blackfin/mach-bf537/boards/cm_bf537.c b/arch/blackfin/mach-bf537/boards/cm_bf537.c
index d8a23cd9b9e..73f2142875e 100644
--- a/arch/blackfin/mach-bf537/boards/cm_bf537.c
+++ b/arch/blackfin/mach-bf537/boards/cm_bf537.c
@@ -35,12 +35,15 @@
 #include <linux/mtd/partitions.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
+#if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 #include <linux/usb/isp1362.h>
+#endif
 #include <linux/ata_platform.h>
 #include <linux/irq.h>
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -428,7 +431,37 @@ static struct platform_device bfin_pata_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 376000000),
+	VRPAIR(VLEV_095, 426000000),
+	VRPAIR(VLEV_100, 426000000),
+	VRPAIR(VLEV_105, 476000000),
+	VRPAIR(VLEV_110, 476000000),
+	VRPAIR(VLEV_115, 476000000),
+	VRPAIR(VLEV_120, 500000000),
+	VRPAIR(VLEV_125, 533000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *cm_bf537_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_FB_HITACHI_TX09) || defined(CONFIG_FB_HITACHI_TX09_MODULE)
 	&hitachi_fb_device,
 #endif
diff --git a/arch/blackfin/mach-bf537/boards/stamp.c b/arch/blackfin/mach-bf537/boards/stamp.c
index d3727b7c2d7..9a756d1f3d7 100644
--- a/arch/blackfin/mach-bf537/boards/stamp.c
+++ b/arch/blackfin/mach-bf537/boards/stamp.c
@@ -47,6 +47,7 @@
 #include <asm/bfin5xx_spi.h>
 #include <asm/reboot.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 #include <linux/spi/ad7877.h>
 
 /*
@@ -817,7 +818,37 @@ static struct platform_device bfin_pata_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 376000000),
+	VRPAIR(VLEV_095, 426000000),
+	VRPAIR(VLEV_100, 426000000),
+	VRPAIR(VLEV_105, 476000000),
+	VRPAIR(VLEV_110, 476000000),
+	VRPAIR(VLEV_115, 476000000),
+	VRPAIR(VLEV_120, 500000000),
+	VRPAIR(VLEV_125, 533000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *stamp_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_BFIN_CFPCMCIA) || defined(CONFIG_BFIN_CFPCMCIA_MODULE)
 	&bfin_pcmcia_cf_device,
 #endif
diff --git a/arch/blackfin/mach-bf548/boards/cm_bf548.c b/arch/blackfin/mach-bf548/boards/cm_bf548.c
index e3e8479fffb..3b74f96d359 100644
--- a/arch/blackfin/mach-bf548/boards/cm_bf548.c
+++ b/arch/blackfin/mach-bf548/boards/cm_bf548.c
@@ -36,7 +36,9 @@
 #include <linux/spi/flash.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
+#if defined(CONFIG_USB_MUSB_HDRC) || defined(CONFIG_USB_MUSB_HDRC_MODULE)
 #include <linux/usb/musb.h>
+#endif
 #include <asm/bfin5xx_spi.h>
 #include <asm/cplb.h>
 #include <asm/dma.h>
@@ -44,6 +46,7 @@
 #include <asm/nand.h>
 #include <asm/portmux.h>
 #include <asm/mach/bf54x_keys.h>
+#include <asm/dpmc.h>
 #include <linux/input.h>
 #include <linux/spi/ad7877.h>
 
@@ -590,7 +593,38 @@ static struct platform_device bfin_device_gpiokeys = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+/*
+ * Internal VLEV BF54XSBBC1533
+ ****temporarily using these values until data sheet is updated
+ */
+	VRPAIR(VLEV_085, 150000000),
+	VRPAIR(VLEV_090, 250000000),
+	VRPAIR(VLEV_110, 276000000),
+	VRPAIR(VLEV_115, 301000000),
+	VRPAIR(VLEV_120, 525000000),
+	VRPAIR(VLEV_125, 550000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *cm_bf548_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 	&rtc_device,
 #endif
diff --git a/arch/blackfin/mach-bf548/boards/ezkit.c b/arch/blackfin/mach-bf548/boards/ezkit.c
index b00f68ac6bc..d1682bb3750 100644
--- a/arch/blackfin/mach-bf548/boards/ezkit.c
+++ b/arch/blackfin/mach-bf548/boards/ezkit.c
@@ -46,6 +46,7 @@
 #include <asm/dma.h>
 #include <asm/gpio.h>
 #include <asm/nand.h>
+#include <asm/dpmc.h>
 #include <asm/portmux.h>
 #include <asm/mach/bf54x_keys.h>
 #include <linux/input.h>
@@ -689,7 +690,38 @@ static struct platform_device bfin_gpios_device = {
 	.resource = &bfin_gpios_resources,
 };
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+/*
+ * Internal VLEV BF54XSBBC1533
+ ****temporarily using these values until data sheet is updated
+ */
+	VRPAIR(VLEV_085, 150000000),
+	VRPAIR(VLEV_090, 250000000),
+	VRPAIR(VLEV_110, 276000000),
+	VRPAIR(VLEV_115, 301000000),
+	VRPAIR(VLEV_120, 525000000),
+	VRPAIR(VLEV_125, 550000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *ezkit_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_RTC_DRV_BFIN) || defined(CONFIG_RTC_DRV_BFIN_MODULE)
 	&rtc_device,
 #endif
diff --git a/arch/blackfin/mach-bf561/boards/cm_bf561.c b/arch/blackfin/mach-bf561/boards/cm_bf561.c
index 9fd580952fd..466ef5929a2 100644
--- a/arch/blackfin/mach-bf561/boards/cm_bf561.c
+++ b/arch/blackfin/mach-bf561/boards/cm_bf561.c
@@ -33,12 +33,15 @@
 #include <linux/mtd/partitions.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/flash.h>
+#if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 #include <linux/usb/isp1362.h>
+#endif
 #include <linux/ata_platform.h>
 #include <linux/irq.h>
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -339,8 +342,37 @@ static struct platform_device bfin_pata_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 300000000),
+	VRPAIR(VLEV_095, 313000000),
+	VRPAIR(VLEV_100, 350000000),
+	VRPAIR(VLEV_105, 400000000),
+	VRPAIR(VLEV_110, 444000000),
+	VRPAIR(VLEV_115, 450000000),
+	VRPAIR(VLEV_120, 475000000),
+	VRPAIR(VLEV_125, 500000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *cm_bf561_devices[] __initdata = {
 
+	&bfin_dpmc,
+
 #if defined(CONFIG_FB_HITACHI_TX09) || defined(CONFIG_FB_HITACHI_TX09_MODULE)
 	&hitachi_fb_device,
 #endif
diff --git a/arch/blackfin/mach-bf561/boards/ezkit.c b/arch/blackfin/mach-bf561/boards/ezkit.c
index 0d74b7d9920..61d8f7648b2 100644
--- a/arch/blackfin/mach-bf561/boards/ezkit.c
+++ b/arch/blackfin/mach-bf561/boards/ezkit.c
@@ -39,6 +39,7 @@
 #include <asm/dma.h>
 #include <asm/bfin5xx_spi.h>
 #include <asm/portmux.h>
+#include <asm/dpmc.h>
 
 /*
  * Name the Board for the /proc/cpuinfo
@@ -443,7 +444,37 @@ static struct platform_device i2c_gpio_device = {
 };
 #endif
 
+static const unsigned int cclk_vlev_datasheet[] =
+{
+	VRPAIR(VLEV_085, 250000000),
+	VRPAIR(VLEV_090, 300000000),
+	VRPAIR(VLEV_095, 313000000),
+	VRPAIR(VLEV_100, 350000000),
+	VRPAIR(VLEV_105, 400000000),
+	VRPAIR(VLEV_110, 444000000),
+	VRPAIR(VLEV_115, 450000000),
+	VRPAIR(VLEV_120, 475000000),
+	VRPAIR(VLEV_125, 500000000),
+	VRPAIR(VLEV_130, 600000000),
+};
+
+static struct bfin_dpmc_platform_data bfin_dmpc_vreg_data = {
+	.tuple_tab = cclk_vlev_datasheet,
+	.tabsize = ARRAY_SIZE(cclk_vlev_datasheet),
+	.vr_settling_time = 25 /* us */,
+};
+
+static struct platform_device bfin_dpmc = {
+	.name = "bfin dpmc",
+	.dev = {
+		.platform_data = &bfin_dmpc_vreg_data,
+	},
+};
+
 static struct platform_device *ezkit_devices[] __initdata = {
+
+	&bfin_dpmc,
+
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
diff --git a/arch/blackfin/mach-common/Makefile b/arch/blackfin/mach-common/Makefile
index 393081e9b68..422bfee34ad 100644
--- a/arch/blackfin/mach-common/Makefile
+++ b/arch/blackfin/mach-common/Makefile
@@ -6,5 +6,6 @@ obj-y := \
 	cache.o cacheinit.o entry.o \
 	interrupt.o lock.o irqpanic.o arch_checks.o ints-priority.o
 
-obj-$(CONFIG_PM)         += pm.o dpmc.o
-obj-$(CONFIG_CPU_FREQ)   += cpufreq.o
+obj-$(CONFIG_PM)          += pm.o dpmc_modes.o
+obj-$(CONFIG_CPU_FREQ)    += cpufreq.o
+obj-$(CONFIG_CPU_VOLTAGE) += dpmc.o
diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
index ed81e00d20e..75cdad291e8 100644
--- a/arch/blackfin/mach-common/cpufreq.c
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -62,6 +62,14 @@ static struct bfin_dpm_state {
 	unsigned int tscale; /* change the divider on the core timer interrupt */
 } dpm_state_table[3];
 
+/*
+   normalized to maximum frequncy offset for CYCLES,
+   used in time-ts cycles clock source, but could be used
+   somewhere also.
+ */
+unsigned long long __bfin_cycles_off;
+unsigned int __bfin_cycles_mod;
+
 /**************************************************************************/
 
 static unsigned int bfin_getfreq(unsigned int cpu)
@@ -80,6 +88,7 @@ static int bfin_target(struct cpufreq_policy *policy,
 	unsigned int index, plldiv, tscale;
 	unsigned long flags, cclk_hz;
 	struct cpufreq_freqs freqs;
+	cycles_t cycles;
 
 	if (cpufreq_frequency_table_target(policy, bfin_freq_table,
 		 target_freq, relation, &index))
@@ -101,8 +110,14 @@ static int bfin_target(struct cpufreq_policy *policy,
 		bfin_write_PLL_DIV(plldiv);
 		/* we have to adjust the core timer, because it is using cclk */
 		bfin_write_TSCALE(tscale);
+		cycles = get_cycles();
 		SSYNC();
+	cycles += 10; /* ~10 cycles we loose after get_cycles() */
+	__bfin_cycles_off += (cycles << __bfin_cycles_mod) - (cycles << index);
+	__bfin_cycles_mod = index;
 	local_irq_restore(flags);
+	/* TODO: just test case for cycles clock source, remove later */
+	pr_debug("cpufreq: done\n");
 	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
 
 	return 0;
@@ -119,22 +134,13 @@ static int __init __bfin_cpu_init(struct cpufreq_policy *policy)
 	unsigned long cclk, sclk, csel, min_cclk;
 	int index;
 
-#ifdef CONFIG_CYCLES_CLOCKSOURCE
-/*
- * Clocksource CYCLES is still CONTINUOUS but not longer MONOTONIC in case we enable
- * CPU frequency scaling, since CYCLES runs off Core Clock.
- */
-	printk(KERN_WARNING "CPU frequency scaling not supported: Clocksource not suitable\n"
-		return -ENODEV;
-#endif
-
 	if (policy->cpu != 0)
 		return -EINVAL;
 
 	cclk = get_cclk();
 	sclk = get_sclk();
 
-#if ANOMALY_05000273
+#if ANOMALY_05000273 || (!defined(CONFIG_BF54x) && defined(CONFIG_BFIN_DCACHE))
 	min_cclk = sclk * 2;
 #else
 	min_cclk = sclk;
diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
new file mode 100644
index 00000000000..02c7efd1bcf
--- /dev/null
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright 2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+#include <linux/cpufreq.h>
+
+#include <asm/delay.h>
+#include <asm/dpmc.h>
+
+#define DRIVER_NAME "bfin dpmc"
+
+#define dprintk(msg...) \
+	cpufreq_debug_printk(CPUFREQ_DEBUG_DRIVER, DRIVER_NAME, msg)
+
+struct bfin_dpmc_platform_data *pdata;
+
+/**
+ *	bfin_set_vlev - Update VLEV field in VR_CTL Reg.
+ *			Avoid BYPASS sequence
+ */
+static void bfin_set_vlev(unsigned int vlev)
+{
+	unsigned pll_lcnt;
+
+	pll_lcnt = bfin_read_PLL_LOCKCNT();
+
+	bfin_write_PLL_LOCKCNT(1);
+	bfin_write_VR_CTL((bfin_read_VR_CTL() & ~VLEV) | vlev);
+	bfin_write_PLL_LOCKCNT(pll_lcnt);
+}
+
+/**
+ *	bfin_get_vlev - Get CPU specific VLEV from platform device data
+ */
+static unsigned int bfin_get_vlev(unsigned int freq)
+{
+	int i;
+
+	if (!pdata)
+		goto err_out;
+
+	freq >>= 16;
+
+	for (i = 0; i < pdata->tabsize; i++)
+		if (freq <= (pdata->tuple_tab[i] & 0xFFFF))
+			return pdata->tuple_tab[i] >> 16;
+
+err_out:
+	printk(KERN_WARNING "DPMC: No suitable CCLK VDDINT voltage pair found\n");
+	return VLEV_120;
+}
+
+#ifdef CONFIG_CPU_FREQ
+static int
+vreg_cpufreq_notifier(struct notifier_block *nb, unsigned long val, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (val == CPUFREQ_PRECHANGE && freq->old < freq->new) {
+		bfin_set_vlev(bfin_get_vlev(freq->new));
+		udelay(pdata->vr_settling_time); /* Wait until Volatge settled */
+
+	} else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new)
+		bfin_set_vlev(bfin_get_vlev(freq->new));
+
+	return 0;
+}
+
+static struct notifier_block vreg_cpufreq_notifier_block = {
+	.notifier_call	= vreg_cpufreq_notifier
+};
+#endif /* CONFIG_CPU_FREQ */
+
+/**
+ *	bfin_dpmc_probe -
+ *
+ */
+static int __devinit bfin_dpmc_probe(struct platform_device *pdev)
+{
+	if (pdev->dev.platform_data)
+		pdata = pdev->dev.platform_data;
+	else
+		return -EINVAL;
+
+	return cpufreq_register_notifier(&vreg_cpufreq_notifier_block,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+/**
+ *	bfin_dpmc_remove -
+ */
+static int __devexit bfin_dpmc_remove(struct platform_device *pdev)
+{
+	pdata = NULL;
+	return cpufreq_unregister_notifier(&vreg_cpufreq_notifier_block,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+struct platform_driver bfin_dpmc_device_driver = {
+	.probe   = bfin_dpmc_probe,
+	.remove  = __devexit_p(bfin_dpmc_remove),
+	.driver  = {
+		.name = DRIVER_NAME,
+	}
+};
+
+/**
+ *	bfin_dpmc_init - Init driver
+ */
+static int __init bfin_dpmc_init(void)
+{
+	return platform_driver_register(&bfin_dpmc_device_driver);
+}
+module_init(bfin_dpmc_init);
+
+/**
+ *	bfin_dpmc_exit - break down driver
+ */
+static void __exit bfin_dpmc_exit(void)
+{
+	platform_driver_unregister(&bfin_dpmc_device_driver);
+}
+module_exit(bfin_dpmc_exit);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("cpu power management driver for Blackfin");
+MODULE_LICENSE("GPL");
diff --git a/arch/blackfin/mach-common/dpmc.S b/arch/blackfin/mach-common/dpmc_modes.S
index 9d45aa3265b..b7981d31c39 100644
--- a/arch/blackfin/mach-common/dpmc.S
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -1,30 +1,7 @@
 /*
- * File:         arch/blackfin/mach-common/dpmc.S
- * Based on:
- * Author:       LG Soft India
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * Created:      ?
- * Description:  Watchdog Timer APIs
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * 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
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/linkage.h>
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index f2fb87e9a46..038f70e0be6 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -151,26 +151,62 @@ ENTRY(_ex_soft_bp)
 ENDPROC(_ex_soft_bp)
 
 ENTRY(_ex_single_step)
+	/* If we just returned from an interrupt, the single step event is
+	   for the RTI instruction.  */
 	r7 = retx;
 	r6 = reti;
 	cc = r7 == r6;
-	if cc jump _bfin_return_from_exception
-	r7 = syscfg;
-	bitclr (r7, 0);
-	syscfg = R7;
+	if cc jump _bfin_return_from_exception;
 
+	/* If we were in user mode, do the single step normally.  */
 	p5.l = lo(IPEND);
 	p5.h = hi(IPEND);
 	r6 = [p5];
-	cc = bittst(r6, 5);
-	if !cc jump _ex_trap_c;
-	p4.l = lo(EVT5);
-	p4.h = hi(EVT5);
-	r6.h = _exception_to_level5;
-	r6.l = _exception_to_level5;
-	r7 = [p4];
-	cc = r6 == r7;
-	if !cc jump _ex_trap_c;
+	r7 = 0xffe0 (z);
+	r7 = r7 & r6;
+	cc = r7 == 0;
+	if !cc jump 1f;
+
+	/* Single stepping only a single instruction, so clear the trace
+	 * bit here.  */
+	r7 = syscfg;
+	bitclr (r7, 0);
+	syscfg = R7;
+	jump _ex_trap_c;
+
+1:
+	/*
+	 * We were in an interrupt handler.  By convention, all of them save
+	 * SYSCFG with their first instruction, so by checking whether our
+	 * RETX points at the entry point, we can determine whether to allow
+	 * a single step, or whether to clear SYSCFG.
+	 *
+	 * First, find out the interrupt level and the event vector for it.
+	 */
+	p5.l = lo(EVT0);
+	p5.h = hi(EVT0);
+	p5 += -4;
+2:
+	r7 = rot r7 by -1;
+	p5 += 4;
+	if !cc jump 2b;
+
+	/* What we actually do is test for the _second_ instruction in the
+	 * IRQ handler.  That way, if there are insns following the restore
+	 * of SYSCFG after leaving the handler, we will not turn off SYSCFG
+	 * for them.  */
+
+	r7 = [p5];
+	r7 += 2;
+	r6 = RETX;
+	cc = R7 == R6;
+	if !cc jump _bfin_return_from_exception;
+
+	r7 = syscfg;
+	bitclr (r7, 0);
+	syscfg = R7;
+
+	/* Fall through to _bfin_return_from_exception.  */
 ENDPROC(_ex_single_step)
 
 ENTRY(_bfin_return_from_exception)
@@ -234,20 +270,26 @@ ENTRY(_ex_trap_c)
 	p5.l = _saved_icplb_fault_addr;
 	[p5] = r7;
 
-	p4.l = __retx;
-	p4.h = __retx;
+	p4.l = _excpt_saved_stuff;
+	p4.h = _excpt_saved_stuff;
+
 	r6 = retx;
 	[p4] = r6;
-	p4.l = lo(SAFE_USER_INSTRUCTION);
-	p4.h = hi(SAFE_USER_INSTRUCTION);
-	retx = p4;
+
+	r6 = SYSCFG;
+	[p4 + 4] = r6;
+	BITCLR(r6, 0);
+	SYSCFG = r6;
 
 	/* Disable all interrupts, but make sure level 5 is enabled so
 	 * we can switch to that level.  Save the old mask.  */
 	cli r6;
-	p4.l = _excpt_saved_imask;
-	p4.h = _excpt_saved_imask;
-	[p4] = r6;
+	[p4 + 8] = r6;
+
+	p4.l = lo(SAFE_USER_INSTRUCTION);
+	p4.h = hi(SAFE_USER_INSTRUCTION);
+	retx = p4;
+
 	r6 = 0x3f;
 	sti r6;
 
@@ -295,6 +337,11 @@ ENTRY(_double_fault)
          */
         SAVE_ALL_SYS
 
+	/* The dumping functions expect the return address in the RETI
+	 * slot.  */
+	r6 = retx;
+	[sp + PT_PC] = r6;
+
         r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
         SP += -12;
         call _double_fault_c;
@@ -307,16 +354,17 @@ ENDPROC(_double_fault)
 ENTRY(_exception_to_level5)
 	SAVE_ALL_SYS
 
-	p4.l = __retx;
-	p4.h = __retx;
+	p4.l = _excpt_saved_stuff;
+	p4.h = _excpt_saved_stuff;
 	r6 = [p4];
 	[sp + PT_PC] = r6;
 
+	r6 = [p4 + 4];
+	[sp + PT_SYSCFG] = r6;
+
 	/* Restore interrupt mask.  We haven't pushed RETI, so this
 	 * doesn't enable interrupts until we return from this handler.  */
-	p4.l = _excpt_saved_imask;
-	p4.h = _excpt_saved_imask;
-	r6 = [p4];
+	r6 = [p4 + 8];
 	sti r6;
 
 	/* Restore the hardware error vector.  */
@@ -1344,7 +1392,14 @@ ENTRY(_sys_call_table)
 	.rept NR_syscalls-(.-_sys_call_table)/4
 	.long _sys_ni_syscall
 	.endr
-_excpt_saved_imask:
+
+	/*
+	 * Used to save the real RETX, IMASK and SYSCFG when temporarily
+	 * storing safe values across the transition from exception to IRQ5.
+	 */
+_excpt_saved_stuff:
+	.long 0;
+	.long 0;
 	.long 0;
 
 _exception_stack:
@@ -1358,7 +1413,3 @@ _exception_stack_top:
 _last_cplb_fault_retx:
 	.long 0;
 #endif
-	/* Used to save the real RETX when temporarily storing a safe
-	 * return address.  */
-__retx:
-	.long 0;