mxs: Implement common function to setup VDDx

Implement common function to setup the VDDIO, VDDD and VDDA voltage.
Right now, there are two almost identical functions to setup VDDIO
and VDDD, which is prone to breakage. Pull out the differences into
constant structure and pass them as an argument to the common function.

Moreover, the function has almost identical loops for setting higher
and lower VDDx voltage. Merge these two loops.

Signed-off-by: Marek Vasut <marex@denx.de>
Cc: Stefano Babic <sbabic@denx.de>
Cc: Fabio Estevam <fabio.estevam@freescale.com>

diff --git a/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c b/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
index 4b917bd186df4ea62651690c1a99c91ff2b0118a..0d80158fa3600d09c245015dbe318bad5a2a08bf 100644 (file)
--- a/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
+++ b/arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
@@ -766,198 +766,112 @@ int mxs_get_vddd_power_source_off(void)
        return 0;
 }
 
-void mxs_power_set_vddio(uint32_t new_target, uint32_t new_brownout)
+struct mxs_vddx_cfg {
+       uint32_t                *reg;
+       uint8_t                 step_mV;
+       uint16_t                lowest_mV;
+       int                     (*powered_by_linreg)(void);
+       uint32_t                trg_mask;
+       uint32_t                bo_irq;
+       uint32_t                bo_enirq;
+       uint32_t                bo_offset_mask;
+       uint32_t                bo_offset_offset;
+};
+
+const struct mxs_vddx_cfg mxs_vddio_cfg = {
+       .reg                    = &(((struct mxs_power_regs *)MXS_POWER_BASE)->
+                                       hw_power_vddioctrl),
+       .step_mV                = 50,
+       .lowest_mV              = 2800,
+       .powered_by_linreg      = mxs_get_vddio_power_source_off,
+       .trg_mask               = POWER_VDDIOCTRL_TRG_MASK,
+       .bo_irq                 = POWER_CTRL_VDDIO_BO_IRQ,
+       .bo_enirq               = POWER_CTRL_ENIRQ_VDDIO_BO,
+       .bo_offset_mask         = POWER_VDDIOCTRL_BO_OFFSET_MASK,
+       .bo_offset_offset       = POWER_VDDIOCTRL_BO_OFFSET_OFFSET,
+};
+
+const struct mxs_vddx_cfg mxs_vddd_cfg = {
+       .reg                    = &(((struct mxs_power_regs *)MXS_POWER_BASE)->
+                                       hw_power_vdddctrl),
+       .step_mV                = 25,
+       .lowest_mV              = 800,
+       .powered_by_linreg      = mxs_get_vddd_power_source_off,
+       .trg_mask               = POWER_VDDDCTRL_TRG_MASK,
+       .bo_irq                 = POWER_CTRL_VDDD_BO_IRQ,
+       .bo_enirq               = POWER_CTRL_ENIRQ_VDDD_BO,
+       .bo_offset_mask         = POWER_VDDDCTRL_BO_OFFSET_MASK,
+       .bo_offset_offset       = POWER_VDDDCTRL_BO_OFFSET_OFFSET,
+};
+
+static void mxs_power_set_vddx(const struct mxs_vddx_cfg *cfg,
+                               uint32_t new_target, uint32_t new_brownout)
 {
        struct mxs_power_regs *power_regs =
                (struct mxs_power_regs *)MXS_POWER_BASE;
        uint32_t cur_target, diff, bo_int = 0;
        uint32_t powered_by_linreg = 0;
+       int adjust_up, tmp;
 
-       new_brownout = (new_target - new_brownout + 25) / 50;
+       new_brownout = DIV_ROUND(new_target - new_brownout, cfg->step_mV);
 
-       cur_target = readl(&power_regs->hw_power_vddioctrl);
-       cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-       cur_target *= 50;       /* 50 mV step*/
-       cur_target += 2800;     /* 2800 mV lowest */
+       cur_target = readl(cfg->reg);
+       cur_target &= cfg->trg_mask;
+       cur_target *= cfg->step_mV;
+       cur_target += cfg->lowest_mV;
 
-       powered_by_linreg = mxs_get_vddio_power_source_off();
-       if (new_target > cur_target) {
+       adjust_up = new_target > cur_target;
+       powered_by_linreg = cfg->powered_by_linreg();
 
+       if (adjust_up) {
                if (powered_by_linreg) {
-                       bo_int = readl(&power_regs->hw_power_vddioctrl);
-                       clrbits_le32(&power_regs->hw_power_vddioctrl,
-                                       POWER_CTRL_ENIRQ_VDDIO_BO);
+                       bo_int = readl(cfg->reg);
+                       clrbits_le32(cfg->reg, cfg->bo_enirq);
                }
+               setbits_le32(cfg->reg, cfg->bo_offset_mask);
+       }
 
-               setbits_le32(&power_regs->hw_power_vddioctrl,
-                               POWER_VDDIOCTRL_BO_OFFSET_MASK);
-               do {
-                       if (new_target - cur_target > 100)
+       do {
+               if (abs(new_target - cur_target) > 100) {
+                       if (adjust_up)
                                diff = cur_target + 100;
                        else
-                               diff = new_target;
-
-                       diff -= 2800;
-                       diff /= 50;
-
-                       clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-                               POWER_VDDIOCTRL_TRG_MASK, diff);
-
-                       if (powered_by_linreg ||
-                               (readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_VDD5V_GT_VDDIO))
-                               early_delay(500);
-                       else {
-                               while (!(readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_DC_OK))
-                                       ;
-
-                       }
-
-                       cur_target = readl(&power_regs->hw_power_vddioctrl);
-                       cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-                       cur_target *= 50;       /* 50 mV step*/
-                       cur_target += 2800;     /* 2800 mV lowest */
-               } while (new_target > cur_target);
-
-               if (powered_by_linreg) {
-                       writel(POWER_CTRL_VDDIO_BO_IRQ,
-                               &power_regs->hw_power_ctrl_clr);
-                       if (bo_int & POWER_CTRL_ENIRQ_VDDIO_BO)
-                               setbits_le32(&power_regs->hw_power_vddioctrl,
-                                               POWER_CTRL_ENIRQ_VDDIO_BO);
-               }
-       } else {
-               do {
-                       if (cur_target - new_target > 100)
                                diff = cur_target - 100;
-                       else
-                               diff = new_target;
-
-                       diff -= 2800;
-                       diff /= 50;
-
-                       clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-                               POWER_VDDIOCTRL_TRG_MASK, diff);
-
-                       if (powered_by_linreg ||
-                               (readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_VDD5V_GT_VDDIO))
-                               early_delay(500);
-                       else {
-                               while (!(readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_DC_OK))
-                                       ;
-
-                       }
-
-                       cur_target = readl(&power_regs->hw_power_vddioctrl);
-                       cur_target &= POWER_VDDIOCTRL_TRG_MASK;
-                       cur_target *= 50;       /* 50 mV step*/
-                       cur_target += 2800;     /* 2800 mV lowest */
-               } while (new_target < cur_target);
-       }
-
-       clrsetbits_le32(&power_regs->hw_power_vddioctrl,
-                       POWER_VDDIOCTRL_BO_OFFSET_MASK,
-                       new_brownout << POWER_VDDIOCTRL_BO_OFFSET_OFFSET);
-}
-
-void mxs_power_set_vddd(uint32_t new_target, uint32_t new_brownout)
-{
-       struct mxs_power_regs *power_regs =
-               (struct mxs_power_regs *)MXS_POWER_BASE;
-       uint32_t cur_target, diff, bo_int = 0;
-       uint32_t powered_by_linreg = 0;
-
-       new_brownout = (new_target - new_brownout + 12) / 25;
-
-       cur_target = readl(&power_regs->hw_power_vdddctrl);
-       cur_target &= POWER_VDDDCTRL_TRG_MASK;
-       cur_target *= 25;       /* 25 mV step*/
-       cur_target += 800;      /* 800 mV lowest */
-
-       powered_by_linreg = mxs_get_vddd_power_source_off();
-       if (new_target > cur_target) {
-               if (powered_by_linreg) {
-                       bo_int = readl(&power_regs->hw_power_vdddctrl);
-                       clrbits_le32(&power_regs->hw_power_vdddctrl,
-                                       POWER_CTRL_ENIRQ_VDDD_BO);
+               } else {
+                       diff = new_target;
                }
 
-               setbits_le32(&power_regs->hw_power_vdddctrl,
-                               POWER_VDDDCTRL_BO_OFFSET_MASK);
-
-               do {
-                       if (new_target - cur_target > 100)
-                               diff = cur_target + 100;
-                       else
-                               diff = new_target;
-
-                       diff -= 800;
-                       diff /= 25;
-
-                       clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-                               POWER_VDDDCTRL_TRG_MASK, diff);
+               diff -= cfg->lowest_mV;
+               diff /= cfg->step_mV;
 
-                       if (powered_by_linreg ||
-                               (readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_VDD5V_GT_VDDIO))
-                               early_delay(500);
-                       else {
-                               while (!(readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_DC_OK))
-                                       ;
+               clrsetbits_le32(cfg->reg, cfg->trg_mask, diff);
 
+               if (powered_by_linreg ||
+                       (readl(&power_regs->hw_power_sts) &
+                               POWER_STS_VDD5V_GT_VDDIO))
+                       early_delay(500);
+               else {
+                       for (;;) {
+                               tmp = readl(&power_regs->hw_power_sts);
+                               if (tmp & POWER_STS_DC_OK)
+                                       break;
                        }
-
-                       cur_target = readl(&power_regs->hw_power_vdddctrl);
-                       cur_target &= POWER_VDDDCTRL_TRG_MASK;
-                       cur_target *= 25;       /* 25 mV step*/
-                       cur_target += 800;      /* 800 mV lowest */
-               } while (new_target > cur_target);
-
-               if (powered_by_linreg) {
-                       writel(POWER_CTRL_VDDD_BO_IRQ,
-                               &power_regs->hw_power_ctrl_clr);
-                       if (bo_int & POWER_CTRL_ENIRQ_VDDD_BO)
-                               setbits_le32(&power_regs->hw_power_vdddctrl,
-                                               POWER_CTRL_ENIRQ_VDDD_BO);
                }
-       } else {
-               do {
-                       if (cur_target - new_target > 100)
-                               diff = cur_target - 100;
-                       else
-                               diff = new_target;
 
-                       diff -= 800;
-                       diff /= 25;
+               cur_target = readl(cfg->reg);
+               cur_target &= cfg->trg_mask;
+               cur_target *= cfg->step_mV;
+               cur_target += cfg->lowest_mV;
+       } while (new_target > cur_target);
 
-                       clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-                                       POWER_VDDDCTRL_TRG_MASK, diff);
-
-                       if (powered_by_linreg ||
-                               (readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_VDD5V_GT_VDDIO))
-                               early_delay(500);
-                       else {
-                               while (!(readl(&power_regs->hw_power_sts) &
-                                       POWER_STS_DC_OK))
-                                       ;
-
-                       }
-
-                       cur_target = readl(&power_regs->hw_power_vdddctrl);
-                       cur_target &= POWER_VDDDCTRL_TRG_MASK;
-                       cur_target *= 25;       /* 25 mV step*/
-                       cur_target += 800;      /* 800 mV lowest */
-               } while (new_target < cur_target);
+       if (adjust_up && powered_by_linreg) {
+               writel(cfg->bo_irq, &power_regs->hw_power_ctrl_clr);
+               if (bo_int & cfg->bo_enirq)
+                       setbits_le32(cfg->reg, cfg->bo_enirq);
        }
 
-       clrsetbits_le32(&power_regs->hw_power_vdddctrl,
-                       POWER_VDDDCTRL_BO_OFFSET_MASK,
-                       new_brownout << POWER_VDDDCTRL_BO_OFFSET_OFFSET);
+       clrsetbits_le32(cfg->reg, cfg->bo_offset_mask,
+                       new_brownout << cfg->bo_offset_offset);
 }
 
 void mxs_setup_batt_detect(void)
@@ -982,9 +896,8 @@ void mxs_power_init(void)
        mxs_power_configure_power_source();
        mxs_enable_output_rail_protection();
 
-       mxs_power_set_vddio(3300, 3150);
-
-       mxs_power_set_vddd(1350, 1200);
+       mxs_power_set_vddx(&mxs_vddio_cfg, 3300, 3150);
+       mxs_power_set_vddx(&mxs_vddd_cfg, 1350, 1200);
 
        writel(POWER_CTRL_VDDD_BO_IRQ | POWER_CTRL_VDDA_BO_IRQ |
                POWER_CTRL_VDDIO_BO_IRQ | POWER_CTRL_VDD5V_DROOP_IRQ |