diff --git a/firmware/SOURCES b/firmware/SOURCES
index b69d51af62..b13f6a0df8 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -880,7 +880,7 @@ target/arm/imx31/i2c-imx31.c
 target/arm/imx31/iomuxc-imx31.c
 target/arm/imx31/mc13783-imx31.c
 target/arm/imx31/mmu-imx31.c
-target/arm/imx31/rolo_restart.S
+target/arm/imx31/rolo_restart_firmware.S
 target/arm/imx31/sdma-imx31.c
 target/arm/imx31/spi-imx31.c
 target/arm/imx31/gigabeat-s/adc-gigabeat-s.c
diff --git a/firmware/export/config/gigabeats.h b/firmware/export/config/gigabeats.h
index 6d0c25a4d6..e64ceb0171 100644
--- a/firmware/export/config/gigabeats.h
+++ b/firmware/export/config/gigabeats.h
@@ -147,7 +147,7 @@
 #define GPIO_EVENT_MASK (USE_GPIO1_EVENTS)
 
 /* Define this if target has an additional number of threads specific to it */
-#define TARGET_EXTRA_THREADS 2
+#define TARGET_EXTRA_THREADS 3
 
 /* Type of mobile power - check this out */
 #define BATTERY_CAPACITY_DEFAULT 700 /* default battery capacity */
@@ -168,9 +168,10 @@
 #define FLASH_SIZE 0x200000
 
 /* Define this to the CPU frequency */
-/* TODO */
 #define CPU_FREQ 264000000 /* Set by retailOS loader */
 
+#define FREQ cpu_frequency
+
 /* define this if the unit can be powered or charged via USB */
 #define HAVE_USB_POWER
 #define USBPOWER_BUTTON     BUTTON_MENU
diff --git a/firmware/export/imx31l.h b/firmware/export/imx31l.h
index 3f94156650..66ae0acc4d 100644
--- a/firmware/export/imx31l.h
+++ b/firmware/export/imx31l.h
@@ -1336,39 +1336,6 @@
 #define CCM_DCVR_ELV                    (0x3ff <<  2) /* Emergency limit */
 #define CCM_DCVR_ELV_POS                (2)
 
-#if 0
-enum DVFS_W_SIGS
-{
-    DVFS_W_SIGS_M3IF_M0_BUF    = 0,  /* Hready signal of M3IF's master #0
-                                        (L2 Cache) */
-    DVFS_W_SIGS_M3IF_M1        = 1,  /* Hready signal of M3IF's master #1
-                                        (L2 Cache) */
-    DVFS_W_SIGS_MBX_MBXCLKGATE = 2,  /* Hready signal of M3IF's master #2
-                                        (MBX) */
-    DVFS_W_SIGS_M3IF_M3        = 3,  /* Hready signal of M3IF's master #3
-                                        (MAX) */
-    DVFS_W_SIGS_M3IF_M4        = 4,  /* Hready signal of M3IF's master #4
-                                        (SDMA) */
-    DVFS_W_SIGS_M3IF_M5        = 5,  /* Hready signal of M3IF's master #5
-                                        (mpeg4_vga_encoder) */
-    DVFS_W_SIGS_M3IF_M6        = 6,  /* Hready signal of M3IF's master #6
-                                        (IPU) */
-    DVFS_W_SIGS_M3IF_M7        = 7,  /* Hready signal of M3IF's master #7
-                                        (IPU) */
-    DVFS_W_SIGS_ARM11_P_IRQ_B_RBT_GATE = 8, /* ARM normal interrupt */
-    DVFS_W_SIGS_ARM11_P_FIQ_B_RBT_GATE = 9, /* ARM fast interrupt */
-    DVFS_W_SIGS_IPI_GPIO1_INT0 = 10, /* Interrupt line from GPIO */
-    DVFS_W_SIGS_IPI_INT_IPU_FUNC = 11, /* Interrupt line from IPU */
-    DVFS_W_SIGS_DVGP0          = 12, /* Software-controllable general-purpose
-                                        bits from the CCM */
-    DVFS_W_SIGS_DVGP1          = 13, /* Software-controllable general-purpose
-                                        bits from the CCM */
-    DVFS_W_SIGS_DVGP2          = 14, /* Software-controllable general-purpose
-                                        bits from the CCM */
-    DVFS_W_SIGS_DVGP3          = 15, /* Software-controllable general-purpose
-                                        bits from the CCM */
-};
-#endif
 
 /* LTR0 */
 #define CCM_LTR0_UPTHR                  (0x3f << 22)
@@ -1383,15 +1350,26 @@ enum DVFS_W_SIGS
 #define CCM_LTR0_DIV3CK_32768           (0x2 << 1) /* 1/32768 ARM clock */
 #define CCM_LTR0_DIV3CK_131072          (0x3 << 1) /* 1/131072 ARM clock */
 
+/* LTR1 */
+#define CCM_LTR1_LTBRSH                 (1 << 23)
+#define CCM_LTR1_LTBRSR                 (1 << 22)
+#define CCM_LTR1_DNCNT                  (0xff << 14)
+#define CCM_LTR1_DNCNT_POS              (14)
+#define CCM_LTR1_UPCNT                  (0xff << 6)
+#define CCM_LTR1_UPCNT_POS              (6)
+#define CCM_LTR1_PNCTHR                 (0x3f << 0)
+#define CCM_LTR1_PNCTHR_POS             (0)
+
+/* LTR2 */
+#define CCM_LTR2_EMAC                   (0x1ff)
+#define CCM_LTR2_EMAC_POS               (0)
+
 /* PMCR0 */
-#define CCM_PMCR0_DVSUP_MCUPLL          (1 << 31)
-#define CCM_PMCR0_DVSUP_POST_DIVIDERS   (1 << 30)
-#define CCM_PMCR0_DVSUP_DVS             (0x3 << 28)
-#define CCM_PMCR0_DVS1_0_DVS0_0         (0x0 << 28) /* Highest frequency/voltage */
-#define CCM_PMCR0_DVS1_0_DVS0_1         (0x1 << 28) /* ... */
-#define CCM_PMCR0_DVS1_1_DVS0_0         (0x2 << 28) /* ... */
-#define CCM_PMCR0_DVS1_1_DVS0_1         (0x3 << 28) /* Lowest frequency/voltage */
-#define CCM_PMCR0_DVS_POS               (28)
+#define CCM_PMCR0_DFSUP_MCUPLL          (1 << 31)
+#define CCM_PMCR0_DFSUP_MCUPLL_POS      (31)
+#define CCM_PMCR0_DFSUP_POST_DIVIDERS   (1 << 30)
+#define CCM_PMCR0_DVSUP                 (0x3 << 28)
+#define CCM_PMCR0_DVSUP_POS             (28)
 #define CCM_PMCR0_UDSC                  (1 << 27)
 #define CCM_PMCR0_VSCNT                 (0x7 << 24)
 #define CCM_PMCR0_VSCNT_POS             (24)
@@ -1431,6 +1409,13 @@ enum DVFS_W_SIGS
 #define CCM_PMCR0_DPTEN                 (1 << 0)
 
 
+/* PMCR1 */
+#define CCM_PMCR1_DVGP_POS              (0)
+#define CCM_PMCR1_DVGP_MASK             (0xf << 0)
+
+/* IC revision 2.0 or greater ONLY! */
+#define CCM_PMCR1_EMIRQ_EN              (1 << 8)
+#define CCM_PMCR1_PLLRDIS               (1 << 7) /* No PLL reset on switch */
 
 
 /* WEIM - CS0 */
diff --git a/firmware/export/system.h b/firmware/export/system.h
index 9df382bc24..3984ebeb11 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -66,15 +66,17 @@ void cpu_boost(bool on_off);
 #endif
 void cpu_idle_mode(bool on_off);
 int get_cpu_boost_counter(void);
-#else
+#else /* ndef HAVE_ADJUSTABLE_CPU_FREQ */
+#ifndef FREQ
 #define FREQ CPU_FREQ
+#endif
 #define set_cpu_frequency(frequency)
 #define cpu_boost(on_off)
 #define cpu_boost_id(on_off, id)
 #define cpu_idle_mode(on_off)
 #define get_cpu_boost_counter()
 #define get_cpu_boost_tracker()
-#endif
+#endif /* HAVE_ADJUSTABLE_CPU_FREQ */
 
 #ifdef CPU_BOOST_LOGGING
 #define cpu_boost(on_off) cpu_boost_(on_off,__FILE__,  __LINE__)
diff --git a/firmware/rolo.c b/firmware/rolo.c
index a8ea1cdf5e..87e6958fca 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -115,7 +115,7 @@ static void rolo_error(const char *text)
 /* these are in assembler file "descramble.S" for SH7034 */
 extern unsigned short descramble(const unsigned char* source,
                                  unsigned char* dest, int length);
-/* this is in firmware/target/arm/imx31/rolo_restart.S for IMX31 */
+/* this is in firmware/target/arm/imx31/rolo_restart.c for IMX31 */
 extern void rolo_restart(const unsigned char* source, unsigned char* dest,
                          int length);
 #else
@@ -300,6 +300,7 @@ int rolo_load(const char* filename)
 #endif
     adc_close();
 
+#if CONFIG_CPU != IMX31L /* We're not finished yet */
 #ifdef CPU_ARM
     /* Should do these together since some ARM version should never have
      * FIQ disabled and not IRQ (imx31 errata). */
@@ -308,6 +309,7 @@ int rolo_load(const char* filename)
     /* Some targets have a higher disable level than HIGEST_IRQ_LEVEL */
     set_irq_level(DISABLE_INTERRUPTS);
 #endif
+#endif /* CONFIG_CPU == IMX31L */
 
 #else /* CONFIG_CPU == SH7034 */
     /* Read file length from header and compare to real file length */
diff --git a/firmware/target/arm/imx31/avic-imx31.c b/firmware/target/arm/imx31/avic-imx31.c
index 4ba7da4be0..c8bf419bcd 100644
--- a/firmware/target/arm/imx31/avic-imx31.c
+++ b/firmware/target/arm/imx31/avic-imx31.c
@@ -128,7 +128,7 @@ void avic_init(void)
         : : : "r0");
 
     /* Enable normal interrupts at all priorities */
