i.MX31: Now that it matters because there's a debug screeen that allows changing things while running, some DVFS/DPTC stuff has to be done more carefully. Trim out some stuff and group functions better as well.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@29122 a1c6a512-1295-4272-9138-f99709370657

6 files changed, 211 insertions, 249 deletions

diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 58be2118a1..1c5ad863ce 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -38,20 +38,28 @@ 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)
+/* Synchronize DPTC comparator value registers to new table row */
+static void update_dptc_counts(void)
 {
-    int oldlevel = disable_irq_save();
-    const struct dptc_dcvr_table_entry *entry = &dptc_dcvr_table[level][wp];
+    const struct dptc_dcvr_table_entry * const entry =
+        &dptc_dcvr_table[dvfs_level][dptc_wp];
 
     CCM_DCVR0 = entry->dcvr0;
     CCM_DCVR1 = entry->dcvr1;
     CCM_DCVR2 = entry->dcvr2;
     CCM_DCVR3 = entry->dcvr3;
-
-    restore_irq(oldlevel);
 }
 
+/* Enable DPTC and unmask interrupt. */
+static void enable_dptc(void)
+{
+    /* Enable DPTC, assert voltage change request. */
+    CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_PTVAIM) | CCM_PMCR0_DPTEN |
+                    CCM_PMCR0_DPVCR;
+    udelay(2);
+    /* Now set that voltage is valid */
+    CCM_PMCR0 |= CCM_PMCR0_DPVV;
+}
 
 static uint32_t check_regulator_setting(uint32_t setting)
 {
@@ -74,9 +82,8 @@ unsigned int dvfs_nr_up = 0;
 unsigned int dvfs_nr_pnc = 0;
 unsigned int dvfs_nr_no = 0;
 
-
 /* Wait for the UPDTEN flag to be set so that all bits may be written */
-static inline void wait_for_dvfs_update_en(void)
+static inline void updten_wait(void)
 {
     while (!(CCM_PMCR0 & CCM_PMCR0_UPDTEN));
 }
@@ -111,6 +118,7 @@ static void do_dvfs_update(unsigned int level)
     /* DVSUP (new frequency index) setup */
     pmcr0 = (pmcr0 & ~CCM_PMCR0_DVSUP) | (level << CCM_PMCR0_DVSUP_POS);
 
+    /* Save new level */
     dvfs_level = level;
 
     if ((setting->pll_num << CCM_PMCR0_DFSUP_MCUPLL_POS) ^
@@ -148,35 +156,30 @@ static void do_dvfs_update(unsigned int level)
 
     if (pmcr0 & CCM_PMCR0_DPTEN)
     {
-        update_dptc_counts(level, dptc_wp);
-        /* Enable DPTC to request voltage changes, unmask interrupt. */
-        CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_PTVAIM) | CCM_PMCR0_DPVCR;
-        udelay(2);
-        /* Voltage is valid. */
-        CCM_PMCR0 |= CCM_PMCR0_DPVV;
+        update_dptc_counts();
+        enable_dptc();
     }
 }
 
-
 /* 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;
+    int oldlevel;
 
-    wait_for_dvfs_update_en();
+    /* Have to wait at least 3 div3 clocks before enabling after being
+     * stopped. */
+    udelay(1500);
 
+    oldlevel = disable_irq_save();
+    CCM_PMCR0 |= CCM_PMCR0_DVFEN;
     do_dvfs_update(level);
+    restore_irq(oldlevel);
 
-    wait_for_dvfs_update_en();
-
-    CCM_PMCR0 &= ~CCM_PMCR0_DVFEN;
+    updten_wait();
 
-    restore_irq(oldlevel);
+    bitclr32(&CCM_PMCR0, CCM_PMCR0_DVFEN);
 }
 
-
 /* DVFS Interrupt handler */
 static void __attribute__((used)) dvfs_int(void)
 {
@@ -234,7 +237,6 @@ static void __attribute__((used)) dvfs_int(void)
     do_dvfs_update(level);
 }
 
-
 /* Interrupt vector for DVFS */
 static __attribute__((naked, interrupt("IRQ"))) void CCM_DVFS_HANDLER(void)
 {
@@ -244,17 +246,9 @@ static __attribute__((naked, interrupt("IRQ"))) void CCM_DVFS_HANDLER(void)
     AVIC_NESTED_NI_CALL_EPILOGUE(32*4);
 }
 
