6 files changed, 178 insertions, 157 deletions

diff --git a/firmware/target/arm/imx31/avic-imx31.c b/firmware/target/arm/imx31/avic-imx31.c
index 51ba14d0b3..e349c97d82 100644
--- a/firmware/target/arm/imx31/avic-imx31.c
+++ b/firmware/target/arm/imx31/avic-imx31.c
@@ -119,8 +119,10 @@ void avic_init(void)
     for (i = 0; i < 8; i++)
         avic->nipriority[i] = 0;
 
-    /* Set NM bit to enable VIC */
-    avic->intcntl |= AVIC_INTCNTL_NM;
+    /* Set NM bit to enable VIC. Mask fast interrupts. Core arbiter rise
+     * for normal interrupts (for lowest latency). */
+    avic->intcntl |= AVIC_INTCNTL_NM | AVIC_INTCNTL_FIDIS |
+                   AVIC_INTCNTL_NIAD;
 
     /* Enable VE bit in CP15 Control reg to enable VIC */
     asm volatile (
@@ -213,7 +215,12 @@ 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)
+void avic_set_ni_level(int level)
 {
-    AVIC_NIMASK = level > 0x1f ? 0x1f : level;
+    if (level < 0)
+        level = 0x1f; /* -1 */
+    else if (level > 15)
+        level = 15;
+
+    AVIC_NIMASK = level;
 }
diff --git a/firmware/target/arm/imx31/avic-imx31.h b/firmware/target/arm/imx31/avic-imx31.h
index 43fd726db3..ba9e50652d 100644
--- a/firmware/target/arm/imx31/avic-imx31.h
+++ b/firmware/target/arm/imx31/avic-imx31.h
@@ -214,14 +214,16 @@ 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);
+#define AVIC_NIL_DISABLE 15
+#define AVIC_NIL_ENABLE  (-1)
+void avic_set_ni_level(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(prio) \
+ * priority. Avoid using any app or other SVC stack by doing it with a mini
+ * "stack on irq stack". Avoid actually enabling IRQ until the routine
+ * decides to do so; epilogue code will always disable them again. */
+#define AVIC_NESTED_NI_CALL_PROLOGUE(prio, stacksize) \
 ({ asm volatile ( \
         "sub    lr, lr, #4               \n" /* prepare return address */ \
         "srsdb  #0x12!                   \n" /* save LR_irq and SPSR_irq */ \
@@ -230,18 +232,25 @@ void avic_set_ni_level(unsigned int level);
         "mov    r1, %0                   \n" /* load interrupt level */ \
         "ldr    r2, [r0, #0x04]          \n" /* save NIMASK */ \
         "str    r1, [r0, #0x04]          \n" /* set interrupt level */ \
-        "cpsie  i, #0x13                 \n" /* change to SVC mode, unmask IRQ */ \
-        "stmfd  sp!, { r2, lr }          \n" /* push NIMASK and LR on SVC stack */ \
-        : : "i"(prio)); })
+        "mov    r0, sp                   \n" /* grab IRQ stack */ \
+        "sub    sp, sp, %1               \n" /* allocate space for routine to SP_irq */ \
+        "cps    #0x13                    \n" /* change to SVC mode */ \
+        "mov    r1, sp                   \n" /* save SP_svc */ \
+        "mov    sp, r0                   \n" /* switch to SP_irq *copy* */ \
+        "stmfd  sp!, { r1, r2, lr }      \n" /* push SP_svc, NIMASK and LR_svc */ \
+        : : "i"(prio), "i"(stacksize)); })
 
-#define AVIC_NESTED_NI_CALL_EPILOGUE() \
+#define AVIC_NESTED_NI_CALL_EPILOGUE(stacksize) \
 ({ asm volatile ( \
-        "ldmfd  sp!, { r2, lr }          \n" /* pop original LR and NIMASK */ \
-        "cpsid  i, #0x12                 \n" /* return to IRQ mode, mask IRQ */ \
+        "cpsid  i                        \n" /* disable IRQ */ \
+        "ldmfd  sp!, { r1, r2, lr }      \n" /* pop SP_svc, NIMASK and LR_svc */ \
+        "mov    sp, r1                   \n" /* restore SP_svc */ \
+        "cps    #0x12                    \n" /* return to IRQ mode */ \
+        "add    sp, sp, %0               \n" /* deallocate routine space */ \
         "mov    r0, #0x68000000          \n" /* AVIC BASE ADDR */ \
         "str    r2, [r0, #0x04]          \n" /* restore NIMASK */ \
         "ldmfd  sp!, { r0-r3, r12 }      \n" /* reload context */ \
         "rfefd  sp!                      \n" /* move stacked SPSR to CPSR, return */ \
-        ); })
+        : : "i"(stacksize)); })
 
 #endif /* AVIC_IMX31_H */