-    avic->nimask = 0x1f;
+    avic->nimask = AVIC_NIL_ENABLE;
 }
 
 void avic_set_int_priority(enum IMX31_INT_LIST ints,
@@ -210,3 +210,8 @@ void avic_set_int_type(enum IMX31_INT_LIST ints, enum INT_TYPE intstype)
 
     restore_interrupt(oldstatus);
 }
+
+void avic_set_ni_level(unsigned int level)
+{
+    AVIC_NIMASK = level > 0x1f ? 0x1f : level;
+}
diff --git a/firmware/target/arm/imx31/avic-imx31.h b/firmware/target/arm/imx31/avic-imx31.h
index a049713600..ba8f91cc1a 100644
--- a/firmware/target/arm/imx31/avic-imx31.h
+++ b/firmware/target/arm/imx31/avic-imx31.h
@@ -172,7 +172,11 @@ struct avic_map
     };
 };
 
+/* #define IRQ priorities for different modules (0-15) */
 #define INT_PRIO_DEFAULT    7
+#define INT_PRIO_DVFS       (INT_PRIO_DEFAULT+1)
+#define INT_PRIO_DPTC       (INT_PRIO_DEFAULT+1)
+#define INT_PRIO_SDMA       (INT_PRIO_DEFAULT+2)
 
 enum INT_TYPE
 {
@@ -210,4 +214,37 @@ void avic_set_int_priority(enum IMX31_INT_LIST ints,
 void avic_disable_int(enum IMX31_INT_LIST ints);
 void avic_set_int_type(enum IMX31_INT_LIST ints, enum INT_TYPE intstype);
 
+#define AVIC_NIL_DISABLE 0xf
+#define AVIC_NIL_ENABLE  0x1f
+void avic_set_ni_level(unsigned int level);
+
+/* Call a service routine while allowing preemption by interrupts of higher
+ * priority. r4-r7 must be preserved for epilogue code to restore context. */
+#define AVIC_NESTED_NI_CALL_PROLOGUE() \
+({ asm volatile ( \
+        "sub    lr, lr, #4               \n" /* prepare return address */ \
+        "stmfd  sp!, { r0-r7, r12, lr }  \n" /* preserve return context */ \
+        "mov    r0, #0x68000000          \n" /* AVIC_BASE_ADDR */ \
+        "mrs    r4, spsr                 \n" /* save SPSR_irq */ \
+        "ldr    r5, [r0, #0x04]          \n" /* save NIMASK */ \
+        "ldr    r1, [r0, #0x40]          \n" /* load NIVECSR */ \
+        "mov    r2, sp                   \n" /* remember IRQ stack to use in call */ \
+        "str    r1, [r0, #0x04]          \n" /* copy NIVECSR to NIMASK */ \
+        "cps    #0x13                    \n" /* switch to SVC mode (+ unmask IRQ) */ \
+        "mov    r6, sp                   \n" /* save SP_svc */ \
+        "mov    r7, lr                   \n" /* save LR_svc */ \
+        "mov    sp, r2                   \n" /* switch to SP_irq */ \
+        ); })
+
+#define AVIC_NESTED_NI_CALL_EPILOGUE() \
+({ asm volatile ( \
+        "mov    sp, r6                   \n" /* restore SP_svc */ \
+        "mov    lr, r7                   \n" /* restore LR_svc */  \
+        "cps    #0x12                    \n" /* return to IRQ mode (+ mask IRQ) */ \
+        "mov    r0, #0x68000000          \n" /* AVIC BASE ADDR */ \
+        "msr    spsr_cxsf, r4            \n" /* restore SPSR_irq */ \
+        "str    r5, [r0, #0x04]          \n" /* restore NIMASK */ \
+        "ldmfd  sp!, { r0-r7, r12, pc }^ \n" /* reload context and return */ \
+        ); })
+
 #endif /* AVIC_IMX31_H */
diff --git a/firmware/target/arm/imx31/ccm-imx31.c b/firmware/target/arm/imx31/ccm-imx31.c
index 0d166e5dbf..2cf2080cf1 100644
--- a/firmware/target/arm/imx31/ccm-imx31.c
+++ b/firmware/target/arm/imx31/ccm-imx31.c
@@ -24,7 +24,8 @@
 #include "cpu.h"
 #include "ccm-imx31.h"
 
-unsigned int ccm_get_src_pll(void)
+/* Return the current source pll for MCU */
+enum IMX31_PLLS ccm_get_src_pll(void)
 {
     return (CCM_PMCR0 & 0xC0000000) == 0 ? PLL_SERIAL : PLL_MCU;
 }
@@ -45,8 +46,21 @@ void ccm_module_clock_gating(enum IMX31_CG_LIST cg, enum IMX31_CG_MODES mode)
     imx31_regmod32(reg, mode << shift, mask);
 }
 
+/* Decode PLL output frequency from register value */
+unsigned int ccm_calc_pll_rate(unsigned int infreq, unsigned long regval)
+{
+    uint32_t mfn = regval & 0x3ff;
+    uint32_t pd  = ((regval >> 26) & 0xf) + 1;
+    uint32_t mfd = ((regval >> 16) & 0x3ff) + 1;
+    uint32_t mfi = (regval >> 10) & 0xf;
+
+    mfi = mfi <= 5 ? 5 : mfi;
+
+    return 2ull*infreq*(mfi * mfd + mfn) / (mfd * pd);
+}
+
 /* Get the PLL reference clock frequency in HZ */
-unsigned int ccm_get_pll_ref_clk(void)
+unsigned int ccm_get_pll_ref_clk_rate(void)
 {
     if ((CCM_CCMR & (3 << 1)) == (1 << 1))
         return CONFIG_CKIL_FREQ * 1024;
@@ -55,41 +69,59 @@ unsigned int ccm_get_pll_ref_clk(void)
 }
 
 /* Return PLL frequency in HZ */
-unsigned int ccm_get_pll(enum IMX31_PLLS pll)
+unsigned int ccm_get_pll_rate(enum IMX31_PLLS pll)
 {
-    uint32_t infreq = ccm_get_pll_ref_clk();
-    uint32_t reg = (&CCM_MPCTL)[pll];
-    uint32_t mfn = reg & 0x3ff;
-    uint32_t pd  = ((reg >> 26) & 0xf) + 1;
-    uint64_t mfd = ((reg >> 16) & 0x3ff) + 1;
-    uint32_t mfi = (reg >> 10) & 0xf;
+    return ccm_calc_pll_rate(ccm_get_pll_ref_clk_rate(), (&CCM_MPCTL)[pll]);
+}
 
-    mfi = mfi <= 5 ? 5 : mfi;
+unsigned int ccm_get_mcu_clk(void)
+{
+    unsigned int pllnum = ccm_get_src_pll();
+    unsigned int fpll = ccm_get_pll_rate(pllnum);
+    unsigned int mcu_podf = (CCM_PDR0 & 0x7) + 1;
 
-    return 2*infreq*(mfi * mfd + mfn) / (mfd * pd);
+    return fpll / mcu_podf;
 }
 
 unsigned int ccm_get_ipg_clk(void)
 {
     unsigned int pllnum = ccm_get_src_pll();
-    unsigned int pll = ccm_get_pll(pllnum);
+    unsigned int fpll = ccm_get_pll_rate(pllnum);
     uint32_t reg = CCM_PDR0;
     unsigned int max_pdf = ((reg >> 3) & 0x7) + 1;
     unsigned int ipg_pdf = ((reg >> 6) & 0x3) + 1;
 
-    return pll / (max_pdf * ipg_pdf);
+    return fpll / (max_pdf * ipg_pdf);
 }
 
 unsigned int ccm_get_ahb_clk(void)
 {
     unsigned int pllnum = ccm_get_src_pll();
-    unsigned int pll = ccm_get_pll(pllnum);
+    unsigned int fpll = ccm_get_pll_rate(pllnum);
     unsigned int max_pdf = ((CCM_PDR0 >> 3) & 0x7) + 1;
 
-    return pll / max_pdf;
+    return fpll / max_pdf;
 }
 
 unsigned int ccm_get_ata_clk(void)
 {
     return ccm_get_ipg_clk();
 }
+
+/* Write new values to the current PLL and post-dividers */
+void ccm_set_mcupll_and_pdr(unsigned long pllctl, unsigned long pdr)
+{
+    unsigned int pll = ccm_get_src_pll();
+    volatile unsigned long *pllreg = &(&CCM_MPCTL)[pll];
+    unsigned long fref = ccm_get_pll_ref_clk_rate();
+    unsigned long curfreq = ccm_calc_pll_rate(fref, *pllreg);
+    unsigned long newfreq = ccm_calc_pll_rate(fref, pllctl);
+
+    if (newfreq > curfreq)
+        CCM_PDR0 = pdr;
+
+    *pllreg = pllctl;
+
+    if (newfreq <= curfreq)
+        CCM_PDR0 = pdr;
+}
diff --git a/firmware/target/arm/imx31/ccm-imx31.h b/firmware/target/arm/imx31/ccm-imx31.h
index e95891255d..400f6cad5d 100644
--- a/firmware/target/arm/imx31/ccm-imx31.h
+++ b/firmware/target/arm/imx31/ccm-imx31.h
@@ -93,11 +93,21 @@ enum IMX31_PLLS
     NUM_PLLS,
 };
 
+
+/* Return the current source pll for MCU */
+enum IMX31_PLLS ccm_get_src_pll(void);
+
+/* Decode PLL output frequency from register value */
+unsigned int ccm_calc_pll_rate(unsigned int infreq, unsigned long regval);
+
 /* Get the PLL reference clock frequency in HZ */
-unsigned int ccm_get_pll_ref_clk(void);
+unsigned int ccm_get_pll_ref_clk_rate(void);
 
 /* Return PLL frequency in HZ */
-unsigned int ccm_get_pll(enum IMX31_PLLS pll);
+unsigned int ccm_get_pll_rate(enum IMX31_PLLS pll);
+
+/* Return MCU frequency in HZ */
+unsigned int ccm_get_mcu_clk(void);
 
 /* Return ipg_clk in HZ */
 unsigned int ccm_get_ipg_clk(void);
