mfd: Avoid twl6040-codec PLL reconfiguration when not needed

Do not reconfigure the PLL in case it has been configured already
with the requested parameters.
In case of different PLL configuration do only the needed changes.
This can save considerable amount of time since we can avoid the
defined protocol (with delays) for the PLL configuration.
Signed-off-by: NPeter Ujfalusi <peter.ujfalusi@ti.com>
Signed-off-by: NSamuel Ortiz <sameo@linux.intel.com>

drivers/mfd/twl6040-core.c

@@ -326,9 +326,17 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 	hppllctl = twl6040_reg_read(twl6040, TWL6040_REG_HPPLLCTL);
 	lppllctl = twl6040_reg_read(twl6040, TWL6040_REG_LPPLLCTL);
 
+	/* Force full reconfiguration when switching between PLL */
+	if (pll_id != twl6040->pll) {
+		twl6040->sysclk = 0;
+		twl6040->mclk = 0;
+	}
+
 	switch (pll_id) {
 	case TWL6040_SYSCLK_SEL_LPPLL:
 		/* low-power PLL divider */
+		/* Change the sysclk configuration only if it has been canged */
+		if (twl6040->sysclk != freq_out) {
 			switch (freq_out) {
 			case 17640000:
 				lppllctl |= TWL6040_LPLLFIN;
@@ -338,11 +346,18 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 				break;
 			default:
 				dev_err(twl6040->dev,
-				"freq_out %d not supported\n", freq_out);
+					"freq_out %d not supported\n",
+					freq_out);
 				ret = -EINVAL;
 				goto pll_out;
 			}
-		twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
+			twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
+					  lppllctl);
+		}
+
+		/* The PLL in use has not been change, we can exit */
+		if (twl6040->pll == pll_id)
+			break;
 
 		switch (freq_in) {
 		case 32768:
@@ -373,6 +388,7 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 			goto pll_out;
 		}
 
+		if (twl6040->mclk != freq_in) {
 			hppllctl &= ~TWL6040_MCLK_MSK;
 
 			switch (freq_in) {
@@ -406,15 +422,22 @@ int twl6040_set_pll(struct twl6040 *twl6040, int pll_id,
 				goto pll_out;
 			}
 
-		/* enable clock slicer to ensure input waveform is square */
+			/*
+			 * enable clock slicer to ensure input waveform is
+			 * square
+			 */
 			hppllctl |= TWL6040_HPLLSQRENA;
 
-		twl6040_reg_write(twl6040, TWL6040_REG_HPPLLCTL, hppllctl);
+			twl6040_reg_write(twl6040, TWL6040_REG_HPPLLCTL,
+					  hppllctl);
 			usleep_range(500, 700);
 			lppllctl |= TWL6040_HPLLSEL;
-		twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
+			twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
+					  lppllctl);
 			lppllctl &= ~TWL6040_LPLLENA;
-		twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL, lppllctl);
+			twl6040_reg_write(twl6040, TWL6040_REG_LPPLLCTL,
+					  lppllctl);
+		}
 		break;
 	default:
 		dev_err(twl6040->dev, "unknown pll id %d\n", pll_id);