-
 /* Initialize the DVFS hardware */
 static void INIT_ATTR 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. */
@@ -308,8 +302,10 @@ static void INIT_ATTR dvfs_init(void)
     /* Set up LTR2-- EMA configuration. */
     bitmod32(&CCM_LTR2, DVFS_EMAC << CCM_LTR2_EMAC_POS, CCM_LTR2_EMAC);
 
-    /* DVFS interrupt goes to MCU. Mask load buffer full interrupt. */
-    bitset32(&CCM_PMCR0, CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI);
+    /* DVFS interrupt goes to MCU. Mask load buffer full interrupt. Do not
+     * always give an event. */
+    bitmod32(&CCM_PMCR0, CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI,
+             CCM_PMCR0_DVFIS | CCM_PMCR0_LBMI | CCM_PMCR0_DVFEV);
 
     /* Initialize current core PLL and dividers for default level. Assumes
      * clocking scheme has been set up appropriately in other init code. */
@@ -322,6 +318,7 @@ static void INIT_ATTR dvfs_init(void)
     logf("DVFS: Initialized");
 }
 
+
 /** DPTC **/
 
 /* Request tracking since boot */
@@ -332,54 +329,41 @@ unsigned int dptc_nr_up = 0;
 unsigned int dptc_nr_pnc = 0;
 unsigned int dptc_nr_no = 0;
 
-static struct spi_transfer_desc dptc_pmic_xfer; /* Transfer descriptor */
-static const unsigned char dptc_pmic_regs[2] =  /* Register subaddresses */
-{ MC13783_SWITCHERS0, MC13783_SWITCHERS1 };
-static uint32_t dptc_reg_shadows[2];            /* shadow regs */
-static uint32_t dptc_regs_buf[2];               /* buffer for async write */
-
-
-/* Enable DPTC and unmask interrupt. */
-static void enable_dptc(void)
+struct dptc_async_buf
 {
-    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);
-}
+    struct spi_transfer_desc xfer; /* transfer descriptor */
+    unsigned int wp;               /* new working point */
+    uint32_t buf[2];               /* buffer for async write */
+};
 
+static struct dptc_async_buf dptc_async_buf;    /* ISR async write buffer */
+static const unsigned char dptc_pmic_regs[2] =  /* Register subaddresses */
+    { MC13783_SWITCHERS0, MC13783_SWITCHERS1 };
+static uint32_t dptc_reg_shadows[2];            /* shadow regs */
 
-/* Called after asynchronous PMIC write is completed */
+/* Called (in SPI INT context) after asynchronous PMIC write is completed */
 static void dptc_transfer_done_callback(struct spi_transfer_desc *xfer)
 {
     if (xfer->count != 0)
         return;
 
-    update_dptc_counts(dvfs_level, dptc_wp);
+    /* Save new working point */
+    dptc_wp = ((struct dptc_async_buf *)xfer)->wp;
+
+    update_dptc_counts();
 
     if (dptc_running)
         enable_dptc();
 }
 
-
 /* Handle the DPTC interrupt and sometimes the manual setting - always call
  * with IRQ masked. */