@@ -108,4 +118,7 @@ unsigned int ccm_get_ahb_clk(void);
 /* Return the ATA frequency in HZ */
 unsigned int ccm_get_ata_clk(void);
 
+/* Write new values to the current PLL and post-dividers */
+void ccm_set_mcupll_and_pdr(unsigned long pllctl, unsigned long pdr);
+
 #endif /* _CCM_IMX31_H_ */
diff --git a/firmware/target/arm/imx31/debug-imx31.c b/firmware/target/arm/imx31/debug-imx31.c
index dc3c293495..d854c9bc36 100644
--- a/firmware/target/arm/imx31/debug-imx31.c
+++ b/firmware/target/arm/imx31/debug-imx31.c
@@ -29,6 +29,7 @@
 #include "mc13783.h"
 #include "adc.h"
 #include "ccm-imx31.h"
+#include "dvfs_dptc-imx31.h"
 
 bool __dbg_hw_info(void)
 {
@@ -39,6 +40,9 @@ bool __dbg_hw_info(void)
     unsigned int freq;
     uint32_t regval;
 
+    extern volatile unsigned int dvfs_nr_dn, dvfs_nr_up, dvfs_nr_pnc;
+    extern volatile unsigned int dptc_nr_dn, dptc_nr_up, dptc_nr_pnc;
+
     lcd_clear_display();
     lcd_setfont(FONT_SYSFIXED);
 
@@ -52,11 +56,11 @@ bool __dbg_hw_info(void)
         spctl = CCM_SPCTL;
         upctl = CCM_UPCTL;
 
-        pllref = ccm_get_pll_ref_clk();
+        pllref = ccm_get_pll_ref_clk_rate();
 
-        mcu_pllfreq = ccm_get_pll(PLL_MCU);
-        ser_pllfreq = ccm_get_pll(PLL_SERIAL);
-        usb_pllfreq = ccm_get_pll(PLL_USB);
+        mcu_pllfreq = ccm_calc_pll_rate(pllref, mpctl);
+        ser_pllfreq = ccm_calc_pll_rate(pllref, spctl);
+        usb_pllfreq = ccm_calc_pll_rate(pllref, upctl);
 
         lcd_putsf(0, line++, "pll_ref_clk: %u", pllref);
         line++;
@@ -107,10 +111,29 @@ bool __dbg_hw_info(void)
 
         freq = usb_pllfreq / (((CCM_PDR0 >> 16) & 0x1f) + 1);
         lcd_putsf(0, line++, "   ipg_per_baud:  %u", freq);
+
+        line++;
+        lcd_putsf(0, line++, "PMCR0: %08lX", CCM_PMCR0);
+
+        line++;
+        lcd_putsf(0, line++, "cpu_frequency: %ld Hz", cpu_frequency);
+
+        lcd_putsf(0, line++, "dvfs_level: %u", dvfs_get_level());
+        lcd_putsf(0, line++, "dvfs d|u|p: %u %u %u", dvfs_nr_dn, dvfs_nr_up, dvfs_nr_pnc);
+        regval = dvfs_dptc_get_voltage();
+        lcd_putsf(0, line++, "cpu_voltage: %d.%03d V", regval / 1000,
+                  regval % 1000);
+
+        lcd_putsf(0, line++, "dptc_wp: %u", dptc_get_wp());
+        lcd_putsf(0, line++, "dptc d|u|p: %u %u %u", dptc_nr_dn, dptc_nr_up, dptc_nr_pnc);
+        lcd_putsf(0, line++, "DVCR0,1: %08lX %08lX", CCM_DCVR0, CCM_DCVR1);
+        lcd_putsf(0, line++, "DVCR2,3: %08lX %08lX", CCM_DCVR2, CCM_DCVR3);
+        lcd_putsf(0, line++, "SWITCHERS0: %08lX", mc13783_read(MC13783_SWITCHERS0));
+        lcd_putsf(0, line++, "SWITCHERS1: %08lX", mc13783_read(MC13783_SWITCHERS1));
           
         lcd_update();
 
-        if (button_get(true) == (DEBUG_CANCEL|BUTTON_REL))
+        if (button_get_w_tmo(HZ/10) == (DEBUG_CANCEL|BUTTON_REL))
             return false;
     }
 }
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 8f32fd0fba..6e177ba6bd 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -22,27 +22,721 @@
  ****************************************************************************/
 #include "config.h"
 #include "system.h"
-#include "ccm-imx31.h"
+#include "logf.h"
 #include "mc13783.h"
+#include "iomuxc-imx31.h"
+#include "ccm-imx31.h"
+#include "avic-imx31.h"
+#include "dvfs_dptc-imx31.h"
+#include "dvfs_dptc_tables-target.h"
 
 /* Most of the code in here is based upon the Linux BSP provided by Freescale
  * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved. */
 