diff --git a/firmware/target/arm/imx31/debug-imx31.c b/firmware/target/arm/imx31/debug-imx31.c
index 2c4f8b4023..783ba728a6 100644
--- a/firmware/target/arm/imx31/debug-imx31.c
+++ b/firmware/target/arm/imx31/debug-imx31.c
@@ -40,8 +40,10 @@ 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;
+    extern volatile unsigned int dvfs_nr_dn, dvfs_nr_up,
+                                 dvfs_nr_pnc, dvfs_nr_no;
+    extern volatile unsigned int dptc_nr_dn, dptc_nr_up,
+                                 dptc_nr_pnc, dptc_nr_no;
 
     lcd_clear_display();
     lcd_setfont(FONT_SYSFIXED);
@@ -119,13 +121,15 @@ bool __dbg_hw_info(void)
         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);
+        lcd_putsf(0, line++, "dvfs d|u|p|n: %u %u %u %u",
+                  dvfs_nr_dn, dvfs_nr_up, dvfs_nr_pnc, dvfs_nr_no);
         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++, "dptc d|u|p|n: %u %u %u %u",
+                  dptc_nr_dn, dptc_nr_up, dptc_nr_pnc, dptc_nr_no);
         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));
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.c b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
index 680b015c81..cae9a384c9 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.c
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.c
@@ -72,6 +72,7 @@ static bool dvfs_running = false; /* Has driver enabled DVFS? */
 unsigned int dvfs_nr_dn = 0;
 unsigned int dvfs_nr_up = 0;
 unsigned int dvfs_nr_pnc = 0;
+unsigned int dvfs_nr_no = 0;
 
 static void dvfs_stop(void);
 
@@ -83,7 +84,7 @@ static inline void wait_for_dvfs_update_en(void)
 }
 
 
-static void do_dvfs_update(unsigned int level)
+static void do_dvfs_update(unsigned int level, bool in_isr)
 {
     const struct dvfs_clock_table_entry *setting = &dvfs_clock_table[level];
     unsigned long pmcr0 = CCM_PMCR0;
@@ -96,7 +97,7 @@ static void do_dvfs_update(unsigned int level)
 
     pmcr0 &= ~CCM_PMCR0_VSCNT;
 
-    if (level > ((pmcr0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS))
+    if (level < ((pmcr0 & CCM_PMCR0_DVSUP) >> CCM_PMCR0_DVSUP_POS))
     { 
         pmcr0 |= CCM_PMCR0_UDSC; /* Up scaling, increase */
         pmcr0 |= setting->vscnt << CCM_PMCR0_VSCNT_POS;
@@ -126,7 +127,15 @@ static void do_dvfs_update(unsigned int level)
     }
 
     CCM_PMCR0 = pmcr0;
+    /* Note: changes to frequency with ints unmaked seem to cause spurious
+     * DVFS interrupts with value CCM_PMCR0_FSVAI_NO_INT. These aren't
+     * supposed to happen. Only do the lengthy delay with them enabled iff
+     * called from the IRQ handler. */
+    if (in_isr)
+        enable_irq();
     udelay(100); /* Software wait for voltage ramp-up */
+    if (in_isr)
+        disable_irq();
     CCM_PDR0 = setting->pdr_val;
 
     if (!(pmcr0 & CCM_PMCR0_DFSUP_POST_DIVIDERS))
@@ -160,7 +169,7 @@ static void set_current_dvfs_level(unsigned int level)
 
     wait_for_dvfs_update_en();
 
-    do_dvfs_update(level);
+    do_dvfs_update(level, false);
 
     wait_for_dvfs_update_en();
 
@@ -191,7 +200,8 @@ static void __attribute__((used)) dvfs_int(void)
 
         /* Upon the DECREASE event, the frequency will be changed to the next
          * higher state index. */
-        level++;
+        while (((1u << ++level) & DVFS_LEVEL_MASK) == 0);
+
         dvfs_nr_dn++;
         break;
 
@@ -202,7 +212,8 @@ static void __attribute__((used)) dvfs_int(void)
 
         /* Upon the INCREASE event, the frequency will be changed to the next
          * lower state index. */
-        level--;
+        while (((1u << --level) & DVFS_LEVEL_MASK) == 0);
+
         dvfs_nr_up++;
         break;
 
@@ -218,11 +229,11 @@ static void __attribute__((used)) dvfs_int(void)
         break;
 
     case CCM_PMCR0_FSVAI_NO_INT:
-    default:
+        dvfs_nr_no++;
         return;     /* Do nothing. Freq change is not required */
     } /* end switch */
 
-    do_dvfs_update(level);
+    do_dvfs_update(level, true);
 }
 
 
@@ -230,9 +241,9 @@ static void __attribute__((used)) dvfs_int(void)
 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(INT_PRIO_DVFS);
+    AVIC_NESTED_NI_CALL_PROLOGUE(INT_PRIO_DVFS, 32*4);
     asm volatile ("bl dvfs_int");
-    AVIC_NESTED_NI_CALL_EPILOGUE();
+    AVIC_NESTED_NI_CALL_EPILOGUE(32*4);
 }
 
 