-static void dptc_int(unsigned long pmcr0)
+static void dptc_int(unsigned long pmcr0, int wp, struct dptc_async_buf *abuf)
 {
     const union dvfs_dptc_voltage_table_entry *entry;
     uint32_t sw1a, sw1advs, sw1bdvs, sw1bstby;
     uint32_t switchers0, switchers1;
 
-    int wp = dptc_wp;
-
     /* Mask DPTC interrupt and disable DPTC until the change request is
      * serviced. */
     CCM_PMCR0 = (pmcr0 & ~CCM_PMCR0_DPTEN) | CCM_PMCR0_PTVAIM;
@@ -387,30 +371,40 @@ static void dptc_int(unsigned long pmcr0)
     switch (pmcr0 & CCM_PMCR0_PTVAI)
     {
     case CCM_PMCR0_PTVAI_DECREASE:
+        /* Decrease voltage request - increment working point */
         wp++;
         dptc_nr_dn++;
         break;
 
     case CCM_PMCR0_PTVAI_INCREASE:
+        /* Increase voltage request - decrement working point */
         wp--;
         dptc_nr_up++;
         break;
 
     case CCM_PMCR0_PTVAI_INCREASE_NOW:
+        /* Panic request - move immediately to panic working point if
+         * decrement results in greater working point than DPTC_WP_PANIC. */
         if (--wp > DPTC_WP_PANIC)
             wp = DPTC_WP_PANIC;
         dptc_nr_pnc++;
         break;
 
     case CCM_PMCR0_PTVAI_NO_INT:
-        break; /* Just maintain at global level */
+        /* Just maintain at global level */
+        if (abuf == &dptc_async_buf)
+            dptc_nr_no++;
+        break;
     }
 
+    /* Keep result in range */
     if (wp < 0)
         wp = 0;
     else if (wp >= DPTC_NUM_WP)
         wp = DPTC_NUM_WP - 1;
 
+    /* Get new regulator register settings, sanity check them and write them
+     * in the background. */
     entry = &dvfs_dptc_voltage_table[wp];
 
     sw1a = check_regulator_setting(entry->sw1a);
@@ -419,154 +413,183 @@ static void dptc_int(unsigned long pmcr0)
     sw1bstby = check_regulator_setting(entry->sw1bstby);
 
     switchers0 = dptc_reg_shadows[0] & ~(MC13783_SW1A | MC13783_SW1ADVS);
-    dptc_regs_buf[0] = switchers0 |
-                       sw1a << MC13783_SW1A_POS |         /* SW1A */
-                       sw1advs << MC13783_SW1ADVS_POS;    /* SW1ADVS */
+    abuf->buf[0] = switchers0 |
+                   sw1a << MC13783_SW1A_POS |         /* SW1A */
+                   sw1advs << MC13783_SW1ADVS_POS;    /* SW1ADVS */
     switchers1 = dptc_reg_shadows[1] & ~(MC13783_SW1BDVS | MC13783_SW1BSTBY);
-    dptc_regs_buf[1] = switchers1 |
-                       sw1bdvs << MC13783_SW1BDVS_POS |   /* SW1BDVS */
-                       sw1bstby << MC13783_SW1BSTBY_POS;  /* SW1BSTBY */
-
-    dptc_wp = wp;
+    abuf->buf[1] = switchers1 |
+                   sw1bdvs << MC13783_SW1BDVS_POS |   /* SW1BDVS */
+                   sw1bstby << MC13783_SW1BSTBY_POS;  /* SW1BSTBY */
 
-    mc13783_write_async(&dptc_pmic_xfer, dptc_pmic_regs,
-                        dptc_regs_buf, 2, dptc_transfer_done_callback);
+    abuf->wp = wp; /* Save new for xfer completion handler */
+    mc13783_write_async(&abuf->xfer, dptc_pmic_regs, abuf->buf, 2,
+                        dptc_transfer_done_callback);
 }
 
-
 /* Handle setting the working point explicitly - always call with IRQ
  * masked */
 static void dptc_new_wp(unsigned int wp)
 {
-    dptc_wp = wp;
+    struct dptc_async_buf buf;
+
     /* "NO_INT" so the working point isn't incremented, just set. */
-    dptc_int((CCM_PMCR0 & ~CCM_PMCR0_PTVAI) | CCM_PMCR0_PTVAI_NO_INT);
-}
+    dptc_int(CCM_PMCR0 & ~CCM_PMCR0_PTVAI, wp, &buf);
 
+    /* Wait for PMIC write */
+    while (!spi_transfer_complete(&buf.xfer))
+    {
+        enable_irq();
+        nop; nop; nop; nop; nop;
+        disable_irq();
+    }
+}
 
 /* Interrupt vector for DPTC */
 static __attribute__((interrupt("IRQ"))) void CCM_CLK_HANDLER(void)
 {
-    unsigned long pmcr0 = CCM_PMCR0;
-
-    if ((pmcr0 & CCM_PMCR0_PTVAI) == CCM_PMCR0_PTVAI_NO_INT)
-        dptc_nr_no++;
-
-    dptc_int(pmcr0);
+    dptc_int(CCM_PMCR0, dptc_wp, &dptc_async_buf);
 }
 