-void dvfs_dptc_start(void)
+/* The current DVFS index level */
+static volatile unsigned int dvfs_level = DVFS_LEVEL_DEFAULT;
+/* The current DPTC working point */
+static volatile unsigned int dptc_wp = DPTC_WP_DEFAULT;
+
+
+static void update_dptc_counts(unsigned int level, unsigned int wp)
 {
-    /* For now, just set the regulator voltage off of overdrive mode */
-    /* For 264 MHz, DPTC is not needed and lower V can be used */
+    int oldlevel = disable_irq_save();
+    const struct dptc_dcvr_table_entry *entry = &dptc_dcvr_table[level][wp];
+
+    CCM_DCVR0 = entry->dcvr0;
+    CCM_DCVR1 = entry->dcvr1;
+    CCM_DCVR2 = entry->dcvr2;
+    CCM_DCVR3 = entry->dcvr3;
+
+    restore_irq(oldlevel);
+}
+
+
+static inline uint32_t check_regulator_setting(uint32_t setting)
+{
+    /* Simply a safety check *in case* table gets scrambled */
+    if (setting < VOLTAGE_SETTING_MIN)
+        setting = VOLTAGE_SETTING_MIN;
+    else if (setting > VOLTAGE_SETTING_MAX)
+        setting = VOLTAGE_SETTING_MAX;
+
+    return setting;
+}
+
+
+/** DVFS **/
+static bool dvfs_running = false; /* Has driver enabled DVFS? */
+
+/* Request tracking since boot */
+unsigned int dvfs_nr_dn = 0;
+unsigned int dvfs_nr_up = 0;
+unsigned int dvfs_nr_pnc = 0;
+
+static void dvfs_stop(void);
+
+
+/* Wait for the UPDTEN flag to be set so that all bits may be written */
+static inline void wait_for_dvfs_update_en(void)
+{
+    while (!(CCM_PMCR0 & CCM_PMCR0_UPDTEN));
+}
+
+
+static void do_dvfs_update(unsigned int level)
+{
+    const struct dvfs_clock_table_entry *setting = &dvfs_clock_table[level];
+    unsigned long pmcr0 = CCM_PMCR0;
+
+    if (pmcr0 & CCM_PMCR0_DPTEN)
+    {
+        /* Ignore voltage change request from DPTC. Voltage is *not* valid. */
+        pmcr0 &= ~CCM_PMCR0_DPVCR;
+    }
+
+    pmcr0 &= ~CCM_PMCR0_VSCNT;
+
+    if (level > ((pmcr0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS))
+    { 
+        pmcr0 |= CCM_PMCR0_UDSC; /* Up scaling, increase */
+        pmcr0 |= setting->vscnt << CCM_PMCR0_VSCNT_POS;
+    }
+    else
+    {
+        pmcr0 &= ~CCM_PMCR0_UDSC; /* Down scaling, decrease */
+        pmcr0 |= 0x1 << CCM_PMCR0_VSCNT_POS;
+    }
+
+    /* DVSUP (new frequency index) setup */
+    pmcr0 = (pmcr0 & ~CCM_PMCR0_DVSUP) | (level << CCM_PMCR0_DVSUP_POS);
+
+    dvfs_level = level;
+
+    if ((setting->pll_num << CCM_PMCR0_DFSUP_MCUPLL_POS) ^
+        (pmcr0 & CCM_PMCR0_DFSUP_MCUPLL))
+    {
+        /* Update pll and post-dividers. */
+        pmcr0 ^= CCM_PMCR0_DFSUP_MCUPLL;
+        pmcr0 &= ~CCM_PMCR0_DFSUP_POST_DIVIDERS;
+    }
+    else
+    {
+        /* Post-dividers update only */
+        pmcr0 |= CCM_PMCR0_DFSUP_POST_DIVIDERS;
+    }
+
+    CCM_PMCR0 = pmcr0;
+    udelay(100); /* Software wait for voltage ramp-up */
+    CCM_PDR0 = setting->pdr_val;
+
+    if (!(pmcr0 & CCM_PMCR0_DFSUP_POST_DIVIDERS))
+    {
+        /* Update the PLL settings */
+        if (pmcr0 & CCM_PMCR0_DFSUP_MCUPLL)
+            CCM_MPCTL = setting->pll_val;
+        else
+            CCM_SPCTL = setting->pll_val;
+    }
+
+    cpu_frequency = ccm_get_mcu_clk();
+
+    if (pmcr0 & CCM_PMCR0_DPTEN)
+    {
+        update_dptc_counts(level, dptc_wp);
+        /* Enable DPTC to request voltage changes. Voltage is valid. */
+        CCM_PMCR0 |= CCM_PMCR0_DPVCR;
+        udelay(2);
+        CCM_PMCR0 |= CCM_PMCR0_DPVV;
+    }
+}
+
+
+/* Start DVFS, change the set point and stop it */
+static void set_current_dvfs_level(unsigned int level)
+{
+    int oldlevel = disable_irq_save();
+
+    CCM_PMCR0 |= CCM_PMCR0_DVFEN;
+
+    wait_for_dvfs_update_en();
+
+    do_dvfs_update(level);
+
+    wait_for_dvfs_update_en();
+
+    CCM_PMCR0 &= ~CCM_PMCR0_DVFEN;
+
+    restore_irq(oldlevel);
+}
+
+
+/* DVFS Interrupt handler */
+static void __attribute__((used)) dvfs_int(void)
+{
+    unsigned long pmcr0 = CCM_PMCR0;
+    unsigned long fsvai = pmcr0 & CCM_PMCR0_FSVAI;
+    unsigned int level = (pmcr0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS;
+
+    if (pmcr0 & CCM_PMCR0_FSVAIM)
+        return; /* Do nothing. DVFS interrupt is masked. */
+
+    if (!(pmcr0 & CCM_PMCR0_UPDTEN))
+        return; /* Do nothing. DVFS didn't finish previous flow update. */
+
+    switch (fsvai)
+    {
+    case CCM_PMCR0_FSVAI_DECREASE:
+        if (level >= DVFS_NUM_LEVELS - 1)
+            return; /* DVFS already at lowest level */
+
+        /* Upon the DECREASE event, the frequency will be changed to the next
+         * higher state index. */
+        level++;
+        dvfs_nr_dn++;
+        break;
+
+    /* Single-step frequency increase */
+    case CCM_PMCR0_FSVAI_INCREASE:
+        if (level == 0)
+            return; /* DVFS already at highest level */
+
+        /* Upon the INCREASE event, the frequency will be changed to the next
+         * lower state index. */
+        level--;
+        dvfs_nr_up++;
+        break;
+
+    /* Right to highest if panic */
+    case CCM_PMCR0_FSVAI_INCREASE_NOW:
+        if (level == 0)
+            return; /* DVFS already at highest level */
+
+        /* Upon the INCREASE_NOW event, the frequency will be increased to
+         * the maximum (index 0). */
+        level = 0;
+        dvfs_nr_pnc++;
+        break;
+
+    case CCM_PMCR0_FSVAI_NO_INT:
+    default:
+        return;     /* Do nothing. Freq change is not required */
+    } /* end switch */
+
+    do_dvfs_update(level);
+}
+
+
+/* Interrupt vector for DVFS */
+static __attribute__((naked, interrupt("IRQ"))) void CCM_DVFS_HANDLER(void)
+{
+    /* Audio can glitch with the long udelay if nested IRQ isn't allowed. */
+    AVIC_NESTED_NI_CALL_PROLOGUE();
+    asm volatile ("bl dvfs_int");
+    AVIC_NESTED_NI_CALL_EPILOGUE();
+}
+
+
+/* Initialize the DVFS hardware */
+static void dvfs_init(void)
+{
+    if (CCM_PMCR0 & CCM_PMCR0_DVFEN)
+    {
+        /* Turn it off first. Really, shouldn't happen though. */
+        dvfs_running = true;
+        dvfs_stop();
+    }
+
+    /* Combine SW1A and SW1B DVS pins for a possible five DVS levels
+     * per working point. Four, MAXIMUM, are actually used, one for each
+     * frequency. */
+    mc13783_set(MC13783_ARBITRATION_SWITCHERS, MC13783_SW1ABDVS);
+
+    /* Set DVS speed to 25mV every 4us. */
+    mc13783_write_masked(MC13783_SWITCHERS4, MC13783_SW1ADVSSPEED_4US,
+                         MC13783_SW1ADVSSPEED);
+
+    /* Set DVFS pins to functional outputs. Input mode and pad setting is
+     * fixed in hardware. */
+    iomuxc_set_pin_mux(IOMUXC_DVFS0,
+                       IOMUXC_MUX_OUT_FUNCTIONAL | IOMUXC_MUX_IN_NONE);
+    iomuxc_set_pin_mux(IOMUXC_DVFS1,
+                       IOMUXC_MUX_OUT_FUNCTIONAL | IOMUXC_MUX_IN_NONE);
+
+#ifndef DVFS_NO_PWRRDY
+    /* Configure PWRRDY signal pin. */
+    imx31_regclr32(&GPIO1_GDIR, (1 << 5));
+    iomuxc_set_pin_mux(IOMUXC_GPIO1_5,
+                       IOMUXC_MUX_OUT_FUNCTIONAL | IOMUXC_MUX_IN_FUNCTIONAL);
+#endif
+    
+    /* Initialize DVFS signal weights and detection modes. */
+    int i;
+    for (i = 0; i < 16; i++)
+    {
+        dvfs_set_lt_weight(i, lt_signals[i].weight);
+        dvfs_set_lt_detect(i, lt_signals[i].detect);
+    }
+
+    /* Set up LTR0. */
+    imx31_regmod32(&CCM_LTR0,
+                   DVFS_UPTHR << CCM_LTR0_UPTHR_POS |
+                   DVFS_DNTHR << CCM_LTR0_DNTHR_POS |
+                   DVFS_DIV3CK,
+                   CCM_LTR0_UPTHR | CCM_LTR0_DNTHR | CCM_LTR0_DIV3CK);
+
+    /* Set up LTR1. */
+    imx31_regmod32(&CCM_LTR1,
+                   DVFS_DNCNT << CCM_LTR1_DNCNT_POS |
+                   DVFS_UPCNT << CCM_LTR1_UPCNT_POS |
+                   DVFS_PNCTHR << CCM_LTR1_PNCTHR_POS |
+                   CCM_LTR1_LTBRSR,
+                   CCM_LTR1_DNCNT | CCM_LTR1_UPCNT |
+                   CCM_LTR1_PNCTHR | CCM_LTR1_LTBRSR);
+
+    /* Set up LTR2-- EMA configuration. */
+    imx31_regmod32(&CCM_LTR2, DVFS_EMAC << CCM_LTR2_EMAC_POS,
+                   CCM_LTR2_EMAC);
+
+    /* DVFS interrupt goes to MCU. Mask load buffer full interrupt. */
+    imx31_regset32(&CCM_PMCR0, CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI);
+
+    /* Initialize current core PLL and dividers for default level. Assumes
+     * clocking scheme has been set up appropriately in other init code. */
+    ccm_set_mcupll_and_pdr(dvfs_clock_table[DVFS_LEVEL_DEFAULT].pll_val,
+                           dvfs_clock_table[DVFS_LEVEL_DEFAULT].pdr_val);
+
+    /* Set initial level and working point. */
+    set_current_dvfs_level(DVFS_LEVEL_DEFAULT);
+
+    logf("DVFS: Initialized");
+}
+
+
+/* Start the DVFS hardware */
+static void dvfs_start(void)
+{
+    int oldlevel;
+
+    /* Have to wait at least 3 div3 clocks before enabling after being
+     * stopped. */
+    udelay(1500);
+
+    oldlevel = disable_irq_save();
+
+    if (!dvfs_running)
+    {
+        dvfs_running = true;
+
+        /* Unmask DVFS interrupt source and enable DVFS. */
+        avic_enable_int(INT_CCM_DVFS, INT_TYPE_IRQ, INT_PRIO_DVFS,
+                        CCM_DVFS_HANDLER);
+
+        CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_FSVAIM) | CCM_PMCR0_DVFEN;
+    }
+
+    restore_irq(oldlevel);
+
+    logf("DVFS: started");
+}
+
+
+/* Stop the DVFS hardware and return to default frequency */
+static void dvfs_stop(void)
+{
+    int oldlevel = disable_irq_save();
+
+    if (dvfs_running)
+    {
+        /* Mask DVFS interrupts. */
+        CCM_PMCR0 |= CCM_PMCR0_FSVAIM | CCM_PMCR0_LBMI;
+        avic_disable_int(INT_CCM_DVFS);
+
+        if (((CCM_PMCR0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS) !=
+                DVFS_LEVEL_DEFAULT)
+        {
+            /* Set default frequency level */
+            wait_for_dvfs_update_en();
+            do_dvfs_update(DVFS_LEVEL_DEFAULT);
+            wait_for_dvfs_update_en();
+        }
+
+        /* Disable DVFS. */
+        CCM_PMCR0 &= ~CCM_PMCR0_DVFEN;
+        dvfs_running = false;
+    }
+
+    restore_irq(oldlevel);
+
+    logf("DVFS: stopped");
+}
+
+
+/** DPTC **/
+
+/* Request tracking since boot */
+unsigned int dptc_nr_dn = 0;
+unsigned int dptc_nr_up = 0;
+unsigned int dptc_nr_pnc = 0;
+
+
+/* Enable DPTC and unmask interrupt. */
+static void enable_dptc(void)
+{
+    int oldlevel = disable_irq_save();
+
+    /* Enable DPTC, assert voltage change request. */
+    CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_PTVAIM) | CCM_PMCR0_DPTEN |
+                    CCM_PMCR0_DPVCR;
+
+    udelay(2);
+
+    /* Set voltage valid *after* setting change request */
+    CCM_PMCR0 |= CCM_PMCR0_DPVV;
+
+    restore_irq(oldlevel);
+}
+
+
+static void dptc_new_wp(unsigned int wp)
+{
+    unsigned int level = dvfs_level;
+    const union dvfs_dptc_voltage_table_entry *entry = &dvfs_dptc_voltage_table[wp];
+
+    uint32_t sw1a = check_regulator_setting(entry->sw1a);
+    uint32_t sw1advs = check_regulator_setting(entry->sw1advs);
+    uint32_t sw1bdvs = check_regulator_setting(entry->sw1bdvs);
+    uint32_t sw1bstby = check_regulator_setting(entry->sw1bstby);
+
+    dptc_wp = wp;
 
     mc13783_write_masked(MC13783_SWITCHERS0,
-                         MC13783_SW_1_350 << MC13783_SW1A_POS,
-                         MC13783_SW1A);
-    imx31_regmod32(&CCM_PMCR0, CCM_PMCR0_DVS1_0_DVS0_0,
-                   CCM_PMCR0_DVSUP_DVS);           
+                         sw1a << MC13783_SW1A_POS |         /* SW1A */
+                         sw1advs << MC13783_SW1ADVS_POS,    /* SW1ADVS */
+                         MC13783_SW1A | MC13783_SW1ADVS);
+
+    mc13783_write_masked(MC13783_SWITCHERS1,
+                         sw1bdvs << MC13783_SW1BDVS_POS |   /* SW1BDVS */
+                         sw1bstby << MC13783_SW1BSTBY_POS,  /* SW1BSTBY */
+                         MC13783_SW1BDVS | MC13783_SW1BSTBY);
+
+
+    udelay(100); /* Wait to settle */
+
+    update_dptc_counts(level, wp);
 }
 
 