@@ -281,7 +292,7 @@ static void dvfs_init(void)
     imx31_regmod32(&CCM_LTR0,
                    DVFS_UPTHR << CCM_LTR0_UPTHR_POS |
                    DVFS_DNTHR << CCM_LTR0_DNTHR_POS |
-                   DVFS_DIV3CK,
+                   DVFS_DIV3CK << CCM_LTR0_DIV3CK_POS,
                    CCM_LTR0_UPTHR | CCM_LTR0_DNTHR | CCM_LTR0_DIV3CK);
 
     /* Set up LTR1. */
@@ -356,7 +367,7 @@ static void dvfs_stop(void)
         {
             /* Set default frequency level */
             wait_for_dvfs_update_en();
-            do_dvfs_update(DVFS_LEVEL_DEFAULT);
+            do_dvfs_update(DVFS_LEVEL_DEFAULT, false);
             wait_for_dvfs_update_en();
         }
 
@@ -379,6 +390,7 @@ static bool dptc_running = false; /* Has driver enabled DPTC? */
 unsigned int dptc_nr_dn = 0;
 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 */
@@ -492,7 +504,12 @@ static void dptc_new_wp(unsigned int wp)
 /* Interrupt vector for DPTC */
 static __attribute__((interrupt("IRQ"))) void CCM_CLK_HANDLER(void)
 {
-    dptc_int(CCM_PMCR0);
+    unsigned long pmcr0 = CCM_PMCR0;
+
+    if ((pmcr0 & CCM_PMCR0_PTVAI) == CCM_PMCR0_PTVAI_NO_INT)
+        dptc_nr_no++;
+
+    dptc_int(pmcr0);
 }
 
 
diff --git a/firmware/target/arm/imx31/dvfs_dptc-imx31.h b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
index 2bf6114a11..844fd6ebff 100644
--- a/firmware/target/arm/imx31/dvfs_dptc-imx31.h
+++ b/firmware/target/arm/imx31/dvfs_dptc-imx31.h
@@ -107,6 +107,16 @@ struct dvfs_lt_signal_descriptor
     uint8_t detect   : 1; /* 1 = edge-detected */
 };
 
+#define DVFS_NUM_LEVELS 4
+#define DPTC_NUM_WP    17
+
+/* 0 and 3 are *required*. DVFS hardware depends upon DVSUP pins showing
+ * minimum (11) and maximum (00) levels or interrupts will be continuously
+ * asserted. */
+#define DVFS_LEVEL_0   (1u << 0)
+#define DVFS_LEVEL_1   (1u << 1)
+#define DVFS_LEVEL_2   (1u << 2)
+#define DVFS_LEVEL_3   (1u << 3)
 
 extern long cpu_voltage_setting;
 
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
index 4876736a2b..7fc7b56dff 100644
--- a/firmware/target/arm/imx31/gigabeat-s/dvfs_dptc_tables-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/dvfs_dptc_tables-target.h
@@ -24,12 +24,12 @@
 #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 DVFS_LEVEL_MASK      (DVFS_LEVEL_0 | DVFS_LEVEL_1 | DVFS_LEVEL_3)
 
 #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
@@ -54,7 +54,7 @@
  * and the values have an additional division or the comments in the BSP are
  * incorrect.
  */
-#define DVFS_DIV3CK     CCM_LTR0_DIV3CK_131072
+#define DVFS_DIV3CK     0x3
 
 /* UPCNT defines the amount of times the up threshold should be exceeded
  * before DVFS will trigger frequency increase request. */
@@ -109,146 +109,120 @@ dvfs_dptc_voltage_table[DPTC_NUM_WP] =
     { { MC13783_SW_1_225, MC13783_SW_1_350, MC13783_SW_1_350, MC13783_SW_1_350 } },
 };
 
-#if 1
+#if CONFIG_CKIH_FREQ == 27000000
 /* 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 },
+dptc_dcvr_table_0[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 },
+static const struct dptc_dcvr_table_entry
+dptc_dcvr_table_1_3[DPTC_NUM_WP] =
+    /*  DCVR0       DCVR1       DCVR2       DCVR3    */
+{   /* 264 MHz, 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 },
 
-    },
-    {   /* 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 },
-    },
+dptc_dcvr_table_0[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 },
+};
+
+static const struct dptc_dcvr_table_entry
+dptc_dcvr_table_1_3[DPTC_NUM_WP] =
+    /*  DCVR0       DCVR1       DCVR2       DCVR3    */
+{   /* 264 MHz, 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
 
+static const struct dptc_dcvr_table_entry * const
+    dptc_dcvr_table [DVFS_NUM_LEVELS] =
+{
+    dptc_dcvr_table_0,
+    dptc_dcvr_table_1_3,
+    NULL,
+    dptc_dcvr_table_1_3,
+};
 
 /* For 27 MHz PLL reference clock */
 static const struct dvfs_clock_table_entry
-dvfs_clock_table[DVFS_NUM_LEVELS] =
+dvfs_clock_table[4] =
 {
     /*  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 */
+    { 0x00082407, 0xff841e5b, 1,   7 }, /* MCUPLL, 528 MHz, /4 = 132 MHz */
 };