-
 /* Initialize the DPTC hardware */
 static void INIT_ATTR dptc_init(void)
 {
-    /* Force DPTC off if running for some reason. */
-    bitmod32(&CCM_PMCR0, CCM_PMCR0_PTVAIM,
-             CCM_PMCR0_PTVAIM | CCM_PMCR0_DPTEN);
+    int oldlevel;
 
-     /* Shadow the regulator registers */
+    /* Shadow the regulator registers */
     mc13783_read_regs(dptc_pmic_regs, dptc_reg_shadows, 2);
 
     /* Set default, safe working point. */
+    oldlevel = disable_irq_save();
     dptc_new_wp(DPTC_WP_DEFAULT);
+    restore_irq(oldlevel);
 
     /* Interrupt goes to MCU, specified reference circuits enabled when
-     * DPTC is active. */
-    bitset32(&CCM_PMCR0, CCM_PMCR0_PTVIS);
-
-    bitmod32(&CCM_PMCR0, DPTC_DRCE_MASK,
+     * DPTC is active, DPTC counting range = 256 system clocks */
+    bitmod32(&CCM_PMCR0,
+             CCM_PMCR0_PTVIS | DPTC_DRCE_MASK,
+             CCM_PMCR0_PTVIS | CCM_PMCR0_DCR |
              CCM_PMCR0_DRCE0 | CCM_PMCR0_DRCE1 |
              CCM_PMCR0_DRCE2 | CCM_PMCR0_DRCE3);
 
-    /* DPTC counting range = 256 system clocks */
-    bitclr32(&CCM_PMCR0, CCM_PMCR0_DCR);
-
     logf("DPTC: Initialized");
 }
 
 
 /** Main module interface **/
 
+/** DVFS+DPTC **/
+
 /* Initialize DVFS and DPTC */
 void INIT_ATTR dvfs_dptc_init(void)
 {
+    /* DVFS or DPTC on for some reason? Force off. */
+    bitmod32(&CCM_PMCR0,
+             CCM_PMCR0_FSVAIM | CCM_PMCR0_LBMI |
+             CCM_PMCR0_PTVAIM,
+             CCM_PMCR0_FSVAIM | CCM_PMCR0_LBMI | CCM_PMCR0_DVFEN |
+             CCM_PMCR0_PTVAIM | CCM_PMCR0_DPTEN);
+
+    /* Ensure correct order - after this, the two appear independent */
     dptc_init();
     dvfs_init();
 }
 
-
-/* Start DVFS and DPTC */
-void dvfs_dptc_start(void)
+/* Obtain the current core voltage setting, in millivolts 8-) */
+unsigned int dvfs_dptc_get_voltage(void)
 {
-    dvfs_start();
-    dptc_start();
-}
+    unsigned int v;
 
+    int oldlevel = disable_irq_save();
+    v = dvfs_dptc_voltage_table[dptc_wp].sw[dvfs_level];
+    restore_irq(oldlevel);
 
-/* Stop DVFS and DPTC */
-void dvfs_dptc_stop(void)
-{
-    dptc_stop();
-    dvfs_stop();
+    /* 25mV steps from 0.900V to 1.675V */
+    return v * 25 + 900;
 }
 
+
+/** DVFS **/
+
 /* Start the DVFS hardware */
 void dvfs_start(void)
 {
-    int oldlevel;
+    if (dvfs_running)
+        return;
 
     /* Have to wait at least 3 div3 clocks before enabling after being
      * stopped. */
     udelay(1500);
 
-    oldlevel = disable_irq_save();
+    /* Unmask DVFS interrupt source and enable DVFS. */
+    bitmod32(&CCM_PMCR0, CCM_PMCR0_DVFEN,
+             CCM_PMCR0_FSVAIM | CCM_PMCR0_DVFEN);
 
-    if (!dvfs_running)
-    {
-        dvfs_running = true;
+    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);
+    avic_enable_int(INT_CCM_DVFS, INT_TYPE_IRQ, INT_PRIO_DVFS,
+                    CCM_DVFS_HANDLER);
 
     logf("DVFS: started");
 }
 
-
 /* Stop the DVFS hardware and return to default frequency */
 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 (!dvfs_running)
+        return;
 
-        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();
-        }
+    /* Mask DVFS interrupts. */
+    avic_disable_int(INT_CCM_DVFS);
+    bitset32(&CCM_PMCR0, CCM_PMCR0_FSVAIM | CCM_PMCR0_LBMI);
 