+/* DPTC service thread */
+#ifdef ROCKBOX_HAS_LOGF
+#define DPTC_STACK_SIZE DEFAULT_STACK_SIZE
+#else
+#define DPTC_STACK_SIZE 160
+#endif
+static int dptc_thread_stack[DPTC_STACK_SIZE/sizeof(int)];
+static const char * const dptc_thread_name = "dptc";
+static struct wakeup dptc_wakeup;   /* Object to signal upon DPTC event */
+static struct mutex dptc_mutex;     /* Avoid mutually disrupting voltage updates */
+static unsigned long dptc_int_data; /* Data passed to thread for each event */
+static bool dptc_running = false;   /* Has driver enabled DPTC? */
+
+
+static void dptc_interrupt_thread(void)
+{
+    int wp;
+
+    mutex_lock(&dptc_mutex);
+
+    while (1)
+    {
+        mutex_unlock(&dptc_mutex);
+
+        wakeup_wait(&dptc_wakeup, TIMEOUT_BLOCK);
+
+        mutex_lock(&dptc_mutex);
+
+        if (!dptc_running)
+            continue;
+
+        wp = dptc_wp;
+
+        switch (dptc_int_data & CCM_PMCR0_PTVAI)
+        {
+        case CCM_PMCR0_PTVAI_DECREASE:
+            wp++;
+            dptc_nr_dn++;
+            break;
+
+        case CCM_PMCR0_PTVAI_INCREASE:
+            wp--;
+            dptc_nr_up++;
+            break;
+
+        case CCM_PMCR0_PTVAI_INCREASE_NOW:
+            wp = DPTC_WP_PANIC;
+            dptc_nr_pnc++;
+            break;
+
+        case CCM_PMCR0_PTVAI_NO_INT:
+            logf("DPTC: unexpected INT");
+            continue;
+        }
+
+        if (wp < 0)
+        {
+            wp = 0;
+            logf("DPTC: already @ highest (%d)", wp);
+        }
+        else if (wp >= DPTC_NUM_WP)
+        {
+            wp = DPTC_NUM_WP - 1;
+            logf("DPTC: already @ lowest (%d)", wp);
+        }
+        else
+        {
+            logf("DPTC: new wp (%d)", wp);
+        }
+
+        dptc_new_wp(wp);
+        enable_dptc();
+    }
+}
+
+
+/* Interrupt vector for DPTC */
+static __attribute__((interrupt("IRQ"))) void CCM_CLK_HANDLER(void)
+{
+    /* Snapshot the interrupt cause */
+    unsigned long pmcr0 = CCM_PMCR0;
+    dptc_int_data = pmcr0;
+
+    /* Mask DPTC interrupt and disable DPTC until the change request is
+     * serviced. */
+    CCM_PMCR0 = (pmcr0 & ~CCM_PMCR0_DPTEN) | CCM_PMCR0_PTVAIM;
+
+    wakeup_signal(&dptc_wakeup);
+}
+
+
+/* Initialize the DPTC hardware */
+static void dptc_init(void)
+{
+    /* Force DPTC off if running for some reason. */
+    imx31_regmod32(&CCM_PMCR0, CCM_PMCR0_PTVAIM,
+                   CCM_PMCR0_PTVAIM | CCM_PMCR0_DPTEN);
+
+    /* Set default, safe working point. */
+    dptc_new_wp(DPTC_WP_DEFAULT);
+
+    /* Interrupt goes to MCU, specified reference circuits enabled when
+     * DPTC is active. */
+    imx31_regset32(&CCM_PMCR0, CCM_PMCR0_PTVIS | DPTC_DRCE_MASK);
+
+    /* DPTC counting range = 256 system clocks */
+    imx31_regclr32(&CCM_PMCR0, CCM_PMCR0_DCR);
+
+    /* Create PMIC regulator service. */
+    wakeup_init(&dptc_wakeup);
+    mutex_init(&dptc_mutex);
+    create_thread(dptc_interrupt_thread,
+            dptc_thread_stack, sizeof(dptc_thread_stack), 0,
+            dptc_thread_name IF_PRIO(, PRIORITY_REALTIME_1) IF_COP(, CPU));
+
+    logf("DPTC: Initialized");
+}
+
+
+/* Start DPTC module */
+static void dptc_start(void)
+{
+    int oldstate;
+
+    mutex_lock(&dptc_mutex);
+
+    oldstate = disable_irq_save();
+
+    if (!dptc_running)
+    {
+        dptc_running = true;
+
+        /* Enable DPTC and unmask interrupt. */
+        avic_enable_int(INT_CCM_CLK, INT_TYPE_IRQ, INT_PRIO_DPTC, 
+                        CCM_CLK_HANDLER);
+
+        update_dptc_counts(dvfs_level, dptc_wp);
+        enable_dptc();
+    }
+
+    restore_irq(oldstate);
+
+    mutex_unlock(&dptc_mutex);
+
+    logf("DPTC: started");
+}
+
+
+/* Stop the DPTC hardware if running and go back to default working point */
+static void dptc_stop(void)
+{
+    int oldlevel;
+
+    mutex_lock(&dptc_mutex);
+
+    oldlevel = disable_irq_save();
+
+    if (dptc_running)
+    {
+        /* Disable DPTC and mask interrupt. */
+        CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_DPTEN) | CCM_PMCR0_PTVAIM;
+        avic_disable_int(INT_CCM_CLK);
+        dptc_int_data = 0;
+
+        dptc_running = false;
+    }
+
+    restore_irq(oldlevel);
+
+    /* Go back to default working point. */
+    dptc_new_wp(DPTC_WP_DEFAULT);
+
+    mutex_unlock(&dptc_mutex);
+
+    logf("DPTC: stopped");
+}
+
+
+/** Main module interface **/
+
+/* Initialize DVFS and DPTC */
+void dvfs_dptc_init(void)
+{
+    dptc_init();
+    dvfs_init();
+}
+
+
+/* Start DVFS and DPTC */
+void dvfs_dptc_start(void)
+{
+    dvfs_start();
+    if (0) /* Hold off for now */
+    {
+        dptc_start();
+    }
+}
+
+
+/* Stop DVFS and DPTC */
 void dvfs_dptc_stop(void)
 {
-    /* Nothing for now */
+    dptc_stop();
+    dvfs_stop();
 }
 