-        /* Disable DVFS. */
-        CCM_PMCR0 &= ~CCM_PMCR0_DVFEN;
-        dvfs_running = false;
+    if (((CCM_PMCR0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS) !=
+            DVFS_LEVEL_DEFAULT)
+    {
+        int oldlevel;
+        /* Set default frequency level */
+        updten_wait();
+        oldlevel = disable_irq_save();
+        do_dvfs_update(DVFS_LEVEL_DEFAULT);
+        restore_irq(oldlevel);
+        updten_wait();
     }
 
-    restore_irq(oldlevel);
+    /* Disable DVFS. */
+    bitclr32(&CCM_PMCR0, CCM_PMCR0_DVFEN);
+    dvfs_running = false;
 
     logf("DVFS: stopped");
 }
 
+/* Is DVFS enabled? */
+bool dvfs_enabled(void)
+{
+   return dvfs_running; 
+}
+
+/* If DVFS is disabled, set the level explicitly */
+void dvfs_set_level(unsigned int level)
+{
+    if (dvfs_running ||
+        level >= DVFS_NUM_LEVELS ||
+        !((1 << level) & DVFS_LEVEL_MASK) ||
+        level == ((CCM_PMCR0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS))
+        return;
+
+    set_current_dvfs_level(level);
+}
+
+/* Get the current DVFS level */
+unsigned int dvfs_get_level(void)
+{
+    return dvfs_level;
+}
+
+/* Get bitmask of levels supported */
+unsigned int dvfs_level_mask(void)
+{
+    return DVFS_LEVEL_MASK;
+}
 
 /* Mask the DVFS interrupt without affecting running status */
 void dvfs_int_mask(bool mask)
@@ -583,7 +606,6 @@ void dvfs_int_mask(bool mask)
     }
 }
 
-
 /* Set a signal load tracking weight */
 void dvfs_set_lt_weight(enum DVFS_LT_SIGS index, unsigned long value)
 {
@@ -603,7 +625,6 @@ void dvfs_set_lt_weight(enum DVFS_LT_SIGS index, unsigned long value)
     bitmod32(reg_p, value << shift, 0x7 << shift);
 }
 
-
 /* Return a signal load tracking weight */
 unsigned long dvfs_get_lt_weight(enum DVFS_LT_SIGS index)
 {
@@ -623,7 +644,6 @@ unsigned long dvfs_get_lt_weight(enum DVFS_LT_SIGS index)
     return (*reg_p & (0x7 << shift)) >> shift;
 }
 
-
 /* Set a signal load detection mode */
 void dvfs_set_lt_detect(enum DVFS_LT_SIGS index, bool edge)
 {
@@ -637,7 +657,6 @@ void dvfs_set_lt_detect(enum DVFS_LT_SIGS index, bool edge)
     bitmod32(&CCM_LTR0, edge ? bit : 0, bit);
 }
 
-
 /* Returns a signal load detection mode */
 bool dvfs_get_lt_detect(enum DVFS_LT_SIGS index)
 {
@@ -651,7 +670,6 @@ bool dvfs_get_lt_detect(enum DVFS_LT_SIGS index)
     return !!((CCM_LTR0 & (1ul << shift)) >> shift);
 }
 
-
 /* Set/clear the general-purpose load tracking bit */
 void dvfs_set_gp_bit(enum DVFS_DVGPS dvgp, bool assert)
 {
@@ -662,7 +680,6 @@ void dvfs_set_gp_bit(enum DVFS_DVGPS dvgp, bool assert)
     }
 }
 
-
 /* Return the general-purpose load tracking bit */
 bool dvfs_get_gp_bit(enum DVFS_DVGPS dvgp)
 {
@@ -671,7 +688,6 @@ bool dvfs_get_gp_bit(enum DVFS_DVGPS dvgp)
     return false;
 }
 
-
 /* Set GP load tracking by code.
  * level_code:
  *     lt 0  =defaults
@@ -738,7 +754,6 @@ void dvfs_get_gp_sense(int *level_code, unsigned long *detect_mask)
         *detect_mask = mask;
 }
 
-
 /* Turn the wait-for-interrupt monitoring on or off */
 void dvfs_wfi_monitor(bool on)
 {
@@ -746,125 +761,68 @@ void dvfs_wfi_monitor(bool on)
 }
 
 