+
+/* Set a signal load tracking weight */
+void dvfs_set_lt_weight(enum DVFS_LT_SIGS index, unsigned long value)
+{
+    volatile unsigned long *reg_p = &CCM_LTR2;
+    unsigned int shift = 3 * index;
+
+    if (index < 9)
+    {
+        reg_p = &CCM_LTR3;
+        shift += 5; /* Bits 7:5, 10:8 ... 31:29 */
+    }
+    else if (index < 16)
+    {
+        shift -= 16; /* Bits 13:11, 16:14 ... 31:29 */
+    }
+
+    imx31_regmod32(reg_p, value << shift, 0x7 << shift);
+}
+
+
+/* Set a signal load detection mode */
+void dvfs_set_lt_detect(enum DVFS_LT_SIGS index, bool edge)
+{
+    unsigned long bit = 0;
+
+    if ((unsigned)index < 13)
+        bit = 1ul << (index + 3);
+    else if ((unsigned)index < 16)
+        bit = 1ul << (index + 29);
+
+    imx31_regmod32(&CCM_LTR0, edge ? bit : 0, bit);
+}
+
+
+void dvfs_set_gp_bit(enum DVFS_DVGPS dvgp, bool assert)
+{
+    if ((unsigned)dvgp <= 3)
+    {
+        unsigned long bit = 1ul << dvgp;
+        imx31_regmod32(&CCM_PMCR1, assert ? bit : 0, bit);
+    }
+}
+
+
+/* Turn the wait-for-interrupt monitoring on or off */
+void dvfs_wfi_monitor(bool on)
+{
+    imx31_regmod32(&CCM_PMCR0, on ? 0 : CCM_PMCR0_WFIM,
+                   CCM_PMCR0_WFIM);
+}
+
+
+/* Obtain the current core voltage setting, in millivolts 8-) */
+unsigned int dvfs_dptc_get_voltage(void)
+{
+    unsigned int v;
+
+    int oldlevel = disable_irq_save();
+    v = dvfs_dptc_voltage_table[dptc_wp].sw[dvfs_level];
+    restore_irq(oldlevel);
+
+    /* 25mV steps from 0.900V to 1.675V */
+    return v * 25 + 900;
+}
+
+
+/* Get the current DVFS level */
+unsigned int dvfs_get_level(void)
+{
+    return dvfs_level;
+}
+
+
+/* If DVFS is disabled, set the level explicitly */
+void dvfs_set_level(unsigned int level)
+{
+    int oldlevel = disable_irq_save();
+
+    unsigned int currlevel =
+        (CCM_PMCR0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS;
+
+    if (!dvfs_running && level < DVFS_NUM_LEVELS && level != currlevel)
+        set_current_dvfs_level(level);
+
+    restore_irq(oldlevel);
+}
+
+
+/* Get the current DPTC working point */
+unsigned int dptc_get_wp(void)
+{
+    return dptc_wp;
+}
+
+
+/* If DPTC is not running, set the working point explicitly */
+void dptc_set_wp(unsigned int wp)
+{
+    mutex_lock(&dptc_mutex);
+
+    if (!dptc_running && wp < DPTC_NUM_WP)
+    {
+        dptc_new_wp(wp);
+    }
+
+    mutex_unlock(&dptc_mutex);
+}
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.h b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
index 8f6f5da98d..2bf6114a11 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.h
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
@@ -24,7 +24,106 @@
 #ifndef _DVFS_DPTC_IMX31_H_
 #define _DVFS_DPTC_IMX31_H_
 
+/* DVFS load tracking signals */
+enum DVFS_LT_SIGS
+{
+    DVFS_LT_SIG_M3IF_M0_BUF    = 0,  /* Hready signal of M3IF's master #0
+                                        (L2 Cache) */
+    DVFS_LT_SIG_M3IF_M1        = 1,  /* Hready signal of M3IF's master #1
+                                        (L2 Cache) */
+    DVFS_LT_SIG_MBX_MBXCLKGATE = 2,  /* Hready signal of M3IF's master #2
+                                        (MBX) */
+    DVFS_LT_SIG_M3IF_M3        = 3,  /* Hready signal of M3IF's master #3
+                                        (MAX) */
+    DVFS_LT_SIG_M3IF_M4        = 4,  /* Hready signal of M3IF's master #4
+                                        (SDMA) */
+    DVFS_LT_SIG_M3IF_M5        = 5,  /* Hready signal of M3IF's master #5
+                                        (mpeg4_vga_encoder) */
+    DVFS_LT_SIG_M3IF_M6        = 6,  /* Hready signal of M3IF's master #6
+                                        (IPU) */
+    DVFS_LT_SIG_M3IF_M7        = 7,  /* Hready signal of M3IF's master #7
+                                        (IPU) */
+    DVFS_LT_SIG_ARM11_P_IRQ_B_RBT_GATE = 8, /* ARM normal interrupt */
+    DVFS_LT_SIG_ARM11_P_FIQ_B_RBT_GATE = 9, /* ARM fast interrupt */
+    DVFS_LT_SIG_IPI_GPIO1_INT0 = 10, /* Interrupt line from GPIO */
+    DVFS_LT_SIG_IPI_INT_IPU_FUNC = 11, /* Interrupt line from IPU */
+    DVFS_LT_SIG_DVGP0          = 12, /* Software-controllable general-purpose
+                                        bits from the CCM */
+    DVFS_LT_SIG_DVGP1          = 13, /* Software-controllable general-purpose
+                                        bits from the CCM */
+    DVFS_LT_SIG_DVGP2          = 14, /* Software-controllable general-purpose
+                                        bits from the CCM */
+    DVFS_LT_SIG_DVGP3          = 15, /* Software-controllable general-purpose
+                                        bits from the CCM */
+};
+
+
+enum DVFS_DVGPS
+{
+    DVFS_DVGP_0 = 0,
+    DVFS_DVGP_1,
+    DVFS_DVGP_2,
+    DVFS_DVGP_3,
+};
+
+union dvfs_dptc_voltage_table_entry
+{
+    uint8_t sw[4];        /* Access as array            */
+
+    struct
+    {
+                          /* Chosen by PMIC pin states  */
+                          /* when SWxABDVS bit is 1:    */
+                          /* DVSSWxA  DVSSWxB           */
+        uint8_t sw1a;     /*    0        0              */
+        uint8_t sw1advs;  /*    1        0              */
+        uint8_t sw1bdvs;  /*    0        1              */
+        uint8_t sw1bstby; /*    1        1              */
+    };
+};
+
+
+struct dptc_dcvr_table_entry
+{
+    uint32_t dcvr0; /* DCVR register values for working point */
+    uint32_t dcvr1;
+    uint32_t dcvr2;
+    uint32_t dcvr3;
+};
+
+
+struct dvfs_clock_table_entry
+{
+    uint32_t pll_val;     /* Setting for target PLL */
+    uint32_t pdr_val;     /* Post-divider for target setting */
+    uint32_t pll_num : 1; /* 1 = MCU PLL, 0 = Serial PLL */
+    uint32_t vscnt   : 3; /* Voltage scaling counter, CKIL delay */
+};
+
+
+struct dvfs_lt_signal_descriptor
+{
+    uint8_t weight   : 3; /* Signal weight = 0-7 */
+    uint8_t detect   : 1; /* 1 = edge-detected */
+};
+
+
+extern long cpu_voltage_setting;
+
+void dvfs_dptc_init(void);
 void dvfs_dptc_start(void);
 void dvfs_dptc_stop(void);
 
+void dvfs_wfi_monitor(bool on);
+void dvfs_set_lt_weight(enum DVFS_LT_SIGS index, unsigned long value);
+void dvfs_set_lt_detect(enum DVFS_LT_SIGS index, bool edge);
+void dvfs_set_gp_bit(enum DVFS_DVGPS dvgp, bool assert);
+
+unsigned int dvfs_dptc_get_voltage(void);
+unsigned int dvfs_get_level(void);
+void dvfs_set_level(unsigned int level);
+
+unsigned int dptc_get_wp(void);
+void dptc_set_wp(unsigned int wp);
+
 #endif /* _DVFS_DPTC_IMX31_H_ */
diff --git a/firmware/target/arm/imx31/gigabeat-s/dvfs_dptc_tables-target.h b/firmware/target/arm/imx31/gigabeat-s/dvfs_dptc_tables-target.h
new file mode 100644
index 0000000000..2356e23252
--- /dev/null
+++ b/firmware/target/arm/imx31/gigabeat-s/dvfs_dptc_tables-target.h
@@ -0,0 +1,279 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2010 by Michael Sevakis
+ *
+ * Target-specific i.MX31 DVFS and DPTC driver declarations
+ *
+ * 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 software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef _DVFS_DPTC_TARGET_H_
+#define _DVFS_DPTC_TARGET_H_
+
+#define DVFS_LEVEL_DEFAULT   1 /* 264 MHz - safe frequency for 1.35V */
+#define DVFS_NUM_LEVELS      3 /* 528 MHz, 264 MHz, 132 MHz */
+#define DVFS_NO_PWRRDY         /* PWRRDY is connected to different SoC port */
+
+#define DPTC_WP_DEFAULT      1 /* 1.600, 1.350, 1.350 */
+#define DPTC_WP_PANIC        3 /* Up to minimum for > 400 MHz */
+#define DPTC_NUM_WP         17
+
+#define VOLTAGE_SETTING_MIN     MC13783_SW_1_350
+#define VOLTAGE_SETTING_MAX     MC13783_SW_1_625
+
+/* Frequency increase threshold. Increase frequency change request
+ * will be sent if DVFS counter value will be more than this value. */
+#define DVFS_UPTHR      30
+
+/* Frequency decrease threshold. Decrease frequency change request
+ * will be sent if DVFS counter value will be less than this value. */
+#define DVFS_DNTHR      18
+
+/* Panic threshold. Panic frequency change request
+ * will be sent if DVFS counter value will be more than this value. */
+#define DVFS_PNCTHR     63
+
+/* With the ARM clocked at 532, this setting yields a DIV_3_CLK of 2.03 kHz.
+ *
+ * Note: To get said clock, the divider would have to be 262144. The values
+ * and their meanings are not published in the reference manual for i.MX31
+ * but show up in the i.MX35 reference manual. Either that chip is different
+ * and the values have an additional division or the comments in the BSP are
+ * incorrect.
+ */
+#define DVFS_DIV3CK     CCM_LTR0_DIV3CK_131072
+
+/* UPCNT defines the amount of times the up threshold should be exceeded
+ * before DVFS will trigger frequency increase request. */
+
+#if 0
+/* Freescale BSP value: a bit too agressive IMHO */
+#define DVFS_UPCNT       0x33
+#endif
+#define DVFS_UPCNT       0x48
+
+/* DNCNT defines the amount of times the down threshold should be undershot
+ * before DVFS will trigger frequency decrease request. */
+#define DVFS_DNCNT       0x33
+
+/* EMAC defines how many samples are included in EMA calculation */
+#define DVFS_EMAC        0x20
+
+/* Define mask of which reference circuits are employed for DPTC */
+#define DPTC_DRCE_MASK (CCM_PMCR0_DRCE1 | CCM_PMCR0_DRCE3)
+
+/* When panicing, this working point is used */
+#define DPTC_PANIC_WP
+
+/* Due to a hardware bug in chip revisions < 2.0, when switching between
+ * Serial and MCU PLLs, DVFS forces the target PLL to go into reset and
+ * relock, only post divider frequency scaling is possible.
+ */
+
+static const union dvfs_dptc_voltage_table_entry
+dvfs_dptc_voltage_table[DPTC_NUM_WP] =
+{
+    /* For each working point, there are four DVFS settings, chosen by the
+     * DVS pin states on the PMIC set by the DVFS routines. Pins are reversed
+     * and actual order as used by PMIC for DVSUP values of 00, 01, 10 and 11
+     * is below.
+     *
+     *         SW1A            SW1ADVS           SW1BDVS           SW1BSTBY
+     *  0                 2                 1                 3                */
+    { { MC13783_SW_1_625, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_600, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_575, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_550, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_525, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_500, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_475, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_450, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_425, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_400, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_375, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_325, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_300, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_275, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_250, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+    { { MC13783_SW_1_225, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
+};
+
+#if 1
+/* For 27 MHz PLL reference clock */
+static const struct dptc_dcvr_table_entry
+dptc_dcvr_table[DVFS_NUM_LEVELS][DPTC_NUM_WP] =
+{
+        /*  DCVR0       DCVR1       DCVR2       DCVR3    */
+    {   /* 528 MHz */
+        { 0xffc00000, 0x90400000, 0xffc00000, 0xdd000000 },
+        { 0xffc00000, 0x90629890, 0xffc00000, 0xdd34ed20 },
+        { 0xffc00000, 0x90629890, 0xffc00000, 0xdd34ed20 },
+        { 0xffc00000, 0x90629894, 0xffc00000, 0xdd74fd24 },
+        { 0xffc00000, 0x90a2a894, 0xffc00000, 0xddb50d28 },
+        { 0xffc00000, 0x90e2b89c, 0xffc00000, 0xde352d30 },
+        { 0xffc00000, 0x9162d8a0, 0xffc00000, 0xdef55d38 },
+        { 0xffc00000, 0x91e2f8a8, 0xffc00000, 0xdfb58d44 },
+        { 0xffc00000, 0x926308b0, 0xffc00000, 0xe0b5cd54 },
+        { 0xffc00000, 0x92e328bc, 0xffc00000, 0xe1f60d64 },
+        { 0xffc00000, 0x93a358c0, 0xffc00000, 0xe3365d74 },
+        { 0xffc00000, 0xf66388cc, 0xffc00000, 0xf6768d84 },
+        { 0xffc00000, 0xf663b8d4, 0xffc00000, 0xf676dd98 },
+        { 0xffc00000, 0xf663e8e0, 0xffc00000, 0xf6773da4 },
+        { 0xffc00000, 0xf66418ec, 0xffc00000, 0xf6778dbc },
+        { 0xffc00000, 0xf66458fc, 0xffc00000, 0xf677edd0 },
+        { 0xffc00000, 0xf6648908, 0xffc00000, 0xf6783de8 },
+
+    },
+    {   /* 264 MHz */
+        { 0xffc00000, 0x90400000, 0xffc00000, 0xdd000000 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd0d4348 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd0d4348 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd4d4348 },
+        { 0xffc00000, 0x9088b228, 0xffc00000, 0xdd8d434c },
+        { 0xffc00000, 0x90c8b228, 0xffc00000, 0xde0d534c },
+        { 0xffc00000, 0x9148b228, 0xffc00000, 0xdecd5350 },
+        { 0xffc00000, 0x91c8c22c, 0xffc00000, 0xdf8d6354 },
+        { 0xffc00000, 0x9248d22c, 0xffc00000, 0xe08d7354 },
+        { 0xffc00000, 0x92c8d230, 0xffc00000, 0xe1cd8358 },
+        { 0xffc00000, 0x9388e234, 0xffc00000, 0xe30d935c },
+        { 0xffc00000, 0xf648e234, 0xffc00000, 0xf64db364 },
+        { 0xffc00000, 0xf648f238, 0xffc00000, 0xf64dc368 },
+        { 0xffc00000, 0xf648f23c, 0xffc00000, 0xf64dd36c },
+        { 0xffc00000, 0xf649023c, 0xffc00000, 0xf64de370 },
+        { 0xffc00000, 0xf649123c, 0xffc00000, 0xf64df374 },
+        { 0xffc00000, 0xf6492240, 0xffc00000, 0xf64e1378 },
+
+    },
+    {   /* 132 MHz */
+        { 0xffc00000, 0x90400000, 0xffc00000, 0xdd000000 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd0d4348 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd0d4348 },
+        { 0xffc00000, 0x9048a224, 0xffc00000, 0xdd4d4348 },
+        { 0xffc00000, 0x9088b228, 0xffc00000, 0xdd8d434c },
+        { 0xffc00000, 0x90c8b228, 0xffc00000, 0xde0d534c },
+        { 0xffc00000, 0x9148b228, 0xffc00000, 0xdecd5350 },
+        { 0xffc00000, 0x91c8c22c, 0xffc00000, 0xdf8d6354 },
+        { 0xffc00000, 0x9248d22c, 0xffc00000, 0xe08d7354 },
+        { 0xffc00000, 0x92c8d230, 0xffc00000, 0xe1cd8358 },
+        { 0xffc00000, 0x9388e234, 0xffc00000, 0xe30d935c },
+        { 0xffc00000, 0xf648e234, 0xffc00000, 0xf64db364 },
+        { 0xffc00000, 0xf648f238, 0xffc00000, 0xf64dc368 },
+        { 0xffc00000, 0xf648f23c, 0xffc00000, 0xf64dd36c },
+        { 0xffc00000, 0xf649023c, 0xffc00000, 0xf64de370 },
+        { 0xffc00000, 0xf649123c, 0xffc00000, 0xf64df374 },
+        { 0xffc00000, 0xf6492240, 0xffc00000, 0xf64e1378 },
+    },
+};
+#else/* For 26 MHz PLL reference clock */
+static const struct dptc_dcvr_table_entry
+dptc_dcvr_table[DVFS_NUM_LEVELS][DPTC_NUM_WP] =
+{
+        /*  DCVR0       DCVR1       DCVR2       DCVR3    */
+    {   /* 528 MHz */
+        { 0xffc00000, 0x95c00000, 0xffc00000, 0xe5800000 },
+        { 0xffc00000, 0x95e3e8e4, 0xffc00000, 0xe5b6fda0 },
+        { 0xffc00000, 0x95e3e8e4, 0xffc00000, 0xe5b6fda0 },
+        { 0xffc00000, 0x95e3e8e8, 0xffc00000, 0xe5f70da4 },
+        { 0xffc00000, 0x9623f8e8, 0xffc00000, 0xe6371da8 },
+        { 0xffc00000, 0x966408f0, 0xffc00000, 0xe6b73db0 },
+        { 0xffc00000, 0x96e428f4, 0xffc00000, 0xe7776dbc },
+        { 0xffc00000, 0x976448fc, 0xffc00000, 0xe8379dc8 },
+        { 0xffc00000, 0x97e46904, 0xffc00000, 0xe977ddd8 },
+        { 0xffc00000, 0x98a48910, 0xffc00000, 0xeab81de8 },
+        { 0xffc00000, 0x9964b918, 0xffc00000, 0xebf86df8 },
+        { 0xffc00000, 0xffe4e924, 0xffc00000, 0xfff8ae08 },
+        { 0xffc00000, 0xffe5192c, 0xffc00000, 0xfff8fe1c },
+        { 0xffc00000, 0xffe54938, 0xffc00000, 0xfff95e2c },
+        { 0xffc00000, 0xffe57944, 0xffc00000, 0xfff9ae44 },
+        { 0xffc00000, 0xffe5b954, 0xffc00000, 0xfffa0e58 },
+        { 0xffc00000, 0xffe5e960, 0xffc00000, 0xfffa6e70 },
+    },
+    {   /* 264 MHz */
+        { 0xffc00000, 0x95c00000, 0xffc00000, 0xe5800000 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe58dc368 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe58dc368 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe5cdc368 },
+        { 0xffc00000, 0x9609023c, 0xffc00000, 0xe60dc36c },
+        { 0xffc00000, 0x9649023c, 0xffc00000, 0xe68dd36c },
+        { 0xffc00000, 0x96c9023c, 0xffc00000, 0xe74dd370 },
+        { 0xffc00000, 0x97491240, 0xffc00000, 0xe80de374 },
+        { 0xffc00000, 0x97c92240, 0xffc00000, 0xe94df374 },
+        { 0xffc00000, 0x98892244, 0xffc00000, 0xea8e0378 },
+        { 0xffc00000, 0x99493248, 0xffc00000, 0xebce137c },
+        { 0xffc00000, 0xffc93248, 0xffc00000, 0xffce3384 },
+        { 0xffc00000, 0xffc9424c, 0xffc00000, 0xffce4388 },
+        { 0xffc00000, 0xffc95250, 0xffc00000, 0xffce538c },
+        { 0xffc00000, 0xffc96250, 0xffc00000, 0xffce7390 },
+        { 0xffc00000, 0xffc97254, 0xffc00000, 0xffce8394 },
+        { 0xffc00000, 0xffc98258, 0xffc00000, 0xffcea39c },
+    },
+    {   /* 132 MHz */
+        { 0xffc00000, 0x95c00000, 0xffc00000, 0xe5800000 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe58dc368 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe58dc368 },
+        { 0xffc00000, 0x95c8f238, 0xffc00000, 0xe5cdc368 },
+        { 0xffc00000, 0x9609023c, 0xffc00000, 0xe60dc36c },
+        { 0xffc00000, 0x9649023c, 0xffc00000, 0xe68dd36c },
+        { 0xffc00000, 0x96c9023c, 0xffc00000, 0xe74dd370 },
+        { 0xffc00000, 0x97491240, 0xffc00000, 0xe80de374 },
+        { 0xffc00000, 0x97c92240, 0xffc00000, 0xe94df374 },
+        { 0xffc00000, 0x98892244, 0xffc00000, 0xea8e0378 },
+        { 0xffc00000, 0x99493248, 0xffc00000, 0xebce137c },
+        { 0xffc00000, 0xffc93248, 0xffc00000, 0xffce3384 },
+        { 0xffc00000, 0xffc9424c, 0xffc00000, 0xffce4388 },
+        { 0xffc00000, 0xffc95250, 0xffc00000, 0xffce538c },
+        { 0xffc00000, 0xffc96250, 0xffc00000, 0xffce7390 },
+        { 0xffc00000, 0xffc97254, 0xffc00000, 0xffce8394 },
+        { 0xffc00000, 0xffc98258, 0xffc00000, 0xffcea39c },
+    },
+};
+#endif
+
+
+/* For 27 MHz PLL reference clock */
+static const struct dvfs_clock_table_entry
+dvfs_clock_table[DVFS_NUM_LEVELS] =
+{
+    /*  PLL val    PDR0 val  PLL VSCNT */
+    { 0x00082407, 0xff841e58, 1,   7 }, /* MCUPLL, 528 MHz, /1 = 528 MHz */
+    { 0x00082407, 0xff841e59, 1,   7 }, /* MCUPLL, 528 MHz, /2 = 264 MHz */
+    { 0x00082407, 0xff841e5b, 1,   7 }, /* MCUPLL, 528 MHz, /4 = 132 MHz */
+};
+
+
+/* DVFS load-tracking signal weights and detect modes */
+static const struct dvfs_lt_signal_descriptor lt_signals[16] =
+{
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M0_BUF */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M1 */
+    { 0, 0 }, /* DVFS_LT_SIG_MBX_MBXCLKGATE */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M3 */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M4 */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M5 */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M6 */
+    { 0, 0 }, /* DVFS_LT_SIG_M3IF_M7 */
+    { 0, 0 }, /* DVFS_LT_SIG_ARM11_P_IRQ_B_RBT_GATE */
+    { 0, 0 }, /* DVFS_LT_SIG_ARM11_P_FIQ_B_RBT_GATE */
+    { 0, 0 }, /* DVFS_LT_SIG_IPI_GPIO1_INT0 */
+    { 0, 0 }, /* DVFS_LT_SIG_IPI_INT_IPU_FUNC */
+    { 7, 0 }, /* DVFS_LT_SIG_DVGP0 */
+    { 7, 0 }, /* DVFS_LT_SIG_DVGP1 */
+    { 7, 0 }, /* DVFS_LT_SIG_DVGP2 */
+    { 7, 0 }, /* DVFS_LT_SIG_DVGP3 */
+};
+
+#endif /* _DVFS_DPTC_TARGET_H_ */
diff --git a/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c
index 8e81447bd3..894aea4fd3 100644
--- a/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c
@@ -70,10 +70,11 @@ void kernel_device_init(void)
     sdma_init();
     spi_init();
     mc13783_init();
-    dvfs_dptc_start();
+    dvfs_dptc_init();
+    dvfs_wfi_monitor(true); /* Monitor the WFI signal */
+    dvfs_dptc_start(); /* Should be ok to start even so early */
 }
 
-#ifdef BOOTLOADER
 void tick_stop(void)
 {
     avic_disable_int(INT_EPIT1);            /* Disable insterrupt */
@@ -81,4 +82,4 @@ void tick_stop(void)
     EPITSR1 = EPITSR_OCIF;                  /* Clear pending */
     ccm_module_clock_gating(CG_EPIT1, CGM_OFF); /* Turn off module clock */
 }
-#endif
+
diff --git a/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
index 7e3b39dba8..d7fe87f168 100644
--- a/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
@@ -28,6 +28,7 @@
 #include "backlight-target.h"
 #include "avic-imx31.h"
 #include "mc13783.h"
+#include "dvfs_dptc-imx31.h"
 #if CONFIG_TUNER
 #include "fmradio_i2c.h"
 #endif
@@ -121,6 +122,9 @@ bool tuner_powered(void)
 
 void power_off(void)
 {
+    /* Turn off voltage and frequency scaling */
+    dvfs_dptc_stop();
+
     /* Cut backlight */
     _backlight_off();
 
@@ -131,9 +135,7 @@ void power_off(void)
     mc13783_set(MC13783_POWER_CONTROL0, MC13783_USEROFFSPI);
 
     /* Wait for power cut */
-    disable_interrupt(IRQ_FIQ_STATUS);
-
-    while (1);
+    system_halt();
 }
 
 void power_init(void)
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
index cd684e77ac..7c0d30c783 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
@@ -26,6 +26,7 @@
 #include "gpio-imx31.h"
 #include "mmu-imx31.h"
 #include "system-target.h"
+#include "powermgmt-target.h"
 #include "lcd.h"
 #include "serial-imx31.h"
 #include "debug.h"
@@ -115,18 +116,24 @@ int system_memory_guard(int newmode)
     return 0;
 }
 
+void system_halt(void)
+{
+    disable_interrupt(IRQ_FIQ_STATUS);
+    avic_set_ni_level(AVIC_NIL_DISABLE);
+    while (1)
+        core_idle();
+}
+
 void system_reboot(void)
 {
     /* Multi-context so no SPI available (WDT?)  */
-    while (1);
+    system_halt();
 }
 
 void system_exception_wait(void)
 {
     /* Called in many contexts so button reading may be a chore */
-    avic_disable_int(INT_ALL);
-    core_idle();
-    while (1);
+    system_halt();
 }
 
 void system_init(void)
@@ -175,6 +182,9 @@ void system_init(void)
 
     unsigned int i;
 
+    /* Initialize frequency with current */
+    cpu_frequency = ccm_get_mcu_clk();
+
     /* MCR WFI enables wait mode (CCM_CCMR_LPM_WAIT_MODE = 0) */
     imx31_regclr32(&CCM_CCMR, CCM_CCMR_LPM);
 
@@ -239,16 +249,33 @@ void __attribute__((naked)) imx31_regclr32(volatile uint32_t *reg_p,
     (void)reg_p; (void)mask;
 }
 
-#ifdef BOOTLOADER
+
 void system_prepare_fw_start(void)
 {
     dvfs_dptc_stop();
-    disable_interrupt(IRQ_FIQ_STATUS);
-    avic_disable_int(INT_ALL);
     mc13783_close();
     tick_stop();
+    disable_interrupt(IRQ_FIQ_STATUS);
+    avic_set_ni_level(AVIC_NIL_DISABLE);
 }
-#endif
+
+
+#ifndef BOOTLOADER
+void rolo_restart_firmware(const unsigned char *source, unsigned char *dest,
+                           int length) __attribute__((noreturn));
+
+void __attribute__((noreturn))
+rolo_restart(const unsigned char *source, unsigned char *dest, int length)
+{
+    /* Some housekeeping tasks must be performed for a safe changeover */
+    charging_algorithm_close();
+    system_prepare_fw_start();
+
+    /* Copying routine where new image is run */
+    rolo_restart_firmware(source, dest, length);
+}
+#endif /* BOOTLOADER */
+
 
 inline void dumpregs(void) 
 {
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index b859093c58..af95471db6 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -24,12 +24,12 @@
 #include "system-arm.h"
 #include "mmu-arm.h"
 
-#ifndef HAVE_ADJUSTABLE_CPU_FREQ
-/* TODO: implement CPU frequency scaling */
-#define CPUFREQ_DEFAULT CPU_FREQ
-#define CPUFREQ_NORMAL CPU_FREQ
-#define CPUFREQ_MAX CPU_FREQ
-#endif
+/* High enough for most tasks but low enough for reduced voltage */
+#define CPUFREQ_DEFAULT     264000000
+/* Still quite powerful, minimum possible frequency */
+#define CPUFREQ_NORMAL      132000000
+/* Overdrive mode */
+#define CPUFREQ_MAX         528000000
 
 static inline void udelay(unsigned int usecs)
 {
@@ -45,10 +45,11 @@ void gpt_stop(void);
 
 unsigned int iim_system_rev(void);
 
-/* Prepare for transition to firmware */
+/* Prepare for transition to (new) firmware */
 void system_prepare_fw_start(void);
 void tick_stop(void);
 void kernel_device_init(void);
+void system_halt(void);
 
 void imx31_regmod32(volatile uint32_t *reg_p, uint32_t value,
                     uint32_t mask);
diff --git a/firmware/target/arm/imx31/mc13783-imx31.c b/firmware/target/arm/imx31/mc13783-imx31.c
index 1c43b3b6fa..a083614488 100644
--- a/firmware/target/arm/imx31/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/mc13783-imx31.c
@@ -26,10 +26,6 @@
 #include "debug.h"
 #include "kernel.h"
 
-#ifdef BOOTLOADER
-#define PMIC_DRIVER_CLOSE
-#endif
-
 extern const struct mc13783_event_list mc13783_event_list;
 extern struct spi_node mc13783_spi;
 
@@ -47,10 +43,7 @@ static const unsigned char pmic_intm_regs[2] =
 static const unsigned char pmic_ints_regs[2] =
     { MC13783_INTERRUPT_STATUS0, MC13783_INTERRUPT_STATUS1 };
 
-#ifdef PMIC_DRIVER_CLOSE
-static bool pmic_close = false;
-static unsigned int mc13783_thread_id = 0;
-#endif
+static volatile unsigned int mc13783_thread_id = 0;
 
 static void mc13783_interrupt_thread(void)
 {
@@ -65,10 +58,8 @@ static void mc13783_interrupt_thread(void)
 
         wakeup_wait(&mc13783_wake, TIMEOUT_BLOCK);
 
-#ifdef PMIC_DRIVER_CLOSE
-        if (pmic_close)
+        if (mc13783_thread_id == 0)
             break;
-#endif
 
         mc13783_read_regset(pmic_ints_regs, pending, 2);
 
@@ -107,9 +98,7 @@ static void mc13783_interrupt_thread(void)
         while (++event < event_last);
     }
 
-#ifdef PMIC_DRIVER_CLOSE
     gpio_disable_event(MC13783_EVENT_ID);
-#endif
 }
 
 /* GPIO interrupt handler for mc13783 */
@@ -136,15 +125,12 @@ void mc13783_init(void)
 
     MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
 
-#ifdef PMIC_DRIVER_CLOSE
     mc13783_thread_id =
-#endif
         create_thread(mc13783_interrupt_thread,
             mc13783_thread_stack, sizeof(mc13783_thread_stack), 0,
             mc13783_thread_name IF_PRIO(, PRIORITY_REALTIME) IF_COP(, CPU));
 }
 
-#ifdef PMIC_DRIVER_CLOSE
 void mc13783_close(void)
 {
     unsigned int thread_id = mc13783_thread_id;
@@ -153,12 +139,9 @@ void mc13783_close(void)
         return;
 
     mc13783_thread_id = 0;
-
-    pmic_close = true;
     wakeup_signal(&mc13783_wake);
     thread_wait(thread_id);
 }
-#endif /* PMIC_DRIVER_CLOSE */
 
 bool mc13783_enable_event(enum mc13783_event_ids id)
 {
diff --git a/firmware/target/arm/imx31/rolo_restart.S b/firmware/target/arm/imx31/rolo_restart_firmware.S
similarity index 88%
rename from firmware/target/arm/imx31/rolo_restart.S
rename to firmware/target/arm/imx31/rolo_restart_firmware.S
index 902f513f62..5f24f653e0 100644
--- a/firmware/target/arm/imx31/rolo_restart.S
+++ b/firmware/target/arm/imx31/rolo_restart_firmware.S
@@ -9,7 +9,7 @@
  *
  * Copyright (C) 2008 by Michael Sevakis
  *
- * RoLo restart code for IMX31
+ * RoLo firmware restart code for IMX31
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -24,13 +24,13 @@
 #include "cpu.h"
 
 /****************************************************************************
- * void rolo_restart(const unsigned char* source, unsigned char* dest,
- *                   int length) __attribute__((noreturn));
+ * void rolo_restart_firmware(const unsigned char* source, unsigned char* dest,
+ *                            int length) __attribute__((noreturn));
  */
     .section    .text, "ax", %progbits
     .align      2
-    .global     rolo_restart
-rolo_restart:
+    .global     rolo_restart_firmware
+rolo_restart_firmware:
     adr     r4, restart_copy_start
     adr     r5, restart_copy_end
     ldr     r6, =IRAM_BASE_ADDR
diff --git a/firmware/target/arm/imx31/sdma-imx31.c b/firmware/target/arm/imx31/sdma-imx31.c
index 6f715ebb3f..40a43f8121 100644
--- a/firmware/target/arm/imx31/sdma-imx31.c
+++ b/firmware/target/arm/imx31/sdma-imx31.c
@@ -533,7 +533,7 @@ void sdma_init(void)
     /* 32-word channel contexts, use default bootscript address */
     SDMA_CHN0ADDR = SDMA_CHN0ADDR_SMSZ | 0x0050;
 
-    avic_enable_int(INT_SDMA, INT_TYPE_IRQ, INT_PRIO_DEFAULT+1, SDMA_HANDLER);
+    avic_enable_int(INT_SDMA, INT_TYPE_IRQ, INT_PRIO_SDMA, SDMA_HANDLER);
 
     /* SDMA core must run at the proper frequency based upon the AHB/IPG
      * ratio */