-/* 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;
-}
-
-
-/* Is DVFS enabled? */
-bool dvfs_enabled(void)
-{
-   return dvfs_running; 
-}
-
-
-/* Get bitmask of levels supported */
-unsigned int dvfs_level_mask(void)
-{
-    return DVFS_LEVEL_MASK;
-}
-
-
-/* If DVFS is disabled, set the level explicitly */
-void dvfs_set_level(unsigned int level)
-{
-    int oldlevel = disable_irq_save();
-
-    if (!dvfs_running && level < DVFS_NUM_LEVELS)
-    {
-        unsigned int currlevel =
-            (CCM_PMCR0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS;
-        if (level != currlevel && ((1 << level) & DVFS_LEVEL_MASK))
-            set_current_dvfs_level(level);
-    }
-
-    restore_irq(oldlevel);
-}
-
+/** DPTC **/
 
 /* Start DPTC module */
 void dptc_start(void)
 {
-    int oldlevel = disable_irq_save();
-
-    if (!dptc_running)
-    {
-        dptc_running = true;
+    int oldlevel;
 
-        /* Enable DPTC and unmask interrupt. */
-        avic_enable_int(INT_CCM_CLK, INT_TYPE_IRQ, INT_PRIO_DPTC, 
-                        CCM_CLK_HANDLER);
+    if (dptc_running)
+        return;
 
-        update_dptc_counts(dvfs_level, dptc_wp);
-        enable_dptc();
-    }
+    oldlevel = disable_irq_save();
+    dptc_running = true;
 
+    /* Enable DPTC and unmask interrupt. */
+    update_dptc_counts();
+    enable_dptc();
     restore_irq(oldlevel);
 
+    avic_enable_int(INT_CCM_CLK, INT_TYPE_IRQ, INT_PRIO_DPTC, 
+                    CCM_CLK_HANDLER);
+
     logf("DPTC: started");
 }
 
-
 /* Stop the DPTC hardware if running and go back to default working point */
 void dptc_stop(void)
 {
-    int oldlevel = disable_irq_save();
+    int oldlevel;
 
-    if (dptc_running)
-    {
-        dptc_running = false;
+    if (!dptc_running)
+        return;
 
-        /* Disable DPTC and mask interrupt. */
-        CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_DPTEN) | CCM_PMCR0_PTVAIM;
-        avic_disable_int(INT_CCM_CLK);
+    avic_disable_int(INT_CCM_CLK);
 
-        /* Go back to default working point. */
-        dptc_new_wp(DPTC_WP_DEFAULT);
-    }
+    oldlevel = disable_irq_save();
+    dptc_running = false;
 
+    /* Disable DPTC and mask interrupt. */
+    CCM_PMCR0 = (CCM_PMCR0 & ~CCM_PMCR0_DPTEN) | CCM_PMCR0_PTVAIM;
+
+    /* Go back to default working point. */
+    dptc_new_wp(DPTC_WP_DEFAULT);
     restore_irq(oldlevel);
 
     logf("DPTC: stopped");
 }
 
-
-/* Get the current DPTC working point */
-unsigned int dptc_get_wp(void)
-{
-    return dptc_wp;
-}
-
-
 /* Is DPTC enabled? */
 bool dptc_enabled(void)
 {
     return dptc_running;
 }
 
-
 /* If DPTC is not running, set the working point explicitly */
 void dptc_set_wp(unsigned int wp)
 {
-    int oldlevel = disable_irq_save();
-
     if (!dptc_running && wp < DPTC_NUM_WP)
         dptc_new_wp(wp);
+}
 
-    restore_irq(oldlevel);
+/* Get the current DPTC working point */
+unsigned int dptc_get_wp(void)
+{
+    return dptc_wp;
 }
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.h b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
index 1f3f1ab20d..8c599fe402 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.h
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
@@ -118,15 +118,19 @@ struct dvfs_lt_signal_descriptor
 #define DVFS_LEVEL_2   (1u << 2)
 #define DVFS_LEVEL_3   (1u << 3)
 
-extern long cpu_voltage_setting;
-
+/* DVFS+DPTC */
 void dvfs_dptc_init(void);
-void dvfs_dptc_start(void);
-void dvfs_dptc_stop(void);
+unsigned int dvfs_dptc_get_voltage(void);
 
+/* DVFS */
 void dvfs_start(void);
 void dvfs_stop(void);
 bool dvfs_enabled(void);
+unsigned int dvfs_level_mask(void);
+void dvfs_set_level(unsigned int level);
+unsigned int dvfs_get_level(void);
+void dvfs_int_mask(bool mask);
+
 void dvfs_wfi_monitor(bool on);
 void dvfs_set_lt_weight(enum DVFS_LT_SIGS index, unsigned long value);
 unsigned long dvfs_get_lt_weight(enum DVFS_LT_SIGS index);
@@ -134,19 +138,14 @@ void dvfs_set_lt_detect(enum DVFS_LT_SIGS index, bool edge);
 bool dvfs_get_lt_detect(enum DVFS_LT_SIGS index);
 void dvfs_set_gp_bit(enum DVFS_DVGPS dvgp, bool assert);
 bool dvfs_get_gp_bit(enum DVFS_DVGPS dvgp);
-void dvfs_int_mask(bool mask);
 void dvfs_set_gp_sense(int level_code, unsigned long detect_mask);
 void dvfs_get_gp_sense(int *level_code, unsigned long *detect_mask);
-unsigned int dvfs_level_mask(void);
-
-unsigned int dvfs_dptc_get_voltage(void);
-unsigned int dvfs_get_level(void);
-void dvfs_set_level(unsigned int level);
 
+/* DPTC */
 void dptc_start(void);
 void dptc_stop(void);
 bool dptc_enabled(void);
-unsigned int dptc_get_wp(void);
 void dptc_set_wp(unsigned int wp);
+unsigned int dptc_get_wp(void);
 
 #endif /* _DVFS_DPTC_IMX31_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 481cb467bd..f30287d4e9 100644
--- a/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/kernel-gigabeat-s.c
@@ -71,10 +71,11 @@ void INIT_ATTR kernel_device_init(void)
     spi_init();
     enable_interrupt(IRQ_FIQ_STATUS);
     mc13783_init();
-    dvfs_dptc_init();
-    dvfs_wfi_monitor(true); /* Monitor the WFI signal */
+    dvfs_dptc_init();       /* Init also sets default points */
 #ifndef BOOTLOADER
-    dvfs_dptc_start(); /* Should be ok to start even so early */
+    dvfs_wfi_monitor(true); /* Monitor the WFI signal */
+    dvfs_start();           /* Should be ok to start even so early */
+    dptc_start();
 #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 9d7d30547b..11276a6c3a 100644
--- a/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/power-gigabeat-s.c
@@ -129,7 +129,8 @@ bool tuner_powered(void)
 void power_off(void)
 {
     /* Turn off voltage and frequency scaling */
-    dvfs_dptc_stop();
+    dvfs_stop();
+    dptc_stop();
 
     /* Cut backlight */
     _backlight_off();
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 1f177d4252..16b17ba403 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
@@ -210,7 +210,8 @@ void INIT_ATTR system_init(void)
 
 void system_prepare_fw_start(void)
 {
-    dvfs_dptc_stop();
+    dvfs_stop();
+    dptc_stop();
     mc13783_close();
     tick_stop();
     disable_interrupt(IRQ_FIQ_STATUS);
diff --git a/firmware/target/arm/imx31/mc13783-imx31.c b/firmware/target/arm/imx31/mc13783-imx31.c
index b236dcd2ab..d5d22e2c75 100644
--- a/firmware/target/arm/imx31/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/mc13783-imx31.c
@@ -396,6 +396,7 @@ bool mc13783_read_async(struct spi_transfer_desc *xfer,
     xfer->rxbuf = buffer;
     xfer->count = count;
     xfer->callback = callback;
+    xfer->next = NULL;
 
     return spi_transfer(xfer);
 }
@@ -422,6 +423,7 @@ bool mc13783_write_async(struct spi_transfer_desc *xfer,
     xfer->rxbuf = NULL;
     xfer->count = count;
     xfer->callback = callback;
+    xfer->next = NULL;
 
     return spi_transfer(xfer);
 }